Robotics: Tutorial 3

Size: px

Start display at page:

Download "Robotics: Tutorial 3"

Aldous Anthony
5 years ago
Views:

1 Robotics: Tutorial 3 Mechatronics Engineering Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz German University in Cairo Faculty of Engineering and Material Science October 4, 2016 Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 1 / 33

2 Inverse Position Kinematics Introducing one of the methods available to solve inverse position kinematics level. Using one of the most important numerical methods, Newton Raphson. It depends on finding a numerical solution of a non-linear equation(s) instead of evaluating the closed form solution. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 2 / 33

3 Newton Raphson - Graphical Approach Now, For the shown non-linear function: Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 3 / 33

4 Starting from Any initial guess,we evaluate the function F (x 0 ) at x 0. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 4 / 33

5 Then get the slope F (x 0 ), which when intersect the x-axis, produce (in most cases) a point closer to the rest of the function. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 5 / 33

6 This is done in several iteration. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 6 / 33

7 until it converge to the acceptable error value. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 7 / 33

8 Problems of Newton Raphson Function diverge away from the root by choosing the wrong initial solution. Slope = zero at initial solution chosen. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 8 / 33

9 Newton Raphson - Mathematical Approach Let s take a look at the mathematics behind this: at the initial guess point (x 0 )), the first derivative equation, which is the slope as well: slope = y x = F (x 0) 0 (1) x 0 x 1 F (x 0 )[x 0 x 1 ] = F (x 0 ) (2) x 0 x 1 = F (x 0) F (x 0 ) which can be written as:(this is for a single iteration, starting an initial guess). x 1 = x 0 [ F (x 0 )] 1 F (x 0 )] (4) In General Form: (3) x n+1 = x n [ F (x n )] 1 F (x n )] (5) where, n is number of current iteration. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 9 / 33

10 Application of Newton Raphson in Robotics We will expand this same formula for our application, in robotics,by changing equation from scalar into matrix and mainly to be used in the inverse position kinematics problem, where we search for the values of (q) joint angles that will make end effector reach desired goal. Thus, q n+1 = q n [ F q ] 1 q=qn F (q n ) (6) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 10 / 33

11 Example 1 For the shown 2 DOF planar robot, Derive the inverse position level Kinematics equations: Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 11 / 33

12 Solution From the forward kinematics (previous tutorial): x = l 1 cos(q 1 ) + l 2 cos(q 2 ) (7) l 1 cos(q 1 ) + l 2 cos(q 2 ) x = 0 (f 1 ) (8) y = l 1 sin(q 1 ) + l 2 sin(q 2 ) (9) l 1 sin(q 1 ) + l 2 sin(q 2 ) y = 0 (f 2 ) (10) Relation between joints (q) and position (x) is nonlinear. Therefore, for the inverse kinematics solution, Newton Raphson technique is used: We ll be searching for the values of q 1,q 2 that satisfy these function, and accordingly end-effector reach its destination: [ q1 q 2 ] 1 values after one iteration = [ q1 ] q 2 0 initial values [ F1 F 2 F 2 F 1 ] 1 [ f1 (q 1, q 2 ) 0 f 2 (q 1, q 2 ) 0 ] (11) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 12 / 33

13 Evaluation of different elements of equation (11) and Substitution: [ q 1 q 2 ] 1 [ = q 1 q 2 ] 0 [ l 1 sin(q 1 ) l 2 sin(q 2 ) l 1 cos(q 1 ) l 2 cos(q 2 ) ] 1 q 1 =q 10, q 2 =q 20 [ l 1 cos(q 10 )+l 2 cos(q 20 ) x l 1 sin(q 10 )+l 2 sin(q 20 ) y This produce values of q 1, q 2 after 1 iteration, and then this process is repeated for several iterations, in which their values converge to the acceptable values to satisfy the equations f 1, f 2 and these values become the solution of the inverse position level kinematics. (12) ] Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 13 / 33

14 Inverse Velocity Kinematics Relation between end effector velocities vector and Joints velocities vector is linear, and we can get q 1 and q 2 from position kinematics. The solution is: q = J 1 (q)ẋ (13) q Joints velocities vector. J 1 (q) Inverse of Jacobian Matrix. can be calculated using MATLAB. ẋ end effector velocities vector. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 14 / 33

