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