View
11
Download
0
Category
Preview:
Citation preview
Robotics 1
Inverse kinematicsProf. Alessandro De Luca
Robotics 1 1
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§ 𝑇 = 𝑅 𝑝
0' 1= )𝐴+(𝑞)
n a typical nonlinear problemn existence of a solution (workspace definition)n uniqueness/multiplicity of solutions (𝑟 ∈ ℝ1, 𝑞 ∈ ℝ+)n solution methods
§ 𝑟 = 𝑓4(𝑞), for a task function
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 workspacefor tasks related to a desired end-effector Cartesian pose
n primary workspace 𝑊𝑆7: set of all positions 𝑝 that can be reached with at least one orientation (𝜙 or 𝑅)n out of WS1 there is no solution to the problemn if 𝑝 ∈ 𝑊𝑆7, there is a suitable 𝜙 (or 𝑅) for which a solution exists
n secondary (or dexterous) workspace 𝑊𝑆9: set of positions p that can be reached with any orientation (among those feasible for the robot direct kinematics)n if 𝑝 ∈ 𝑊𝑆9, there exists a solution for any feasible 𝜙 (or 𝑅)
n 𝑊𝑆9 Í 𝑊𝑆7
Robotics 1 4
Workspace of Fanuc R-2000i/165F
𝑊𝑆7 ⊂ ℝ;(≈ 𝑊𝑆9 for spherical wrist
without joint limits)
Robotics 1 5
section for aconstant angle 𝑞7
rotating the base joint angle 𝑞7
Workspace of a planar 2R arm
n if 𝑙7 ¹ 𝑙9n 𝑊𝑆7 = 𝑝 Î ℝ9: 𝑙7 − 𝑙9 ≤ 𝑝 ≤ 𝑙7 + 𝑙9 ⊂ ℝ9
n 𝑊𝑆9 = Æ
n if 𝑙7 = 𝑙9 = 𝑙n 𝑊𝑆7 = 𝑝 Î ℝ9: 𝑝 ≤ 2𝑙 ⊂ ℝ9n 𝑊𝑆9 = 𝑝 = 0 (all feasible orientations at the origin!... an infinite number)
Robotics 1 6
𝑥𝑞1
𝑞2
𝑃•
𝑙1
𝑙2
𝑦
𝑙7 + 𝑙9
𝑙7 − 𝑙9𝜕𝑊𝑆7outer and inner
boundaries
2 orientationsif 𝑝 Î int(𝑊𝑆7)= 𝑊𝑆7 − 𝜕𝑊𝑆7
1 orientationon 𝑊𝑆7
𝑂
𝑂 •
𝑝 = 𝑂𝑃
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
Counting/visualizing the 8 solutionsof the inverse kinematics for a Unimation Puma 560
Robotics 1 8
LEFT DOWN
LEFT UP
RIGHT DOWN
RIGHT UP
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 −𝜋/2 0 −𝜋/2 0 0 T
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 solutionsfew examples
n E-E positioning (𝑚 = 2) of a planar 2R robotn 2 regular solutions in int(𝑊𝑆7)n 1 solution on 𝜕𝑊𝑆7n for 𝑙7 = 𝑙9: ∞ solutions in 𝑊𝑆9
n E-E positioning (𝑚 = 3) of an elbow-type spatial 3R robotn 4 regular solutions in 𝑊𝑆7 (with singular cases yet to be investigated ...)
n spatial 6R robot armsn £ 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 𝛼\ = 0 or ±𝜋/2 (∀𝑖)
n analysis based on algebraic transformations of robot kinematicsn transcendental equations are transformed into a single polynomial
equation in one variable (number of roots = degree of the polynomial)n seek for a transformed polynomial equation of the least possible degree
singular solutions
Robotics 1 11
Algebraic transformationswhiteboard …
Robotics 1 12
start with some trigonometric equation in the joint angle 𝜃 to be solved …𝑎 sin 𝜃 + 𝑏 cos 𝜃 = 𝑐
introduce the algebraic transformation𝑢 = tan ⁄𝜃 2
cos 𝜃 =1 − 𝑢9
1 + 𝑢9sin 𝜃 =
2𝑢1 + 𝑢9 (⇒ sin9 𝜃 + cos9 𝜃 = 1)
tan 𝜃 = tan 2 ⁄𝜃 2 =2 tan ⁄𝜃 2
1 − tan9 ⁄𝜃 2 =2𝑢
1 − 𝑢9(using the duplication formula)
substituting in (✻)
(✻)
𝑎2𝑢
1 + 𝑢9 + 𝑏1 − 𝑢9
1 + 𝑢9 = 𝑐
⇒
⇒ 𝑏 + 𝑐 𝑢9− 2𝑎 𝑢 − 𝑏 − 𝑐 = 0
⇒ 𝑢7,9 =𝑎 ± 𝑎9 + 𝑏9 − 𝑐9
𝑏 + 𝑐 ⇒ 𝜃7,9 = 2 arctan 𝑢7,9only if argument is real, else no solution
polynomial equation of second degree in 𝑢
(… and the related inverse formulas)
1. in int 𝑊𝑆7 : ∞7 regular (except for 3.) solutions, at which the E-E can take a continuum of ∞ orientations (but not all orientations in the plane!)
2. if 𝑝 = 3𝑙 : only 1 solution, singular3. if 𝑝 = 𝑙 :∞7 solutions, 3 of which singular
4. if 𝑝 < 𝑙: ∞7 regular solutions (that are never singular)
A planar 3R armworkspace and number/type of inverse solutions
𝑙7 = 𝑙9 = 𝑙; = 𝑙
any planar orientation is feasible in 𝑊𝑆9
Robotics 1 13
𝑦
𝑥𝑞7
𝑞9
𝑙1
𝑙2𝑙3
𝑞; 𝑝•
𝑝𝑥
𝑝𝑦𝑛 = 3,𝑚 = 2
𝑊𝑆7 = 𝑝 ∈ ℝ9: 𝑝 ≤ 3𝑙 ⊂ ℝ9
𝑊𝑆9 = 𝑝 ∈ ℝ9: 𝑝 ≤ 𝑙 ⊂ ℝ9
Workspace of a planar 3R armwith generic link lengths
Robotics 1 14
lmin= l3 = 0.3⇒
⇒
Rin = 0,
0.5
Exercise #3 in classroom testof 21 Nov 2014
Multiplicity of solutionssummary of the general cases
n if 𝑚 = 𝑛n ∄ 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 𝑚 < 𝑛 (robot is kinematically redundant for the task)n ∄ solutionsn ∞+o1 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 𝑚×𝑛 Jacobian matrix 𝐽(𝑞)
Robotics 1 15
Dexter 8R robot armn 𝑚 = 6 (position and orientation of E-E)n 𝑛 = 8 (all revolute joints)n ∞9 inverse kinematic solutions (redundancy degree = 𝑛 −𝑚 = 2)
exploring inverse kinematic solutions by a robot self-motion
video
Robotics 1 16
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 𝑛 > 𝑚(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…
Robotics 1 17D. Pieper, PhD thesis, Stanford University, 1968
𝐽4 𝑞 =𝜕𝑓4(𝑞)𝜕𝑞
Inverse kinematics of planar 2R arm
direct kinematics
𝑝s = 𝑙7𝑐7 + 𝑙9𝑐79
𝑝t = 𝑙7𝑠7 + 𝑙9𝑠79
data 𝑞7, 𝑞9 unknowns
“squaring and summing” the equations of the direct kinematics𝑝s9 + 𝑝t9 − 𝑙79 + 𝑙99 = 2𝑙7𝑙9 𝑐7𝑐79 + 𝑠7𝑠79 = 2𝑙7𝑙9𝑐9
and from this𝑐9 = v𝑝s9 + 𝑝t9 − 𝑙79 + 𝑙99 2𝑙7𝑙9, 𝑠9 = ± 1 − 𝑐99
2 solutions in analytical formmust be in [−1,1] (else, point 𝑃is outside robot workspace!)
Robotics 1 18
𝑞1
𝑞2
𝑃•
𝑙1
𝑙2
𝑦
𝑥𝑝𝑥
𝑝𝑦
𝑞9 = atan2 𝑠9, 𝑐9
Inverse kinematics of 2R arm (cont’d)
𝑞7 = atan2 𝑝t, 𝑝s − atan2 𝑙9𝑠9, 𝑙7 + 𝑙9𝑐9
a
b
by geometric inspection𝑞7 = 𝛼 − 𝛽
2 solutions (one for each value of 𝑠2)
note: difference of atan2’s needsto be re-expressed in (−𝜋 , 𝜋]!
𝑞9{ and 𝑞9{{ have same absolute value, but opposite signs
Robotics 1 19
• p
𝑞7{
𝑞9{
𝑞9{{
𝑞7{{
{𝑞7, 𝑞9}UP/LEFT {𝑞7, 𝑞9}DOWN/RIGHT
𝑦
𝑥𝑝𝑥
𝑝𝑦 • 𝑃
𝑞9
𝑙1
𝑙2
𝑞7
𝑞7 = atan2 𝑠7, 𝑐7= atan2 ⁄𝑝t 𝑙7 + 𝑙9𝑐9 − 𝑝s𝑙9𝑠9 det , ⁄𝑝s 𝑙7 + 𝑙9𝑐9 + 𝑝t𝑙9𝑠9 det
Algebraic solution for 𝑞7
det = 𝑙79 + 𝑙99 + 2𝑙7𝑙9𝑐9 > 0except if 𝑙7 = 𝑙9 and 𝑐9 = −1
being then 𝑞7 undefined(singular case: ∞7 solutions)
notes: a) this method provides directly the result in (−𝜋, 𝜋]b) when evaluating atan2, det > 0 can be in fact eliminated
from the expressions of 𝑠7 and 𝑐7 (not changing the result)
linear in𝑠7 and 𝑐7
another solution method…
Robotics 1 20
𝑝s = 𝑙7𝑐7 + 𝑙9𝑐79 = 𝑙7𝑐7 + 𝑙9 𝑐7𝑐9 − 𝑠7𝑠9
𝑝t = 𝑙7𝑠7 + 𝑙9𝑠79 = 𝑙7𝑠7 + 𝑙9 𝑠7𝑐9 + 𝑐7𝑠9
𝑙7 + 𝑙9𝑐9 −𝑙9𝑠9
𝑙9𝑠9 𝑙7 + 𝑙9𝑐9
𝑐7𝑠7 =
𝑝s𝑝t
Robotics 1 21
Inverse kinematics of polar (RRP) arm
𝑝𝑥
𝑝𝑦
𝑝𝑧
𝑞1
𝑞2
𝑞3
𝑑1
𝑞7 = atan2 ⁄𝑝t 𝑐9 , ⁄𝑝s 𝑐9
if 𝑝s9 + 𝑝t9 = 0, then 𝑞7 remains undefined (stop); else (2 regular solutions 𝑞7, 𝑞9, 𝑞; )
𝑞9 = atan2 ⁄𝑝� − 𝑑7 𝑞; , v± 𝑝s9 + 𝑝t9 𝑞;
if 𝑞; = 0, then 𝑞7 and 𝑞9 remain both undefined (stop); else (if we stop, it isa singular case:
∞9 or ∞7
solutions)
eliminating 𝑞; > 0 from both arguments
𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 = 𝑞;9
𝑞; = + 𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9
our choice: take here only the positive value...
note: here𝑞9 is NOT aDH variable!
𝑝s = 𝑞;𝑐9𝑐7𝑝t = 𝑞;𝑐9𝑠7𝑝� = 𝑑7 + 𝑞;𝑠9
directkinematics
𝑃
Robotics 1 22
Inverse kinematics of 3R elbow-type arm
𝑝s
𝑝𝑦𝑝𝑧
𝑞1
𝑞2𝑞3
𝑑1
𝐿2
𝐿3
symmetric structure without offsetse.g., first 3 joints of Mitsubishi PA10 robot
more details (e.g., full handling of singular cases) can be found in the solution of Exercise #1
in written exam of 11 Apr 2017
𝑓, 𝑏 : facing, backingpoint 𝑝 = 𝑝s, 𝑝t, 𝑝�
𝑢, 𝑑 : elbow up, down
4 regular inverse kinematics solutions in 𝑊𝑆7
𝑊𝑆7 = {spherical shell centered at (0,0, 𝑑7), with outer radius 𝑅��� = 𝐿9 + 𝐿;and inner radius 𝑅\+ = 𝐿9 − 𝐿; }
𝑃
Robotics 1 23
Inverse kinematics of 3R elbow-type armstep 1
∈ −1,+1 (else, 𝑝 is out of workspace!)
± 𝑠;= ± 1 − 𝑐;9
directkinematics
𝑝s
𝑝𝑦
𝑝𝑧
𝑞1
𝑞2𝑞3
𝑑1
𝐿2
𝐿3
𝑝s = 𝑐7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝t = 𝑠7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝� = 𝑑7 + 𝐿9𝑠9 + 𝐿;𝑠9;
𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 = 𝑐79 𝐿9𝑐9 + 𝐿;𝑐9; 9 + 𝑐79 𝐿9𝑐9 + 𝐿;𝑐9; 9 + 𝐿9𝑠9 + 𝐿;𝑠9; 9
= ⋯ = 𝐿99 + 𝐿;9 + 2𝐿9𝐿; 𝑐9𝑐9; + 𝑠9𝑠9; = 𝐿99 + 𝐿;9 + 2𝐿9𝐿;𝑐;𝑐; = ⁄𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 − 𝐿99 − 𝐿;9 2𝐿9𝐿;
𝑞;� = atan2 𝑠;, 𝑐;two solutions
𝑞;o = atan2 −𝑠;, 𝑐; = − 𝑞;
�
Robotics 1 24
only when 𝑝s9 + 𝑝t9 > 0 …
… being 𝑝s9 + 𝑝t9 = 𝐿9𝑐9 + 𝐿;𝑐9; 9 > 0𝑝s
𝑝𝑦
𝑝𝑧
𝑞1
𝑞2𝑞3
𝑑1
𝐿2
𝐿3
again, two solutions𝑞7� = atan2 𝑝t, 𝑝s
𝑞7o = atan2 −𝑝t, −𝑝s
directkinematics
𝑝s = 𝑐7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝t = 𝑠7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝� = 𝑑7 + 𝐿9𝑠9 + 𝐿;𝑠9;
𝑐7 = v𝑝s ± 𝑝s9 + 𝑝t9
𝑠7 = v𝑝t ± 𝑝s9 + 𝑝t9(else 𝑞1 is undefined —infinite solutions!)
Inverse kinematics of 3R elbow-type armstep 2
Robotics 1 25
define and solve a linear system 𝐴𝑥 = 𝑏in the algebraic unknowns 𝑥 = (𝑐9, 𝑠9)
combine first the two equations of direct kinematics and rearrange the last one
coefficient matrix 𝐴 known vector 𝑏
4 regular solutions for 𝑞9,depending on the combinations of +,− from 𝑞7 and 𝑞;
provided det 𝐴 = 𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 ≠ 0
𝑝s
𝑝𝑦
𝑝𝑧
𝑞1
𝑞2𝑞3
𝑑1
𝐿2
𝐿3
Inverse kinematics of 3R elbow-type armstep 3
𝑐7𝑝s + 𝑠7𝑝t = 𝐿9𝑐9 + 𝐿;𝑐9;
𝑝� − 𝑑7 = 𝐿9𝑠9 + 𝐿;𝑠9;
= 𝐿9 + 𝐿;𝑐; 𝑐9 − 𝐿;𝑠;𝑠9
= 𝐿;𝑠;𝑐9 + 𝐿9 + 𝐿;𝑐; 𝑠9
𝐿9 + 𝐿;𝑐; −𝐿;𝑠;�,o
𝐿;𝑠;�,o 𝐿9 + 𝐿;𝑐;
𝑐9𝑠9 = 𝑐7
�,o 𝑝s + 𝑠7�,o 𝑝t
𝑝� − 𝑑7
(else 𝑞9 is undefined —infinite solutions!)𝑞9
�,� , �,�
= atan2 𝑠9�,� , �,� , 𝑐9
�,� , �,�
Inverse kinematics for robots with spherical wrist
𝑥0
𝑦0
𝑧0
𝑥6𝑦6
𝑧6 = 𝑎first 3 robot jointsof any type (RRR, RRP, PPP, …)
find 𝑞7,⋯ , 𝑞� from the input data§ 𝑝 (origin 𝑂�)§ 𝑅 = 𝑛 𝑠 𝑎 (orientation of 𝑅𝐹�)
𝑧3 𝑧5
j4j5
j6𝑂� = 𝑝
𝑊
𝑑6
j1
1. 𝑊 = 𝑝 − 𝑑�𝑎 ⇒ 𝑞7, 𝑞9, 𝑞; (inverse “position” kinematics for main axes)
2. 𝑅 = )𝑅;(𝑞7,𝑞9,𝑞;) ;𝑅�(𝑞�,𝑞�,𝑞�)
given Euler 𝑍𝑌𝑍 or 𝑍𝑋𝑍rotation matrix with𝑞�,𝑞�,𝑞� (𝜃�,𝜃�,𝜃�)
known,after step 1
⇒ ;𝑅� 𝑞�,𝑞�,𝑞� = )𝑅;'𝑅
Robotics 1 26
𝑧4
⇒ 𝑞�, 𝑞�, 𝑞�
last 3 joints RRR, withaxes intersecting in 𝑊
(inverse “orientation”kinematics for the wrist)two regular
solutions
6R robot Unimation PUMA 600
8 different (regular) inverse solutions that can be found in closed form
sphericalwrist
Robotics 1 27
𝑎 = 0𝑧6(𝑞)
𝑛 = 0𝑥6(𝑞)
𝑠 = 0𝑦6(𝑞)
𝑝 = 𝑂6(𝑞)
here 𝑑� = 0, so that 𝑂� = 𝑊 directly
a function of𝑞7, 𝑞9, 𝑞; only!
Finding nice kinematic relationswhiteboard …
Robotics 1 28
§ the most complex inverse kinematics that could be solved in principle in closed form (i.e., analytically) is that of a 6R serial manipulator, with arbitrary DH table§ ways to systematically generate equations from the direct kinematics that could be
easier to solve ⇒ some scalar equations may contain perhaps a single unknown variable!
§ generating from the direct kinematics a reduced set of equations to be solved (setting w.l.o.g. 𝑑7 = 𝑑� = 0) ⇒ 4 compact scalar equations in the 4 unknowns 𝜃9, … , 𝜃�
Manseur and Doty: International Journal of Robotics Research, 1988
)𝑇� = )𝐴7 𝜃7 7𝐴9 𝜃9 ⋯ �𝐴� 𝜃� = 𝑈)
(*) Paul, Shimano, and Mayer: IEEE Transactions on Systems, Man, and Cybernetics, 1981
method used for theUnimation PUMA 600 in (*)
)𝐴7o7 )𝑇� = 𝑈7 (= 7𝐴9 ⋯ �𝐴�)7𝐴9o7 )𝐴7o7 )𝑇� = 𝑈9 = 9𝐴; ⋯ �𝐴�
⋯�𝐴�o7 ⋯
7𝐴9o7 )𝐴7o7 )𝑇� = 𝑈� (= �𝐴�)
)𝑇� �𝐴�o7 = 𝑉� (= 7𝐴9 ⋯ �𝐴�))𝑇� �𝐴�o7 �𝐴�o7 = 𝑉� = 7𝐴9 ⋯ ;𝐴�
⋯)𝑇� �𝐴�o7 �𝐴�o7 ⋯
7𝐴9o7 = 𝑉7 (= )𝐴7)
or also ...
)𝑇� =𝑛 𝑠 𝑎 𝑝0 0 0 1 = )𝐴� 𝜃
𝑎� = 𝑎' 𝜃 𝑧𝑝� = 𝑝' 𝜃 𝑧
𝑝 9 = 𝑝' 𝜃 𝑝(𝜃)𝑝'𝑎 = 𝑝' 𝜃 𝑎 𝜃
… then solve easily for the remaining 𝜃7 and 𝜃�
solved analyticallyor numerically …
𝑧 = 0 0 1 '
n use when a closed-form solution 𝑞 to 𝑟� = 𝑓4(𝑞) does not exist or is “too hard” to be found
n all methods are iterative and need the matrix 𝐽4 𝑞 = ���(�)��
n Newton method (here only for 𝑚 = 𝑛, at the 𝑘th iteration)n 𝑟� = 𝑓4 𝑞 = 𝑓4 𝑞� + 𝐽4 𝑞� 𝑞 − 𝑞� + 𝑜 𝑞 − 𝑞�
n convergence for 𝑞) (initial guess) close enough to some 𝑞∗: 𝑓4(𝑞∗) = 𝑟�n problems near singularities of the Jacobian matrix 𝐽4 𝑞n in case of robot redundancy (𝑚 < 𝑛), use the pseudo-inverse 𝐽4#(𝑞)n has quadratic convergence rate when near to a solution (fast!)
¬ neglected
Numerical solution of inverse kinematics problems
Robotics 1 29
𝑞��7 = 𝑞� + 𝐽4o7(𝑞�) 𝑟� − 𝑓4 𝑞�
(analytical Jacobian)
Operation of Newton methodn in the scalar case, also known as “method of the tangent”n for a differentiable function 𝑓(𝑥), find a root 𝑥∗ of 𝑓 𝑥∗ = 0 by
iterating as
Robotics 1 30
animation fromhttp://en.wikipedia.org/wiki/File:NewtonIteration_Ani.gif
an approximating sequence
𝑥��7 = 𝑥� −𝑓(𝑥�)𝑓{ 𝑥�
𝑥7, 𝑥9, 𝑥;, 𝑥�, 𝑥�,⋯ ⟶ 𝑥∗
n Gradient method (max descent)n minimize the error function
𝐻 𝑞 = 79 𝑟� − 𝑓4(𝑞) 9 = 7
9 𝑟� − 𝑓4(𝑞) ' 𝑟� − 𝑓4(𝑞)
𝑞��7 = 𝑞� − 𝛼 ∇�𝐻(𝑞�)
Numerical solution of inverse kinematics problems (cont’d)
Robotics 1 31
from∇�𝐻(𝑞) = ⁄𝜕𝐻(𝑞) 𝜕𝑞 ' = − 𝑟� − 𝑓4 𝑞
' ⁄𝜕𝑓4(𝑞) 𝜕𝑞'= −𝐽4'(𝑞) 𝑟� − 𝑓4(𝑞)
𝑞��7 = 𝑞� + 𝛼 𝐽4'(𝑞�) 𝑟� − 𝑓4(𝑞�)
n the scalar step size 𝛼 > 0 should be chosen so as to guarantee a decrease of the error function at each iteration: too large values for 𝛼 may lead the method to “miss” the minimum
n when the step size is too small, convergence is extremely slow
we get
Revisited as a feedback scheme
𝐽4'(𝑞) óõ
𝑓4(𝑞)
+
-
𝑒𝑟� 𝑞�̇�𝑞(0)
𝑟� = cost
𝑒 = 𝑟� − 𝑓4 𝑞 ® 0 ⟺ closed-loop equilibrium 𝑒 = 0is asymptotically stable
𝑉 = 79𝑒'𝑒 ³ 0 is a Lyapunov candidate function
in particular, 𝑒 = 0
asymptotic stability
(𝛼 = 1)
Robotics 1 32
𝑟
�̇� = 𝑒'�̇� = 𝑒'𝑑𝑑𝑡
𝑟� − 𝑓4(𝑞) = −𝑒'𝐽4 𝑞 �̇� = −𝑒'𝐽4 𝑞 𝐽4' 𝑞 𝑒 ≤ 0
�̇� = 0 ⟺ 𝑒 ∈ 𝒩 𝐽4'(𝑞)
null space
Properties of Gradient methodn computationally simpler: use the Jacobian transpose, rather
than its (pseudo)-inversen same 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 the scheme can be accelerated by using a gain matrix 𝐾 > 0
note: 𝐾 ⟶ 𝐾 + 𝐾¦, with 𝐾¦ skew-symmetric, can be used also to “escape” from being stuck in a stationary point of 𝑉 = §
¨ 𝑒'𝐾𝑒, by rotating the error
𝐾𝑒 out of the null space of 𝐽4' (when a singularity is encountered) Robotics 1 33
𝛼 = Δ𝑇𝑞��7 = 𝑞� + ∆𝑇 𝐽4'(𝑞�) 𝑟� − 𝑓4(𝑞�) ,
�̇� = 𝐽4' 𝑞 𝐾𝑒 = 𝐽4' 𝑞 𝐾 𝑟� − 𝑓4(𝑞)
𝑞��7 = 𝑞� +
1𝑠9
𝑐79 𝑠79− 𝑐7 + 𝑐79 − 𝑠7 + 𝑠79 |�¬�
𝛼 − 𝑠7 + 𝑠79 𝑐7 + 𝑐79−𝑠79 𝑐79 |�¬�
× 1 − 𝑐7 + 𝑐791 − 𝑠7 + 𝑠79 |�¬�
A case studyanalytic expressions of Newton and gradient iterations
n 2R robot with 𝑙7 = 𝑙9 = 1, desired end-effector position 𝑟� = 𝑝� = (1,1)n direct kinematic function and error
n Jacobian matrix
n Newton versus Gradient iteration
Robotics 1 34
𝑓4 𝑞 = 𝑐7 + 𝑐79𝑠7 + 𝑠79
𝑒 = 𝑝� − 𝑓4 𝑞 = 11 − 𝑓4(𝑞)
𝐽4 𝑞 =𝜕𝑓4(𝑞)𝜕𝑞 = − 𝑠7 + 𝑠79 −𝑠79
𝑐7 + 𝑐79 𝑐79
𝑒�det 𝐽4(𝑞)
𝐽4o7(𝑞�)
𝐽4'(𝑞�)
Error functionn 2R robot with 𝑙7 = 𝑙9 = 1 and desired end-effector position 𝑝� = (1,1)
Robotics 1 35
plot of 𝑒 9 as a function of 𝑞 = (𝑞7, 𝑞9)
𝑒 = 𝑝� − 𝑓4(𝑞)
two local minima(inverse kinematic solutions)
Configuration space of 2R robotwhiteboard …
n can we represent the correct ‘‘distance’’ between two configurations 𝑞{ and 𝑞{{of this robot on a (square) region in ℝ9?
Robotics 1 36
n configuration space is a torus 𝑆𝑂 1 × 𝑆𝑂 1 , i.e., the surface of a ‘‘donut’’
n the right metric is a geodesic on the torus …
𝑞7
𝑞9
−𝜋 𝜋−𝜋
𝜋𝑞{
𝑞{{
𝑞{
𝑞{{
𝑞{
𝑞{{close or far?
join thetwo sides𝑞7 = −𝜋
and 𝑞7 = 𝜋
join thetwo sides𝑞9 = −𝜋
and 𝑞9 = 𝜋
𝑞7
𝑞9
(0,0)
&
Error reduction by Gradient methodn flow of iterations along the negative (or anti-) gradientn two possible cases: convergence
Robotics 1 37
start
one solution
local maximum(stop if this is the initial guess)
. .
another start...
...the other solution
saddle point(stop after some iterations)
(𝑞7, 𝑞9){ = (0, 𝜋/2) (𝑞7, 𝑞9){{ = (𝜋/2,−𝜋/2) (𝑞7, 𝑞9)1®s = (−3𝜋/4,0) (𝑞7, 𝑞9)¦®��¯° = (𝜋/4,0)
𝑒 ∈ 𝒩(𝐽4'(𝑞)) !
or stuck (at zero gradient)
Convergence analysiswhen does the gradient method get stuck?
n lack of convergence occurs whenn the Jacobian matrix 𝐽4(𝑞) is not full rank (the robot is in a “singular configuration”)n AND the error 𝑒 is in the null space of 𝐽4'(𝑞)
Robotics 1 38
(𝑞7, 𝑞9)1®s = (−3𝜋/4,0)
(𝑞7, 𝑞9)¦®��¯° = (𝜋/4,0) 𝑒 ∈ 𝒩(𝐽4'(𝑞))
𝑝�
𝑝
𝑒 ∉ 𝒩(𝐽4'(𝑞)) !!
𝑝�
𝑝
𝑒 ∈ 𝒩(𝐽4'(𝑞))𝑝�
𝑝
(𝑞7, 𝑞9) = (𝜋/9,0)
the algorithm will proceed in this case,
moving out ofthe singularity
𝑞1
𝑞2
𝑝� =11
𝑒 = 𝑝� − 𝑝 =1 − 21 − 2
𝐽4' 𝑞 = − 𝑠7 + 𝑠79 𝑐7 + 𝑐79−𝑠79 𝑐79 |�¬�²³´´µ¶
= − 2 2− 2 2
Issues in implementationn initial guess 𝑞)
n only one inverse solution is generated for each guessn multiple initializations for obtaining other solutions
n optimal step size 𝛼 > 0 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 𝛼 at each iterationn stopping criteria
n understanding closeness to singularities
Robotics 1 39
Cartesian error(possibly, separate for
position and orientation)
algorithmincrement
𝑟� − 𝑓4 𝑞� ≤ 𝜀 𝑞��7 − 𝑞� ≤ 𝜀�
good numerical conditioning of Jacobian matrix (SVD)
(or a simpler test on its determinant, for 𝑚 = 𝑛)𝜎1\+ 𝐽4 𝑞� ≥ 𝜎)
Numerical tests on RRP robot
n RRP/polar robot: desired E-E position 𝑟� = 𝑝� = 1, 1, 1—see slide 21, with 𝑑7 = 0.5
n the two (known) analytical solutions, with 𝑞; ≥ 0, are𝑞∗ = (0.7854, 0.3398, 1.5)𝑞∗∗ = (𝑞7∗ − 𝜋, 𝜋 − 𝑞9∗, 𝑞;∗) = (−2.3562, 2.8018, 1.5)
n norms 𝜀 = 10o� (max Cartesian error), 𝜀� = 10o� (min joint increment)n 𝑘1®s = 15 (max # iterations), det 𝐽4(𝑞) ≤ 10o� (singularity closeness)
n numerical performance of Gradient (with different steps 𝛼) vs. Newtonn test 1: 𝑞) = (0, 0, 1) as initial guessn test 2: 𝑞) = (−𝜋/4, 𝜋/2, 1)— ‘‘singular” start, since 𝑐9 = 0 (see slide 21)n test 3: 𝑞) = (0, 𝜋/2, 0)— ‘‘doubly singular” start, since also 𝑞; = 0n solution and plots with Matlab code
Robotics 1 40
Numerical test - 1n test 1: 𝑞) = (0, 0, 1) as initial guess; evolution of error norm
Robotics 1 41
Gradient: 𝛼 = 0.5
slow, 15 (max)iterations
Gradient: 𝛼 = 1
too large, oscillatesaround solution
Newton
very fast, convergesin 5 iterations
0.15⋅10-8
Gradient: 𝛼 = 0.7
good, convergesin 11 iterations
0.57⋅10-5
Cartesian errorscomponent-wise
𝑒𝑥
𝑒𝑦
𝑒𝑧
Numerical test - 1
n test 1: 𝑞) = (0, 0, 1) as initial guess; evolution of joint variables
Robotics 1 42
Gradient: 𝛼 = 1 Gradient: 𝛼 = 0.7not convergingto a solution
converges in 11 iterations
Newtonconverges in 5 iterations
both to the same solution 𝑞∗ = (0.7854, 0.3398, 1.5)
Numerical test - 2n test 2: 𝑞) = (−𝜋/4, 𝜋/2, 1): singular start
Robotics 1 43
Gradient𝛼 = 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 𝑒 ∈ 𝒩 𝐽4'(𝑞) jo
int v
aria
bles
!!
Numerical test - 3n test 3: 𝑞) = (−𝜋/4, 𝜋/2, 1): doubly singular start
Robotics 1 44
Newtonis either
blocked at startor (w/o check)
explodes!⇒ “NaN” in Matlaber
ror n
orm
Gradient (with 𝛼 = 0.7)① starts toward solution② exits the double singularity③ slowly converges in 19
iterations to the solution𝑞∗ = (0.7854, 0.3398, 1.5)
join
t var
iabl
es
Carte
sian
erro
rs
➀
➁
➂
0.96⋅10-5
𝑒𝑥
𝑒𝑦
𝑒𝑧
Final remarksn an efficient iterative scheme can be devised by combining
n 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 drives 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 𝑡), 𝑡7 = 𝑡) + 𝑇, ..., 𝑡� = 𝑡�o7 + 𝑇 (e.g., every 𝑇 = 40 ms) n a “good” choice for the initial guess 𝑞) at 𝑡� is the solution of the previous
problem at 𝑡�o7 (provides continuity, requires only 1-2 Newton iterations)n crossing of singularities and handling of joint range limits need special care
n Jacobian-based inversion schemes are used also for kinematic control, moving along a continuous task trajectory 𝑟�(𝑡)
Robotics 1 45
Recommended