15 Inverse Acceleration Kinematics Relation between end effector acceleration vector and Joints acceleration vector is Affine (Linear relation with a shift) and we can get q 1 and q 2 from velocity kinematics. q = J 1 (q)[ẍ J(q) q] (14) q Joints acceleration vector. J 1 (q) Inverse of Jacobian Matrix. can be calculated using MATLAB. Ẍ end effector acceleration vector. J(q) rate of change of Jacobian matrix. q joints velocities vector. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 15 / 33

16 Example 2 For the shown 3 DOF non-planar robot, Derive the equations for position, velocity and acceleration level Kinematics equations (Forward and inverse): Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 16 / 33

17 The Forward Position Level Kinematic First Step: Assign the frames: Newtonian Frame and local frames at each joint. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 17 / 33

18 Second Step: Write Down the loop equations moving from Origin (O) to the end-effector (E): r OE = x n 1 + y n 2 + z n 3 (15) r OE = l 1 a 3 + l 2 b 2 + l 3 b 3 (16) Third Step: Derive the rotation matrices: n 1 n 2 n 3 a 1 cos(q 1 ) sin(q 1 ) 0 a 2 sin(q 1 ) cos(q 1 ) 0 a b 2 parallel to a 2 a1 a2 a3 b 1 cos(q 2 ) 0 sin(q 2 ) b b 3 sin(q 2 ) 0 cos(q 2 ) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 18 / 33

19 where, a 1 = cos(q 1 ) n 1 + sin(q 1 ) n 2 (17) a 2 = sin(q 1 ) n 1 + cos(q 1 ) n 2 (18) a 3 = n 3 (19) For second frame, (with respect to first frame) b 1 = cos(q 2 ) a 1 + sin(q 2 ) a 3 (20) b 2 = a 2 (21) b 3 = sin(q 2 ) a 1 + cos(q 2 ) a 3 (22) (with respect to Newtonian frame),from equation (18) and (21): b 2 = sin(q 1 ) n 1 + cos(q 1 ) n 2 (23) From equation (17),(19) and (22): b 3 = sin(q 2 )cos(q 1 ) n 1 sin(q 2 )sin(q 1 ) n 2 + cos(q 2 ) n 3 (24) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 19 / 33

20 Substituting equations (19),(23) and (24) in (15), and equating both sides of the equation (15) and (16): r OE = l 1 n 3 l 2 sin(q 1 ) n 1 + l 2 cos(q 1 ) n 2 l 3 sin(q 2 )cos(q 1 ) n 1 l 3 sin(q 2 )sin(q 1 ) n 2 + l 3 cos(q 2 ) n 3 (25) x n 1 + y n 2 + z n 3 = l 1 n 3 l 2 sin(q 1 ) n 1 + l 2 cos(q 1 ) n 2 l 3 sin(q 2 )cos(q 1 ) n 1 l 3 sin(q 2 )sin(q 1 ) n 2 + l 3 cos(q 2 ) n 3 (26) Taking common factors for the right side of the equation: x n 1 + y n 2 + z n 3 = ( l 2 sin(q 1 ) l 3 sin(q 2 )cos(q 1 )) n 1 + (l 2 cos(q 1 ) l 3 sin(q 2 )sin(q 1 )) n 2 + (l 1 + l 3 cos(q 2 )) n 3 (27) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 20 / 33

21 Forward Position level Kinematics In matrix form, so that X = f (q): x y z = l 2 sin(q 1 ) l 3 sin(q 2 )cos(q 1 ) l 2 cos(q 1 ) l 3 sin(q 2 )sin(q 1 ) l 1 + l 3 cos(q 2 ) (28) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 21 / 33

