A Combined Optimization Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators Roland Mai B659, Spring 2010 Indiana 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 q 1 and q 2, find ( x m, x y ) ▫Generally:  Q   Inverse Kinematics ▫You know ( x n, y n ), find q 1 and q 2 ▫Generally:  -1 : M  Q ▫IK Problem: Find  -1 Q and M may not be homeomorphic or of the same dimensions. m ( x m, y m ) n ( x n, y n ) Forward vs Inverse Kinematics 3

Solving the IK Problem Consider the following robot arm with 3 DOF    c (x c, y c ) - current t (x t, y t ) - target     and   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 P d are the Cartesian Coordinates. R d =[d 1 d 2 d 3 ] T where d i are unit vectors along x d, y d and z d axis. o d is the origin of the coordinate system. z1z1 x1x1 y1y1 zdzd xdxd ydyd d3d3 d1d1 d2d2 PdPd 6

Description of Current Position Generally: q = [q 1 q 2 … q n ] T where q i are the joint variables. P h (q) current position of coordinate system: (x h, y h, z h ) R h (q) = [h 1 (q) h 2 (q) h 3 (q)] T where h i (q) are unit vectors according to x h, y h, and z h axes. z1z1 x1x1 y1y1 zhzh xhxh yhyh h3h3 h1h1 h2h2 PhPh 7

Goal & Error/Objective Function Minimize Position Error (i.e. ||  P||):  P(q) =  P(q)   P(q) If o d = o h, then d i  h i (q) = 1. Minimize Orientation Error:  O(q) =  (d i  h i (q) – 1) 2 Total Error: E(q) =  P(q) +  O(q) z1z1 x1x1 y1y1 zdzd xdxd ydyd PdPd zhzh xhxh yhyh PhPh PP d3d3 d1d1 d2d2 h3h3 h1h1 h2h2 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(  ) = w p g 1 (  ) + w o g2(  ) where g 1 (  ) = P id  [R(z i,  )] P ih – how close to o d the rotation got us g 2 (  ) =  cos(  i (  )) =   d i  h i (  ) – rotational offset w p and w o are weighing factors g(  ) is called the objective function. It can then be shown, that g(  ) is maximized when: d g(  ) d  d 2 g(  ) d   = (k1 – k2) sin(  ) + k 3 cos(  ) = 0 and = (k1 – k2) cos(  ) - k 3 sin(  ) < 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 = (q 1, q 2, …, q n ) Initialize H -1 = I and x 0 = Start Point,  = Step Size  x = -  k H -1  g(x k ) x k+1 = x k +  x Set y k =  g(x k+1 ) -  g(x k ) and H k+1 -1 = complex formula that depends on y k,  x and H k. Stop when some threshold  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 , Aug “Quasi-Newton Methods” edu/education/ME6103/Quasi-Newton.ppt Luenberger, David & Ye Yinyu “Linear and Nonlinear Programming” 3 rd Ed (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 , Sept