Inverse Kinematics of a Rhino Robot

Transcription

1 Inverse Kinematics of a Rhino Robot Rhino Robot ( A Rhino robot is very similar to a 2-link arm with the exception that The base can rotate, allowing you to reach any point in 3-space which is less than 1m away from the base, and There are 3-links, creating redundancy (creating an infinite number of solutions). Recall that the reference frames for a Rhino Robot are: Z1 Y1 Z3 Z2 a2 = 5 a3 = 5 Shoulder X1 Elbow X3 Wrist X2 Y2 Y3 Y5 d1 = 5 Z4 X4 Y4 d5 = 5 X5 Z5 Base Z Y X Reference Frames for a Rhino Robot: The parameters for a Rhino Robot are: 1 May 21, 218

2 Link i α i 1 a i 1 d i θ i The angle between the Zi-1 and Zi axis (twist) The distance from Zi-1 to Zi measured along the Xi-1 axis The distance from Xi-1 to Xi measured along the Zi axis (cm) 1 deg d1 = 5 θ deg θ 2 3 deg a2 = 5 θ 3 4 deg a3 = 5 θ deg d5 = 5 6 The angle between Xi-1 and Xi measured about the Zi axis With all the axis aligned, the forward kinematics simplify somewhat. The previous Matlab routine RRR works for a Rhino robot as well if you change the robot definition: Screen Shot of the Rhino Robot at home position: Q = {,,,, } 2 May 21, 218

3 Forward Kinematics: Given the joint angles, the tip position is: P = T 1 T 12 T 23 T 34 T 45 P 5 where P5 is the origin of reference frame #5 (the tip). For example, at zero position (shown above), the tip position is >> Rhino([;;], [;;;1]) x 1. y. z Inverse Kinematics solves the inverse problem: given the tip position, determine the joint angles. Inverse Kinematics: Algebraic Solution: Given the joint angles, the tip position is known via transformation matricies. From before, the transform matrix to go from reference frame 1 to reference frame is or T 1 = R x (α )D x (a )R z (θ 1 )D z (d 1 ) T 1 = For a Rhino robot Repeating T 1 = T 12 = T 34 = cθ 1 sθ 1 a sθ 1 cα cθ 1 cα sα sα d 1 sθ 1 sα cθ 1 sα cα cα d 1 cθ 1 sθ 1 sθ 1 cθ cθ 2 sθ 2 sθ 2 cθ 2 cθ 4 sθ 4 5 sθ 4 cθ 4 3 May 21, 218

4 P = T 1 T 12 T 23 T 34 P 4 P = c 1 s 1 s 1 c c 2 s 2 s 2 c 2 c 3 s 3 5 s 3 c 3 c 4 s 4 5 s 4 c 4 1 P = ((5c 3 + 5)c 2 5s 2 s 3 )c 1 ((5c 3 + 5)c 2 5s 2 s 3 )s 1 (5c 3 + 5)s 2 5c 2 s or, to solve for the three angles, simply solve the following three equations for three unknowns: x = ((5c 3 + 5)c 2 5s 2 s 3 )c 1 y = ((5c 3 + 5)c 2 5s 2 s 3 )s 1 z = (5c 3 + 5)s 2 5c 2 s Inverse Kinematics: Geometric Solution If know the tip position and the orientation of the hand, you can compute the wrist position. Similarly, we'll assume you want to know the wrist position and compute the joint angles that place the wrist. qc.5.5 Y (x4, y4) (x3,y3) q1 X (,.5) qb qa c r a Z (x2,y2).5 Frame Top View Side View Along Angle Q1 This results in θ = arctan y 4 x 4 Along the plane defined by angle Q1, 4 May 21, 218

5 a = z 4.5 r 2 = x y a 2 c 2 = a 2 + r 2 The, from before (with the indices of the angles adjusted) θ a = arctan ( a r) c 2 = a 2 + b 2 2ab cos θ c c 2 = cos θ c θ c =±arccos (1 2c 2 ) θ 3 = 18 θ c θ b = 18 θ c 2 θ 2 = 9 (θ a +θ b ) The corresponding Matlab Code: function [Q] = RhinoInverseKinematics(Wrist) x4 = Wrist(1) / 1; y4 = Wrist(2) / 1; z4 = Wrist(3) / 1; q1 = atan2(y4,x4); a = z4 -.5; r = sqrt(x4^2 + y4^2); c = sqrt(r^2 + a^2); qa = atan2(a, r); qc = acos(1-2*c^2 ); q3 = qc - pi; qb = (pi - qc)/2; q2 = qa + qb; if (q3 < ) q3 = -q3; q2 = -q2; end Q = [q1; q2; q3]; end 5 May 21, 218

6 Checking the forward and inverse kinematics: see if the point (5cm, -25cm, 3cm) returns the same point. Assume the wrist is pointing down, meaning θ 2 +θ 3 +θ 4 = >> Q = RhinoInverseKinematics([5, -25, 3])*18/pi >> Q = [Q; -(Q(2)+Q(3)); ] >> Rhino(Q*pi/18) Note that the resulting (x, y, z) position is correct: it is the point you input, 5cm lower in the z-direction due to the wrist pointing down with a length of 5cm. Repeating for another point: (2, 3, 7) >> Q = RhinoInverseKinematics([2, 3, 7])*18/pi >> Q = [Q; -(Q(2)+Q(3)); ] >> Rhino(Q*pi/18) Check: you return the point you passed and the joint angles have the elbow up (Q3 > ) Example: Draw a box in 3-space. Assume the box has coordinates (units cm) (5, -25, -25) (5, +25, -25) (5, +25, 25) (5, -25, 25) t = [:.1:1]'; z = *t; 6 May 21, 218

7 xt = [ z; z ; z ; z ] + 5; yt = [ t; z+1 ; 1-t ; z ]*5-25; zt = [ z; t ; 1+z ; 1-t]*5-2; q = []; for i=1:length(xt) Q = RhinoInverseKinematics([xt(i),yt(i), zt(i)]); Q = [Q; -(Q(2)+Q(3)); ]; q = [q; Q']; Rhino(Q); plot(yt-.77*xt, zt-.77*xt,'g'); pause(.1); end This results in the simulation looking like the following at the end: Position of the Rhino Robot as is traces out a square in 3-space with the resulting joint angles vs. time 7 May 21, 218

8 Joint Angles for a Rhino Robot as is traces out a square. Q1 (blue), Q2 (green), Q3 (red), Q4 (cyan), Q5 (pink) 8 May 21, 218