22 Inverse Position Level Kinematic Using the Newton Raphson technique: q 1 q 2 l 3 n+1 = q 1 q 2 l 3 F 1 F 1 F 1 l 3 F 2 F 2 F 2 l 3 n F 3 F 3 F 3 l 3 and the functions are (from forward kinematics): l 2 sin(q 1 ) l 3 sin(q 2 )cos(q 1 ) x = 0 (f 1 ) l 2 cos(q 1 ) l 3 sin(q 2 )sin(q 1 ) y = 0 (f 2 ) l 1 + l 3 cos(q 2 ) z = 0 (f 3 ) 1 q 1 =q 1n q 2 =q 2n L 3 =L 3n f 1 (q 1, q 2, l 3 ) n f 2 (q 1, q 2, l 3 ) n f 3 (q 1, q 2, l 3 ) n (29) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 22 / 33

23 By Substitution/ Evaluation of different elements of this equation: q 1 q 2 = q 1 q 2 l 3 l [ l2cos(q 1) + l 3sin(q 2)sin(q 1) l 3cos(q 2)cos(q 1) sin(q 2)cos(q 1) l 2sin(q 1) l 3sin(q 2)cos(q 1) l 3cos(q 2)sin(q 1) sin(q 2)sin(q 1) 0 l 3sin(q 2) cos(q 2) l 2 sin(q 1 ) l 3 sin(q 2 )cos(q 1 ) x l 2 cos(q 1 ) l 3 sin(q 2 )sin(q 1 ) y l 1 + l 3 cos(q 2 ) z ] 1 (30) This produce values of q 1, q 2 and L 3 after 1 iteration, and then this process is repeated for several iterations, in which their values converge to the acceptable values to satisfy the equations f 1, f 2 and f 3 and these values become the solution of the inverse position level kinematics. q 1 = q 10 q 2 = q 20 l 3 = l 30 Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 23 / 33

24 Forward Velocity Level Kinematics Using Chain rule: ( ) dx x dt = q q (once for each q, in this case q 1,q 2 and L 3 ). ẋ = L 2 cos(q 1 ) q 1 x + L 3 sin(q 2 )sin(q 1 ) x q 1 sin(q 2 )cos(q 1 )) L 3 x L 3 L 3, L 3 cos(q 2 )cos(q 1 ) x q 2 (31) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 24 / 33

25 Similarly for ẏ, ẏ = L 2 sin(q 1 ) q 1 y Similarly for ż, L 3 sin(q 2 )cos(q 1 ) y + sin(q 2 )sin(q 1 ) L 3 y L 3 L 3 ż = L 3 sin(q 2 ) q 2 z q 1, + cos(q 2 ) x L 3 L 3 cos(q 2 )sin(q 1 ) y L 3 L 3 q 2 (32), (33) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 25 / 33

26 Place forward velocity level kinematics equation (31),(32) and (33) in a matrix form so that Ẋ = J(q) q: ẋ ẏ ż = L 2 cq 1 + L 3 sq 2 sq 1 L 3 cq 2 cq 1 sq 2 cq 1 L 2 sq 1 L 3 sq 2 cq 1 L 3 cq 2 sq 1 sq 2 sq 1 0 L 3 sq 2 cq 2 q 1 q 2 L 3 (34) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 26 / 33

27 Inverse Velocity Level Kinematics Place in q = J 1 (q)ẋ : q 1 q 2 L 3 = L 2 cq 1 + L 3 sq 2 sq 1 L 3 cq 2 cq 1 sq 2 cq 1 L 2 sq 1 L 3 sq 2 cq 1 L 3 cq 2 sq 1 sq 2 sq 1 0 L 3 sq 2 cq 2 1 ẋ ẏ ż (35) The inverse of Jacobian Matrix can be calculated using MATLAB. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 27 / 33

28 Forward Acceleration Level Kinematics ( Using the chain rule: dẋ dt = ẋ q q + ẋ q q ) ẍ = L 2 sin(q 1 ) q 1 q 1 L 2 cos(q 1 ) q 1 + L 3 cos(q 2 )sin(q 1 ) q 1 q 2 + L 3 sin(q 2 )cos(q 1 ) q 1 q 1 ẋ + L 3 sin(q 2 )sin(q 1 ) ẋ q 1 q 1 q 1 + sin(q 2 )sin(q 1 ) q 1 ẋ L 3 + L 3 cos(q 2 )sin(q 1 ) q 2 ẋ q 1 ẋ q 1 q 1 + L 3 sin(q 2 )cos(q 1 ) q 2 q 2 ẋ l 3 l 3 L 3 cos(q 2 )cos(q 1 ) q 2 ẋ cos(q 2 )cos(q 1 ) q 2 L 3 ẋ + L 3 sin(q 2 )sin(q 1 ) ẋ q 1 ẋ q 2 L 3 cos(q 2 )cos(q 1 ) ẋ q 2 q 2 ẋ l 3 sin(q 2 )cos(q 1 ) L 3, ẋ l 3 Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 28 / 33 l 3 l 3 (36)

