prof. alessandro de luca - diag.uniroma1.itdeluca/rob1_en/10_inversekinematics.pdf · unimation...
TRANSCRIPT
Inverse kinematicswhat are we looking for?
Robotics 1 2
direct kinematics is always unique;how about inverse kinematics for this 6R robot?
�
Inverse kinematics problem
n “given a desired end-effector pose (position + orientation), find the values of the joint variables that will realize it”
n a synthesis problem, with input data in the form
n T = = 0An(q)
n a typical nonlinear problemn existence of a solution (workspace definition)n uniqueness/multiplicity of solutions (r Î Rm, q ÎRn)n solution methods
R p000 1
pf
§ r = = fr(q), or for any other task vector
Robotics 1 3
classical formulation:inverse kinematics for a given end-effector pose
generalized formulation:inverse kinematics for a given value of task variables
Solvability and robot workspace(for tasks related to a desired end-effector Cartesian pose)
n primary workspace WS1: set of all positions p that can be reached with at least one orientation (f or R)n out of WS1 there is no solution to the problemn when p Î WS1, there is a suitable f (or R) for which a solution exists
n secondary (or dexterous) workspace WS2: set of positions p that can be reached with any orientation (among those feasible for the robot direct kinematics)n when p Î WS2, there exists a solution for any feasible f (or R)
n WS2 Í WS1
Robotics 1 4
Workspace of Fanuc R-2000i/165F
WS1⊂R3(≈ WS2 for spherical wrist
without joint limits)
Robotics 1 5
section for aconstant angle q1
rotating the base joint angle q1
Workspace of planar 2R arm
n if l1 ¹ l2n WS1 = {p Î R2: |l1-l2| £ ║p║£ l1+l2}⊂R2
n WS2 = Æ
n if l1 = l2 = ℓn WS1 = {p Î R2: ║p║£ 2ℓ}⊂R2
n WS2 = {p = 0} (infinite number of feasible orientations at the origin)
x
y • p
l1
l2
q1
q2
l1+l2
|l1-l2|
2 orientationsif p Î int(WS1)= WS1- ¶WS1
1 orientation
¶WS1
Robotics 1 6
outer and innerboundaries
Wrist position and E-E poseinverse solutions for an articulated 6R robot
LEFT DOWN RIGHT DOWN
LEFT UP RIGHT UP
4 inverse solutionsout of singularities(for the position of
the wrist center only)
8 inverse solutions consideringthe complete E-E pose
(spherical wrist: 2 alternative solutions for the last 3 joints)
Unimation PUMA 560
Robotics 1 7
Inverse kinematic solutions of UR106-dof Universal Robot UR10, with non-spherical wrist
Robotics 1 9
video (slow motion)
desired pose
home configuration at start
[rad]
[m]p=−0.2373−0.08321.3224
R =3/2 0.5 0−0.5 3/2 00 0 1
0 = 0 −1/2 0 −1/2 0 0 T
The 8 inverse kinematic solutions of UR10
Robotics 1 10
shoulderRightwristDownelbowUp
shoulderRightwristDown
elbowDown
shoulderRightwristUp
elbowUp
shoulderRightwristUp
elbowDown
shoulderLeftwristDown
elbowDown
shoulderLeftwristDownelbowUp
shoulderLeftwristUp
elbowDown
shoulderLeftwristUp
elbowUp
! =
1.0472−1.2833−0.7376−2.6915−1.57083.1416
! =
1.0472−1.99410.73762.8273−1.57083.1416
! =
1.0472−1.5894−0.52360.54221.57080
! =
1.0472−2.09440.52360
1.57080
! =
2.7686−1.85830.7376−0.45011.5708−1.7214
! =
2.7686−1.1475−0.73760.31431.5708−1.7214
! =
2.7686−1.55220.52362.5994−1.57081.4202
! =
2.7686−1.0472−0.52363.1416−1.57081.4202
Multiplicity of solutionssome examples
n E-E positioning (m=2) of a planar 2R robot arm
n 2 regular solutions in int(WS1)
n 1 solution on ¶WS1
n for l1 = l2: ¥ solutions in WS2
n E-E positioning of an articulated elbow-type 3R robot arm
n 4 regular solutions in WS1 (with singular cases yet to be investigated ...)
n spatial 6R robot arms
n £ 16 distinct solutions, out of singularities: this “upper bound” of
solutions was shown to be attained by a particular instance of
“orthogonal” robot, i.e., with twist angles ai = 0 or ±p/2 ("i)
n analysis based on algebraic transformations of robot kinematics
n transcendental equations are transformed into a single polynomial
equation of one variable
n seek for an equivalent polynomial equation of the least possible degree
singular solutions
Robotics 1 11
1. in WS1 : ¥1 regular solutions (except for 2. and 3.), at which the E-E can take a continuum of ¥ orientations (but not all orientations in the plane!)
2. if ║p║= 3ℓ : only 1 solution, singular
3. if ║p║= ℓ : ¥1 solutions, 3 of which singular
4. if ║p║< ℓ : ¥1 regular solutions (never singular)
A planar 3R armworkspace and number/type of inverse solutions
x
y• p
l1
l2
q1
q2
l3
q3 l1 = l2 = l3 = ℓ, n=3, m=2
WS1 = {p Î R2: ║p║£ 3ℓ}⊂R2
WS2 = {p Î R2: ║p║£ ℓ}⊂R2
any planar orientation is feasible in WS2
Robotics 1 12
Workspace of a planar 3R armgeneral case: different link lengths
Robotics 1 13
lmin= l3 = 0.3⇒
⇒Rin= 0,
0.5
Multiplicity of solutionssummary of the general cases
n if m = nn ∄ solutionsn a finite number of solutions (regular/generic case)n “degenerate” solutions: infinite or finite set, but anyway
different in number from the generic case (singularity)
n if m < n (robot is redundant for the kinematic task)n ∄ solutionsn ¥n-m solutions (regular/generic case)n a finite or infinite number of singular solutions
n use of the term singularity will become clearer when dealing with differential kinematics
n instantaneous velocity mapping from joint to task velocityn lack of full rank of the associated m�n Jacobian matrix J(q)
Robotics 1 14
Dexter robot (8R arm)n m = 6 (position and orientation of E-E)n n = 8 (all revolute joints)n ¥2 inverse kinematic solutions (redundancy degree = n-m = 2)
exploring inverse kinematic solutions by a self-motion
video
Robotics 1 15
Solution methods
ANALYTICAL solution(in closed form)
NUMERICAL solution(in iterative 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
* sufficient conditions for 6-dof arms• 3 consecutive rotational joint axes are
incident (e.g., spherical wrist), or• 3 consecutive rotational joint axes are
parallel
§ 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
§ Newton method, Gradient method, and so on…
Jr(q) = ¶fr (q)¶q
Robotics 1 16D. Pieper, PhD thesis, Stanford University, 1968
Inverse kinematics of planar 2R arm
x
y• p
l1
l2
q1
q2
px
py direct kinematicspx = l1 c1 + l2 c12
py = l1 s1 + l2 s12
data q1, q2 unknowns
“squaring and summing” the equations of the direct kinematicspx
2 + py2 - (l12 + l22) = 2 l1 l2 (c1 c12 + s1 s12) = 2 l1 l2 c2
and from this
c2 = (px2 + py
2 - l12 - l22)/ 2 l1 l2, s2 = ±Ö1 - c22 q2 = ATAN2 {s2, c2}
2 solutions
in analytical form
must be in [-1,1] (else, point pis outside robot workspace!)
Robotics 1 17
Inverse kinematics of 2R arm (cont’d)
x
y• p
q1
q2
px
py
q1 = ATAN2 {py, px} - ATAN2 {l2 s2 , l1 + l2 c2}
a
b
by geometric inspectionq1 = a - b
2 solutions (one for each value of s2)
note: difference of ATAN2 needsto be re-expressed in (-p , p]!
q2’ e q2’’ have same absolute value, but opposite signs
Robotics 1 18
• p
q1’
q2’
q2’’
q1”
{q1,q2}UP/LEFT {q1,q2}DOWN/RIGHT
Algebraic solution for q1
l1 + l2c2 - l2s2
l2s2 l1 + l2c2
c1
s1
px
py=
q1 = ATAN2 {s1, c1} = ATAN2 {(py(l1+l2c2)-pxl2s2)/det, (px(l1+l2c2)+pyl2s2)/det}
det = (l12 + l22 + 2 l1l2c2) > 0except for l1=l2 and c2=-1being then q1 undefined
(singular case: ¥1 solutions)
notes: a) this method provides directly the result in (-p , p]b) when evaluating ATAN2, det > 0 can be eliminated
from the expressions of s1 and c1
px = l1 c1 + l2 c12 = l1 c1 + l2 (c1 c2 - s1 s2)
py = l1 s1 + l2 s12 = l1 s1 + l2 (s1c2 + c1s2)
linear ins1 and c1
another solution method…
Robotics 1 19
Robotics 1 20
Inverse kinematics of polar (RRP) arm
px
py
pz
q1
q2
q3
d1
px = q3 c2 c1
py = q3 c2 s1
pz = d1 + q3 s2
q1 = ATAN2{py/c2, px/c2}
if px2+py
2 = 0, then q1 remains undefined (stop); else (2 regular solutions {q1,q2,q3})
q2 = ATAN2{(pz - d1)/q3, ± Ö(px2 + py
2)/q32 }
if q3 = 0, then q1 and q2 remain both undefined (stop); else (if it stops,
a singular case: ¥2 or ¥1
solutions)
we have eliminated q3>0 from both arguments!
px2 + py
2 + (pz - d1)2 = q32
q3 = + Öpx2 + py
2 + (pz - d1)2
our choice: take here only the positive value...
Note:q2 is NOT aDH variable!
Robotics 1 21
Inverse kinematics of 3R elbow-type arm
px
py
pz
q1
q2
q3
d1
L2
L3
symmetric structure without offsetse.g., first 3 joints of Mitsubishi PA10 robot
px = c1 (L2c2+ L3c23)py = s1 (L2c2+ L3c23)pz = d1+ L2s2+ L3s23
directkinematics
Note: more details (e.g., full handling of singular cases) can be found in the solution of the Robotics 1 written exam of 11.04.2017
{f, b} = facing, backingthe point p=(px, py, pz)
{u, d} = elbow up, down
four regular inverse kinematics solutions in WS1
WS1={spherical shell centered at (0,0,d1), with outer radius Rout= L2 +L3 and inner radius Rin=|L2-L3|}
Robotics 1 22
Inverse kinematics of 3R elbow-type arm
px
py
pz
q1
q2
q3
d1
L2
L3
px2 + py
2 + (pz-d1)2 = c12 (L2c2+ L3c23)2 + s1
2 (L2c2+ L3c23)2 + (L2s2+ L3s23)2
= ... = L22 + L3
2 + 2L2L3 (c2c23+s2s23) = L22 + L3
2 + 2L2L3 c3
c3 = (px2 + py
2 + (pz-d1)2 - L22 - L3
2) / 2L2L3 ∈ [-1,1] (else, p is out of workspace!)
q3{+} = ATAN2{s3, c3}
q3{-} = ATAN2{-s3, c3} = - q3
{+}two solutions�s3 = �Ö1 - c3
2
px = c1 (L2c2+ L3c23)py = s1 (L2c2+ L3c23)pz = d1+ L2s2+ L3s23
directkinematics
Robotics 1 23
Inverse kinematics of 3R elbow-type arm
only when px2 + py
2 > 0
(else q1
is undefined —infinite solutions!)
px
py
pz
q1
q2
q3
d1
L2
L3
(being px2 + p
y2 = (L
2c
2+ L
3c
23)2 > 0)
q1{+} = ATAN2{py, px}
q1{-} = ATAN2{-p
y, -p
x}
again, two solutions
c1 = px /�√px2 + py
2
s1 = py /�√px2 + py
2
px = c1 (L2c2+ L3c23)
py
= s1 (L
2c
2+ L
3c
23)
pz = d1+ L2s2+ L3s23
direct
kinematics
Robotics 1 24
Inverse kinematics of 3R elbow-type arm
px
py
pz
q1
q2
q3
d1
L2
L3
define and solve a linear system Ax = b in the algebraic unknowns x = (c2, s2)
c1px + s1py = L2c2 + L3c23
= (L2+L3c3) c2 – L3s3 s2
pz – d1 = L2s2+ L3s23
= L3s3 c2 + (L2+L3c3) s2
combine the first two direct kinematicsequations and rearrange the last one
coefficient matrix A known vector b
four regular solutions for q2,depending on combinations of {+,-} from q1 and q3
provided det A = px2 + py
2 + (pz-d1)2 > 0(else q2 is undefined —infinite solutions!) q2
{{f,b},{u,d}}
= ATAN2{s2{{f,b},{u,d}}, c2
{{f,b},{u,d}}}
Inverse kinematics for robots with spherical wrist
x0
y0
z0
x6
y6
z6 = a
first 3 robot jointsof any type (RRR, RRP, PPP, …)
find q1, …, q6 from the input data:• p (origin O6)• R = [n s a] (orientation of RF6)
z3z5
j4j5
j6O6 = p
W
d6
j1
1. W = p - d6 a ® q1, q2, q3 (inverse “position” kinematics for main axes)
2. R = 0R3(q1, q2, q3) 3R6(q4, q5, q6)
(inverse “orientation” kinematics for the wrist)given Euler ZYZ or ZXZ
rotation matrixknown,
after step 1
® 3R6(q4, q5, q6) = 0R3T R
Robotics 1 25
z4
® q4, q5, q6
6R example: Unimation PUMA 600
8 different inverse solutions that can be found in closed form(see Paul, Shimano, Mayer; 1981)
sphericalwrist
Robotics 1 26
a = 0z6(q)
n = 0x6(q)
s = 0y6(q)
p = 06(q)
here d6=0, so that 06=W directly
a function ofq1, q2, q3 only!
¬ neglected
n use when a closed-form solution q to rd = fr(q) does not exist or is “too hard” to be found
n (analytical Jacobian)
n Newton method (here for m=n)n rd = fr(q) = fr(qk) + Jr(qk) (q - qk) + o(║q - qk║2)
qk+1 = qk + Jr-1(qk) [rd - fr(qk)]
n convergence if q0 (initial guess) is close enough to some q*: fr(q*) = rdn problems near singularities of the Jacobian matrix Jr(q) n in case of robot redundancy (m<n), use the pseudo-inverse Jr
#(q) n has quadratic convergence rate when near to solution (fast!)
Numerical solution of inverse kinematics problems
Jr(q) = ¶fr¶q
Robotics 1 27
Operation of Newton method
n in the scalar case, also known as “method of the tangent”
n for a differentiable function f(x), find a root of f(x*)=0 by iterating as
Robotics 1 28
animation from
http://en.wikipedia.org/wiki/File:NewtonIteration_Ani.gif
an approximating sequence
n Gradient method (max descent)n minimize the error function
H(q) = ½ ║rd - fr(q)║2 = ½ [rd - fr(q)]T [rd - fr(q)]
qk+1 = qk - a ÑqH(qk)from ÑqH(q) = - Jr
T(q) [rd - fr(q)], we get
qk+1 = qk + a JrT(qk) [rd - fr(qk)]
n the scalar step size a > 0 should be chosen so as to guarantee a decrease of the error function at each iteration (too large values for a may lead the method to “miss” the minimum)
n when the step size a is too small, convergence is extremely slow
Numerical solution of inverse kinematics problems (cont’d)
Robotics 1 29
Revisited as a “feedback” scheme
JrT(q) ó
õ
fr(q)
+
-
erd q.q
q(0)
rd = cost
e = rd - fr(q) ® 0 Û closed-loop equilibrium e=0 is asymptotically stable
V = ½ eTe ³ 0 Lyapunov candidate function.q
.e
.V = eT = eT d (
dt(rd - fr(q)) = - eT Jr = - eT Jr Jr
Te £ 0.V = 0 Û e Î Ker(Jr
T) in particular e = 0
asymptotic stability
(a = 1)
Robotics 1 30
r
Properties of Gradient method
n computationally simpler: Jacobian transpose, rather than its (pseudo)-inverse
n direct use also for robots that are redundant for the taskn may not converge to a solution, but it never divergesn the discrete-time evolution of the continuous scheme
is equivalent to an iteration of the Gradient methodn scheme can be accelerated by using a gain matrix K>0
.q = Jr
T(q) K e
qk+1 = qk + DT JrT(qk) [rd - f(qk)] (a = DT)
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 Jr
T (if a singularity is encountered)
Robotics 1 31
A case studyanalytic expressions of Newton and gradient iterations
n 2R robot with l1 = l2 = 1, desired end-effector position rd = pd = (1,1)n direct kinematic function and error
n Jacobian matrix
n Newton versus Gradient iteration
Robotics 1 32
•
ek
det Jr(q)
Error functionn 2R robot with l1=l2=1, desired end-effector position pd = (1,1)
Robotics 1 33
plot of ║e║2 as a function of q = (q1,q2)
e = pd - fr(q)
two local minima(inverse kinematic solutions)
Error reduction by Gradient methodn flow of iterations along the negative (or anti-) gradientn two possible cases: convergence or stuck (at zero gradient)
Robotics 1 34
start
one solution
local maximum(stop if this is the initial guess)
. .
another start...
...the other solution
saddle point(stop after some iterations)
(q1,q2)’ =(0,π/2) (q1,q2)” =(π/2,-π/2) (q1,q2)max =(-3π/4,0) (q1,q2)saddle =(π/4,0)
e Î Ker(JrT) !
Convergence analysiswhen does the gradient method get stuck?
n lack of convergence occurs whenn the Jacobian matrix Jr(q) is singular (the robot is in a “singular configuration”)n AND the error is in the “null space” of Jr
T(q)
Robotics 1 35
(q1,q2)max =(-3π/4,0)
(q1,q2)saddle =(π/4,0) e Î Ker(JrT)
pd
p
q1
q2
e ∉ Ker(JrT) !!
pd
p
e Î Ker(JrT)
pd
p
(q1,q2) =(π/9,0)
the algorithm will proceed in this case,
moving out ofthe singularity
Issues in implementationn initial guess q0
n only one inverse solution is generated for each guessn multiple initializations for obtaining other solutions
n optimal step size a in Gradient methodn a constant step may work good initially, but not close to the
solution (or vice versa)n an adaptive one-dimensional line search (e.g., Armijo’s rule) could
be used to choose the best a at each iterationn stopping criteria
n understanding closeness to singularities
║qk+1-qk║ ≤ εq║rd - f(qk)║ ≤ εCartesian error
(possibly, separate for position and orientation)
algorithmincrement
σmin{J(qk)} ≥ σ0numerical conditioning
of Jacobian matrix (SVD)(or a simpler test on its determinant, for m=n)
Robotics 1 36
Numerical tests on RRP robot
n RRP/polar robot: desired E-E position rd = pd = (1, 1 ,1) —see slide 20, with d1=0.5
n the two (known) analytical solutions, with q3 ≥ 0, are:q* = (0.7854, 0.3398, 1.5)q** = (q1
*- p, p - q2*, q3
*) = (-2.3562, 2.8018, 1.5)
n norms ε = 10-5 (max Cartesian error), εq =10-6 (min joint increment)n kmax=15 (max # iterations), |det(Jr)| ≤ 10-4 (closeness to singularity)
n numerical performance of Gradient (with different steps a) vs. Newtonn test 1: q0 = (0, 0, 1) as initial guess
n test 2: q0 = (-p/4, p/2, 1) —”singular” start, since c2=0 (see slide 20)
n test 3: q0 = (0, p/2, 0) —”double singular” start, since also q3=0n solution and plots with Matlab code
Robotics 1 37
Numerical test - 1
n test 1: q0 = (0, 0, 1) as initial guess; evolution of error norm
Robotics 1 38
Gradient: a = 0.5 Gradient: a = 1 Gradient: a = 0.7
slow, 15 (max)iterations
too large, oscillatesaround solution good, converges
in 11 iterations
Newton
very fast, convergesin 5 iterations
0.15⋅10-8
0.57⋅10-5
Cartesian errorscomponent-wise
ex
ey
ez
Numerical test - 1
n test 1: q0 = (0, 0, 1) as initial guess; evolution of joint variables
Robotics 1 39
Gradient: a = 1 Gradient: a = 0.7not convergingto a solution
converges in 11 iterations
Newtonconverges in 5 iterations
both to the same solution q* = (0.7854, 0.3398, 1.5)
Numerical test - 2
n test 2: q0 = (-p/4, p/2, 1): singular start
Robotics 1 40
Gradienta = 0.7
with check of singularity:
blocked at start
without check:it diverges!
Newton
erro
r nor
ms
starts towardsolution, butslowly stops
(in singularity):when Cartesian errorvector e ∈ Ker(Jr
T) join
t var
iabl
es
!!
Numerical test - 3
n test 3: q0 = (0, p/2, 0): “double” singular start
Robotics 1 41
Newtonis either
blocked at startor (w/o check)
explodes!→ “NaN” in Matlab
erro
r no
rm
Gradient (with a = 0.7)� starts toward solution� exits the double singularity� slowly converges in 19
iterations to the solutionq*=(0.7854, 0.3398, 1.5)
join
t va
riab
les
Car
tesi
an e
rror
s
�
�
�
0.96⋅10-5
Final remarks
n an efficient iterative scheme can be devised by combiningn initial iterations using Gradient (“sure but slow”, linear convergence rate)n switch then to Newton method (quadratic terminal convergence rate)
n joint range limits are considered only at the endn check if the solution found is feasible, as for analytical methods
n in alternative, an optimization criterion can be included in the search n driving iterations toward an inverse kinematic solution with nicer properties
n if the problem has to be solved on-linen execute iterations and associate an actual robot motion: repeat steps at
times t0, t1=t0+T, ..., tk=tk-1+T (e.g., every T=40 ms)
n the “good” choice for the initial guess q0 at tk is the solution of the previous problem at tk-1 (provides continuity, needs only 1-2 Newton iterations)
n crossing of singularities/handling of joint range limits need special care
n Jacobian-based inversion schemes are used also for kinematic control, along a continuous task trajectory rd(t)
Robotics 1 42