Kinematics for a Three Wheeled Mobile Robot

Transcription

1 Kinematics for a Three Wheeled Mobile Robot Randal W. Beard Updated: March 13, Reference Frames and 2D Rotations î 1 y î 2 y w 1 y w w 2 y î 2 x w 2 x w 1 x î 1 x Figure 1: The vector w can be expressed with respect to two different reference frames. Figure 1 shows a vector w and two different coordinate frames with orthogonal axes denoted F 1 = {i 1 x, i 1 y, i 1 z} for Frame 1, and F 2 = {i 2 x, i 2 y, i 2 z} for Frame 2, where i are unit vectors in the direction of the axes. The two frames are centered at the same location, but frame F 2 is rotated with respect to frame F 1 by a right handed rotation about the i 1 z-axis of θ, where i 1 z = i 2 z. Let w be a vector that lies entirely in the x y plane of both reference frames. The vector w is expressed 1

2 with respect to frame F i by a three dimensional vector w i x w i = w y i, wz i where the superscript i denotes that w is expressed with respect to F i, and where w i is the projection of w along the unit vector îi, i.e., w i = w i i. Referring to Figure 1, w can be expressed with respect to F 1 as and with respect to F 2 as w 1 x w 1 = w y 1, w 2 x w 2 = w y 2. To derive a relationship between w 1 and w 2 note that w = w 1 xi 1 x + w 1 yi 1 y = w 2 xi 2 x + w 2 yi 2 y. Taking the inner product of w with i 2 x gives w i 2 x = w 1 xi 1 x i 2 x + w 1 yi 1 y i 2 x = w 2 x where we have used the fact that i 2 x i 2 x = 1 and i 2 y i 2 x =. Similarly, taking the inner product of w with i 2 y we get w i 2 y = w 1 xi 1 x i 2 y + w 1 yi 1 y i 2 y = w 2 y. Expressing these relationships in matrix form, we get wx 2 i 1 w y 2 x i 2 x i 1 y i 2 x w = i 1 x i 2 y i 1 y i 2 x 1 y w 1 y. (1) 1 2

3 From Figure 1 it can be seen that i 1 x i 2 x = cos θ i 1 y i 2 x = sin θ i 1 x i 2 y = sin θ i 1 y i 2 y = cos θ. Therefore Equation (1) can be written as w 2 = R(θ)w 1 where the rotation matrix R(θ) is given by cos θ sin θ R(θ) = sin θ cos θ. 1 2 Kinematics for Three Wheel Mobile Robot The geometry of a three wheeled robot is shown in Figure 2. The vectors r b i = (r b xi, r b yi, ) denote the position of the center of the i th wheel expressed with respect to a reference frame fixed in the body of the robot. The unit vectors ŝ b i = (s b xi, s b yi, ) are unit vectors that point in the direction of spin of the i th wheel. The vector v b = (v b x, v b y, ) denotes the linear velocity of the center of the body fixed reference frame, expressed in F b, and the vector = (,, ) denotes the angular velocity vector of the robot. We will assume that each wheel has radius R, and that the angular speed of the i th wheel is given by Ω i. The objective of this section is to derive the relationship between the velocity v b and and the wheel speeds Ω i. Let v i be the linear velocity vector of the center of the i th wheel. Then v i is related the velocity of the center of the robot, and the angular velocity by the expression v i = v + r i, (2) where denotes the cross product. The linear speed of the i th wheel is the projection of v i along the rolling direction of the wheel. Therefore RΩ i = v i ŝ i. (3) 3

4 ŝ 1 v 1 y r 2 r 1 ŝ 2! x r 3 v ŝ 3 Figure 2: Geometry for a three wheeled mobile robot. If we express all vectors with respect to the body frame, then we have RΩ i = vi b ŝ b i b r = vy b xi b s b + ryi b xi s b yi = s b xi b s b xiriy b + s b yivy b + s b yirxi b = ( s b xi s b yi (s b yirxi b s b xiryi) ) v b b x vy b. Therefore, the forward kinematic relationship between the inertial speeds expressed in the body frame, and the angular speeds of the wheels as Ω 1 v b x Ω 2 = M v b y, Ω 3 4

5 where M = 1 s b x1 s b y1 (s b y1rx1 b s b x1ry1) b s b x2 s b y2 (s b R y2rx2 b s b x2ry2) b. s b x3 s b y3 (s b y3rx3 b s b x3ry3) b Similarly, the reverse kinematic relationship is given by b Ω 1 vy b = M 1 Ω 2. Ω 3 However, we typically would like to command velocities in the world frame rather than the inertial frame. Note that since i b z = i w z, and the angular speed is the projection of on i z, that we can express the relationship between world frame velocities and body frames velocities as b v vy b x w = R(θ) vy w, where θ is the heading angle of the robot, or in other words, the angle between i w x and i b x. Therefore, the forward kinematic relationship between speeds in the world frame and wheel speeds is given by Ω 1 w Ω 2 = MR(θ) v w y, Ω 3 and the inverse kinematic relationship is given by w Ω 1 vy w = R (θ)m 1 Ω 2, Ω 3 where we have used the fact that R 1 (θ) = R (θ). 3 Velocity Control for Three Wheel Mobile Robot The velocity control scheme for a three wheel mobile robot is shown in Figure 3, where the commanded speeds expressed in the world frame are given by wc, vy wc, and c. 5

6 c 1 PID Motor 1 wc vy wc! c 1 A MR( ) c 2 PID Motor 2 2 c 3 3 PID Motor 3 Figure 3: Velocity control scheme for a three wheeled mobile robot. 6