29 ÿ = L 2 cos(q 1 ) q 1 q 1 L 2 sin(q 1 ) q 1 + L 3 sin(q 2 )sin(q 1 ) q 1 q 1 L 3 cos(q 2 )cos(q 1 ) q 1 q 2 ẏ ẏ q 1 q 1 ẏ ẏ L 3 sin(q 2 )cos(q 1 ) q 1 sin(q 2 )cos(q 1 ) q 1 L 3 L 3 cos(q 2 )cos(q 1 ) q 2 q 1 ẏ q 1 q 1 ẏ l 3 l 3 ẏ + L 3 sin(q 2 )sin(q 1 ) q 2 q 2 L 3 cos(q 2 )sin(q 1 ) q 2 cos(q 2 )sin(q 1 ) q 2 L 3 ẏ ẏ q 2 q 2 ẏ l 3 l 3 L 3 sin(q 2 )cos(q 1 ) q 1 L 3 cos(q 2 )sin(q 1 ) q 2 sin(q 2 )sin(q 1 ) L 3, ẏ ẏ ẏ l 3 l 3 (37) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 29 / 33

30 z = L 3 cos(q 2 ) q 2 q 2 L 3 sin(q 2 ) q 2 sin(q 2 ) q 2 L 3 L 3 sin(q 2 ) q 2 + cos(q 2 ) L 3, ż ż q 2 q 2 ż l 3 l 3 ż ż l 3 l 3 (38) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 30 / 33

31 Forward Acceleration Kinematics Solution: Place equation (36),(37) and (38) in a matrix form so that Ẍ = J(q) q + J(q) q. ẍ ÿ z = L 2 c(q 1 )+L 3 s(q 2 )s(q 1 ) L 3 c(q 2 )c(q 1 ) s(q 2 )c(q 1 ) L 2 s(q 1 ) L 3 s(q 2 )c(q 1 ) L 3 c(q 2 )s(q 1 ) s(q 2 )s(q 1 ) 0 L 3 s(q 2 ) c(q 2 ) L 2 sq 1 q 1 +L 3 sq 2 cq 1 q 1 +L 3 cq 2 sq 1 q 2 + L 3 sq 2 sq 1 L 3 cq 2 sq 1 q 1 +L 3 sq 2 cq 1 q 2 L 3 cq 2 cq 1 sq 2 sq 1 q 1 cq 2 cq 1 q 2 L 2 cq 1 q 1 +L 3 sq 2 sq 1 q 1 L 3 cq 2 cq 1 q 2 L 3 sq 2 cq 1 L 3 cq 2 cq 1 q 1 +L 3 sq 2 cq 2 q 2 L 3 cq 2 sq 1 sq 2 cq 1 q 1 cq 2 sq 1 q 2 q 1 q 2 L L 3 cq 2 q 2 L 3 sq 2 sq 2 q 2 q 1 q 2 L 3 (39) Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 31 / 33

32 Inverse Acceleration Level Kinematics The inverse acceleration kinematics solution is: q = J 1 (q)[ẍ J(q) q] where, J 1 (q) Inverse of Jacobian Matrix can be calculated using MATLAB. Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 32 / 33

33 Any Questions? Thank you Dr. Islam Khalil, MSc. Omar Mahmoud, Eng. Lobna Tarek and Eng. Abdelrahman Ezz Robotics: Tutorial 3 33 / 33

Lecture Note 8: Inverse Kinematics

ECE5463: Introduction to Robotics Lecture Note 8: Inverse Kinematics Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 8 (ECE5463