a combined optimization method for solving the inverse kinematics problem of mechanical manipulators...
TRANSCRIPT
A Combined Optimization Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators
Roland MaiB659, Spring 2010Indiana University
Outline
•Forward vs Inverse Kinematics•Strategy of Method•End-Effector Representation•Review: The Newton-Raphson Method•Cyclic Coordinate Descent•Newton’s Method in High Dimensions•BFGS Algorithm•Performance and other considerations
2
•Forward Kinematics▫You know q1 and q2, find (xm, xy)▫Generally: : f Q M
•Inverse Kinematics▫You know (xn, yn), find q1 and q2
▫Generally: f-1: M Q▫IK Problem: Find f-1
•Q and M may not be homeomorphic or of the same dimensions.
m (xm, ym)
n (xn, yn)
Forward vs Inverse Kinematics
3
Solving the IK Problem
•Consider the following robot arm with 3 DOF
q1
q2
q3
c (xc, yc) - current
t (xt, yt) - target
q1, q2 and q3 need to be such that end-effector reaches t.
Idea: Iteratively minimize error to goal until some threshold.
4
Strategy of Method•Modify the IK problem into an optimization
problem where we try to minimize the error from current configuration to the goal configuration.
•Use Cyclic Coordinate Descent to find a good starting point for the Broyden-Feltcher-Goldfard-Shanno (BFGS) algorithm.
•Run BFGS algorithm to find the joint angles.5
Description of End-Effector’s Goal
• Pd are the Cartesian Coordinates.
• Rd =[d1 d2 d3]T where di are unit vectors along xd, yd and zd axis.
• od is the origin of the coordinate system.
z1
x1 y1
zd
xd yd
d3
d1
d2
Pd
6
Description of Current PositionGenerally:• q = [q1 q2 … qn]T where qi are
the joint variables.
• Ph(q) current position of coordinate system: (xh, yh, zh)
• Rh(q) = [h1(q) h2(q) h3(q)]T
where hi(q) are unit vectors according to xh, yh, and zh axes.
z1
x1 y1
zh
xh yh
h3
h1
h2
Ph
7
Goal & Error/Objective FunctionMinimize Position Error (i.e. ||
dP||):DP(q) = dP(q) dP(q)
If od = oh, then di hi (q) = 1.
Minimize Orientation Error:
DO(q) = S(di hi (q) – 1)2
Total Error:E(q) = DP(q) + DO(q)
z1
x1 y1
zd
xd
yd
Pd zh
xh
yh
Ph
dP
d3
d1d2
h3h1
h2
8
BFGS Algorithm
•BFGS ~ Broyden-Fletcher-Goldfard-Shanno
•A Quasi-Newtonian method.•A directed heuristic search for minimizing
an objective function.•Starting point of the algorithm matters on
stability of solution.▫Cyclic Coordinate Descent provides a very
good start for the BFGS algorithm.▫Next: Review Newton-Raphson method.
Review: The Newton-Raphson Method
•It is an iterative method to quickly find good root approximations to a real valued function.
10
Review: The Newton-Raphson Method
•Problems: The derivates will keep oscillating. Starting at a good point is important.
11
Cyclic Coordinate Descent (CCD)
• 3 DOF arm reaching toward target. A full cycle of the iteration:
The result is a good starting point for the BFGS algorithm.
12
Cyclic Coordinate Descent (CCD)For rotational joints it can be shown that
minimizing E(q) is equivalent to maximizing:
g(f) = wp g1(f) + wo g2(f) whereg1(f) = Pid [R(zi, f)] Pih – how close to od the rotation got us
g2(f) = S cos(i(f)) = S di hi(f) – rotational offsetwp and wo are weighing factors
g(f) is called the objective function.It can then be shown, that g(f) ismaximized when:d g(f) d fd2g(f)d f2
= (k1 – k2) sin(f) + k3 cos(f) = 0 and
= (k1 – k2) cos(f) - k3 sin(f) < 0
13
Newton’s Method in High Dimensions
Where H is the Hessian matrix and is called the gradient, and is the step size.
14
Newton’s Method in High Dimensions
If H was the identity I, Newton’s method would be the gradient descent.
For this example the minima is found very quickly.
15
Newton in High Dimensions
•The Hessian matrix is expensive to compute. H-1 is even more expensive.
•Solution: BFGS Algorithm▫It is a type of Quasi-Newton method
because it estimates the value of H-1 instead of directly computing it.
▫It is preferred over the Davidon-Fletcher-Powel Formula; however a combination of these two may be used.
16
BFGS Algorithm• Inputs: Objective Function, Step Size, Start Point• Outputs: q = (q1, q2, …, qn)
• Initialize H-1 = I and x0 = Start Point, a = Step Size
• Dx = -ak H-1 g(xk)
• xk+1 = xk + Dx
• Set yk = g(xk+1) - g(xk) and
• Hk+1 -1= complex formula that depends on yk,
Dx and Hk. Stop when some threshold e is reached.
k
17
Performance
•Performs better than a very similar method by Kazerounian.
•Even though we talked only about rotational joints, it works well with translational joints.
•Experimental test show that the method works for various problems.
•Has the advantage to not be sensitive to the original position of the robot because of CCD. 18
References• Li-Chun Tommy Wang and Chih Cheng Chen. “A
Combined Optimization Method for Solving the Inverse Kinematics Probem of Mechanical Manipulators”. IEEE Transactions on Robotics and Automation., vol 7, no4. pp. 490-499, Aug 1991.
• “Quasi-Newton Methods” http://www.srl.gatech.edu/education/ME6103/Quasi-Newton.ppt
• Luenberger, David & Ye Yinyu “Linear and Nonlinear Programming” 3rd Ed. 2008. (Chapter 10)
• L.T. Wang and B. Ravani, “Recursive computations of kinematic and dynamic equations for mechanical manipulators” IEEE J. Robotic Automat., vol. RA-1, no. 3, pp. 124-131, Sept. 1985. 19