Beruflich Dokumente
Kultur Dokumente
Inverse kinematics
Prof. Alessandro De Luca
Robotics 1
Inverse kinematics
what are we looking for?
Robotics 1
T=
R p
000 1
classical formulation:
inverse kinematics for a given end-effector pose
" r =
p
!
generalized formulation:
inverse kinematics for a given value of task variables
Robotics 1
for p " WS2 there is at least one solution for any feasible ! (or R)
WS2 # WS1
Robotics 1
WS1
rotating the
base joint angle $1
Robotics 1
p
q2
|l1-l2|
q1
(WS1
x
!
if l1 % l2
!
!
l1+l2
1 orientation
if l1 = l2 = "
!
!
Robotics 1
RIGHT DOWN
4 inverse solutions
out of singularities
(for the position of
the wrist center only)
LEFT UP
Robotics 1
RIGHT UP
RIGHT UP
RIGHT DOWN
LEFT UP
LEFT DOWN
Robotics 1
Multiplicity of solutions
some examples
singular solutions
Robotics 1
A planar 3R arm
y
l2
l1
q1
q2
l1 = l2 = l3 = "
l3
4. if !p!<
Robotics 1
10
Multiplicity of solutions
summary of the general cases
if m = n
!
!
!
! solutions
a finite number of solutions (regular/generic case)
degenerate solutions: infinite or finite set, but anyway
different in number from the generic case (singularity)
! solutions
)n-m solutions (regular/generic case)
a finite or infinite number of singular solutions
Robotics 1
11
video
12
Solution methods
ANALYTICAL solution
(in closed form)
" preferred, if it can be found*
" use ad-hoc geometric inspection
" algebraic methods (solution of
polynomial equations)
" systematic ways for generating a
reduced set of equations to be
solved
*
NUMERICAL solution
(in iterative form)
" certainly needed if n>m (redundant
case), or at/close to singularities
" slower, but easier to be set up
" in its basic form, it uses the
(analytical) Jacobian matrix of the
direct kinematics map
(fr (q)
Jr(q) =
(q
Robotics 1
13
l2
l1
direct kinematics
px = l1 c1 + l2 c12
q2
py = l1 s1 + l2 s12
q1
px
data
q1, q2 unknowns
in analytical form
2 solutions
14
(contd)
by geometric inspection
q1 = * - .
q2
.
q1
*
px
2 solutions
(one for each value of s2)
x
q1 = ATAN2 {py, px} - ATAN2 {l2 s2 , l1 + l2 c2}
q2
{q1,q2}UP/LEFT
q2
q1
q1
Robotics 1
{q1,q2}DOWN/RIGHT
q2 e q2 have same absolute
value, but opposite signs
15
l1 + l2c2
c1
s1
linear in
s1 and c1
px
py
except for l1=l2 and c2=-1
being then q1 undefined
(singular case: )1 solutions)
Robotics 1
16
px = q3 c2 c1
q3
q2 is NOT a
DH variable!
py = q3 c2 s1
pz = d1 + q3 s2
q2
d1
px
py
q1
(if it stops,
q2 = ATAN2{(pz - d1)/q3, -(px +
}
a singular case:
)2 or )1
if px2+py2 = 0, then q1 remains undefined (stop); else
solutions)
q1 = ATAN2{py/c2, px/c2} (2 regular solutions {q1,q2,q3})
2
Robotics 1
py2)/q32
17
Inverse kinematics
for robots with spherical wrist
j6
j5
z0
y0
x0
j1
z4
z3
z5
j4
d6
y6
x6
O6 = p
z6 = a
18
n = 0x6(q)
s = 0y6(q)
a function of
q1, q2, q3 only!
a = 0z6(q)
p = 06(q)
Robotics 1
here d6=0,
so that 06=W directly
19
Numerical solution of
inverse kinematics problems
!
0 neglected
Robotics 1
20
x k+1
f(x k )
= xk "
f'(x k )
an approximating sequence
{x1 , x 2 , x 3 , x 4 , x 5 ,...}
animation from
http://en.wikipedia.org/wiki/File:NewtonIteration_Ani.gif
!
Robotics 1
21
Numerical solution of
inverse kinematics problems
!
(contd)
Robotics 1
22
+
-
Jr
T(q)
.
q
q(0)
2
3
r
fr(q)
rd = cost
(* = 1)
23
!
!
!
(* = 6T)
.
q = JrT(q) K e
note: K can be used also to escape from being stuck in a stationary point,
by rotating the error e out of the kernel of JrT (if a singularity is encountered)
Robotics 1
24
A case study
k+1
=q +
!
Robotics 1
1
s2
Jr -1(qk )
" c12
%
s12
$
'
-(c
+
c
)
-(s
+
s
)
# 1 12
1
12 &
!#
-(s1 + s12 ) c1 + c12 &
" %
(
-s
c
$
'
12
12
!
Jr T (qk )
q=q
ek
q=q k
q=q k
25
Error function
!
e = pd - fr(q)
start
one solution
local maximum
saddle point
another start...
...the other solution
27
Convergence analysis
q1
#1 - 2 &
e = pd " p = %
(
$1 - 2 '
pd
"( 2
" -(s1 + s12 ) c1 + c12 %
J (q) = $
=$
'
-s12
c12 & q=q
#
#( 2
saddle
!
T
r
pd
(q1,q2)saddle =(!/4,0)
2%
'
2&
e " Ker(JrT)
e " Ker(JrT)
p
Robotics 1
(q1,q2)max =(-3!/4,0)
pd
e Ker(JrT) !!
p
(q1,q2) =(!/9,0)
28
Issues in implementation
initial guess q0
!
!
a constant step may work good initially, but not close to the
solution (or vice versa)
an adaptive one-dimensional line search (e.g., Armijos rule) could
be used to choose the best * at each iteration
stopping criteria
Cartesian error
f(qk)!
!qk+1-qk! $ #q
$min{J(qk)} % $0
Robotics 1
$#
algorithm
increment
numerical conditioning
of Jacobian matrix (SVD)
norms
*) vs. Newton
test 2: q0 = (-+/4, +/2, 1) singular start, since c2=0 (see slide 17)
Robotics 1
30
Numerical test
!
-1
Gradient: * = 1
too large, oscillates
around solution
slow, 15 (max)
iterations
good, converges
in 11 iterations
0.5710-5
Newton
very fast, converges
in 5 iterations
Gradient: * = 0.7
Cartesian errors
component-wise
ex
ey
ez
Robotics 1
0.1510-8
31
Numerical test
!
-1
Gradient:
*=1
not converging
to a solution
Gradient:
* = 0.7
converges in
11 iterations
Newton
converges in
5 iterations
32
Numerical test
!
-2
error norms
Newton
with check of
singularity:
blocked at start
without check:
it diverges!
Gradient
* = 0.7
starts toward
solution, but
slowly stops
(in singularity):
when Cartesian error
vector e Ker(JrT)
Robotics 1
joint variables
!!
33
Numerical test
Robotics 1
0.9610-5
Newton
is either
blocked at start
or (w/o check)
explodes (NaN)!!
joint variables
Cartesian errors
error norm
-3
34
Final remarks
!
initial iterations with Gradient method (sure but slow, having linear
convergence rate)
switch then to Newton method (quadratic terminal convergence rate)
Robotics 1
35