齐次坐标kinematics

Page 1 of 25 Introduction to Homogeneous Transformations & Robot Kinematics Jennifer Kay, Rowan University Computer Science Department January 2005 1. Drawing 3 Dimensional Frames in 2 Dimensions We will be working in 3-D coordinates, and will label the axes x, y, and z. Figure 1 contains a sample 3-D coor- dinate frame. Because we are representing 3-D coordinate frames with 2-D drawings, we have to agree on what these drawings mean. Clearly the y axis in Figure 1 points to the right, and the z axis points up, but we have to come up with a convention for what direction the x axis is pointing. Since the three axes must be perpendicular to each other, we know that the x axis either points into the paper, or out of the paper. Most people instantly assume one or the other is the case. To be able to view both cases, it helps to look at the axes overlaid on a cube. Consider the two views of the same cube in Figure 2. In view (a) we are looking at the cube from below, in view (b) we are looking at the cube from above. Let’s try and overlay the 3-D coordinate frame from Figure 1 onto these two views. Before you turn the page, make sure you can see both views of the cube in Figure 2! x y z Figure 1. A 3-D coordinate frame. Figure 2. Two views of the same cube. The cube is missing the front side, has a solid back and sides, and patterned top and bottom. In view a we are looking at the cube from below, in view (b) we are looking at the cube from above. (a) (b) Copyright (C) 2003, 2005, Jennifer Kay. Permission is granted to make individual copies of this for educational purposes as long as this copyright notice remains intact. Permission to make multiple copies of this for educational purposes will generally be granted when an email request is sent to kay@rowan.edu (so I know who is using it). In such cases, I will require that it is distributed for free (or for the actual cost of printing / duplication) and this copy- right notice remains intact. Page 2 of 25 Figure 3 and Figure 4 show the same two views of the cube, this time with the 3-D coordinate frame from Figure 1 overlaid onto the cube. Note that in Figure 3 the x axis points into the paper, away from you, and in Figure 4 the x axis is pointing out of the paper towards you! For the purposes of this document, we will assume that Figure 4 shows the interpretation we will use. In other words, if you see 3 axes drawn as they are in Figure 1, you should assume that the x axis points out of the paper towards you. If you actually wanted the x axis to be pointing into the paper, you should use the illustration shown in Figure 5. Figure 3. A cube viewed from below. The edge labelled x points into the page away from you. x y z Figure 4. The same cube viewed from above. The edge labelled x points out of the page towards you. y z x x y z Figure 5. Another 3-D coordinate frame. In this frame, y points to the right, z points up, and x points away from you into the paper. Page 3 of 25 2. Right Handed Coordinate Systems Most of the time we are going to use right handed coordinate systems. In a right handed coordinate system, if you know the directions of two out of the three axes, you can figure out the direction of the third. Let’s suppose that you know the directions of the x and y axes. For example, suppose that x points to the left, and y points out of the paper, as shown in Figure 6. We want to determine the direction of the z axis. To do so, take your right hand, and hold it so that your fingers point in the direction of the x axis in such a way that you can curl your fin- gers towards the y axis.When you do this, your thumb will point in the direction of the z axis. This process is illustrated in Figure 7. The chart in figure 7 details how to compute the direction of any axis given the directions of the other two. 3. Direction of Positive Rotation Sometimes we want to talk about rotating around one of the axes of a coordinate frame by some angle. Of course, if you are looking down an axis and want to spin it, you need to know whether you should spin it clockwise or counter-clockwise. We are going to use another right hand rule to determine the direction of positive rotation. x y Figure 6. x points to the left, and y points out of the paper towards you. x y Step 1: hold your right hand in such a way that your fingers point in the direction of the x axis and when you curl your fingers, they curl towards the y axis. x y Figure 7. Using the right hand rule to compute the direction of the z axis. Step 2: As you curl your fin- gers from the x axis towards the y axis, stick your thumb in the air. This will be the direction of the z axis x y The final 3-D coordinate system with the z axis shown. z Page 4 of 25 4. Plotting Points in 3 Dimensions All of us have experience in plotting points on 2-D axes. When it comes to plotting points on 3-D axes, things become a bit more difficult. In this section, we will discuss how to plot a number of points on the 3-D axes pre- sented in Figure 1. The first step is to draw tick marks on the axes to indicate scale. For the purposes of this document, we will assume that each tick represents one unit. Figure 10 shows several different right handed coordinate systems with tick marks added. Note that each tick mark is parallel to one of the other axes. This helps the viewer to visualize the 3-D effect. If you know the direction of these axes. Point the fingers of your right hand in the direction of this axis. Curl you right fingers towards the direction of this axis. Your thumb will point in the direction of this axis. x & y x y z y & z y z x x & z z x y Figure 8. Using the right hand rule to compute the direction of any axis given the directions of the other two. Figure 9. The right hand rule to determine the direction of positive angles. Point your right thumb along the positive direction of the axis you wish to rotate around. Curl your fingers. The direction that your fingers curl is the direction of positive rotation. x y z (a) The original axes with a right hand determining the direction of positive rotation around the z axis. x y z (b) After rotating the original axes (a) 90o around the z axis. (c) After rotating the original axes (a) -90o around the z axis. x z y Page 5 of 25 Figure 11 shows the point (2,3,4) plotted on the different axes of Figure 10. The technique is quite straightfor- ward if two of your axes form a plane parallel with the ground. First, draw lines to indicate the projection of the point on that plane. Then, draw a line through that point that is parallel to the remaining axis, add tick marks to it, and plot your point. y Figure 10. Several different right handed 3-D coordinate frames with tick marks to indicate scale. x z x y z x: out of page towards you y: right z: up x: right y: out of page towards you z: down y x z x: up y: right z: into page away from you y x z x y z y x 2 3 4 (a) (b) (c) 2 3 4 3 4 2 z Figure 11. The point (2,3,4) plotted on different axes. Page 6 of 25 For example, in Figure 11 (a) the x and y axes form the groundplane, and so we draw lines to indicate where (2,3,0) would be. Then, we draw a line through the point (2,3,0) that is parallel to the z axis, add tick marks to it, and finally plot our point. Although Figure 11 (b) looks different, the x and y axes still form the groundplane and so the procedure is virtually the same. The only difference is that the tick marks on the z axis have been left out because when they are included they are difficult to distinguish from the tick marks on the vertical line that con- nects to the point (2,3,4). In Figure 11 (c), the y and z axes form the groundplane. Thus, we first plot the point (0, 3, 4), then draw a line through the point (0,3,4) parallel to the x axis, add tick marks to it, and again plot our point (2,3,4). 5. Working With Multiple Coordinate Frames Sometimes we may want the coordinates of a point given with respect to two or more coordinate frames. For example, consider an airport scenario. The coordinate frame of the airport might have its origin at the base of the control tower, with the x axis pointing North, the y axis pointing West, and the z axis pointing up. While this is a useful coordinate frame for the air traffic controllers to use, a pilot may be more interested in where objects are relative to her airplane. Thus, we might have two coordinate frames, “tower coordinates” and “plane coordi- nates” as illustrated in Figure 12. We use subscripts to distinguish between xt, yt, and zt (the tower coordinate frame) and xp, yp, and zp (the plane coordinate frame). xt yt zt xp yp zp Figure 12. The plane is on the ground preparing for takeoff. Tower coordinates are centered at the base of the air traffic control tower. x points North, y points West, and z points up. The airplane coordinate system is centered at the nose of the plane. The x axis points towards the top of the plane, the y axis points out to the right as you are sitting in the captain’s chair, and the z axis points straight out the front of the plane. 50 meters 5 meters Page 7 of 25 When the plane is stopped on the runway as depicted in Figure 12, the nose of the plane might be at location (50, 5, 0) in tower coordinates, but it is at the origin (location (0, 0, 0)) in plane coordinates. Similarly, the base of the tower is at location (0, 0, 0) in tower coordinates, but at location (0, -5, 50) in plane coordinates. Note that the location of the nose of the plane is fixed with respect to the plane, but not with respect to the tower. When the plane begins to take-off as depicted in Figure 13, its nose is still at location (0,0,0) in plane coordi- nates, but it is at location (30, 15, 5) in tower coordinates. 6. Homogeneous Transformations 6.1 Representing Points Up to this point, we have been using the traditional (x,y,z) notation to represent points in 3-D. However, for the remainder of this document, we are going to use a vector notation to represent points. The point (x,y,z) is repre- sented as the vector . The 1 is a weighting factor. So the vectors and all represent that same point (x, y, z). Most of the time we will simply use a weighting factor of 1. Consider the more concrete example depicted in Figure 13. The plane is at location (30, 15, 10) in tower coordinates. We will normally represent that xt yt zt xp ypzp Figure 13. When the plane takes-off, its nose is still at location (0,0,0) in plane coordinates, but it is at a dif- ferent location in tower coordinates. To determine where it is in tower coordinates, the tower’s x and y axes have been extended, and the nose of the plane has been plotted in the manner of Figure 11. Thus we see that the nose of the plane is at location (30, 15, 10) in tower coordinates. 30 20 15 10 x y z 1 2x 2y 2z 2 257x 257y 257z 257 Page 8 of 25 location as , however it can also be represented as , , , and an infinite number of other vectors. In order to save space in this document, we will often write the point as (the transpose of ). 6.2 The Use of Multiple Coordinate Frames in Robotics It is very common in robotics to use two or more coordinate frames to solve a problem. Suppose the airplane in Figure 12 were automatically controlled. It would be very useful to keep track of some things in tower coordi- nates. For example, the altitude of the plane is simply the z coordinate of its location in tower coordinates. It also would be useful to keep track of other things in airplane coordinates. For example, the direction the plane should head to in order to avoid a mountain. Indeed, for many mobile robot applications, it is desirable to know the loca- tions of objects in both “world coordinates” and “robot coordinates.” Multiple coordinate frames are also useful in traditional robotics. For example, consider the simple robot arm depicted in Figure 14. If we want to have the gripper pick up a widget off of a table, then we need to figure out the widget’s location. Perhaps we have a camera that we use to initially determine the location of the widget (in camera coordinates). We might need to transform that location into world coordinates to evaluate if it is accessi- ble to the robot at all, and to gripper coordinates to determine when we should close the jaws of the gripper. 30 15 10 1 60 30 20 2 30– 15– 10– 1– 75 37.5 25 2.5 x y z 1 x y z 1 T x y z 1 zw xw yw z j y j z g x g y g x j Figure 14. A very simple robot arm with one joint and one gripper. The world, camera, joint, and gripper coordinate frames are indicated. joint gripper widget camera zc xc yc Page 9 of 25 6.3 Transforming Points Between Coordinate Frames Suppose that you know the location of a point in one coordinate frame (for example, airplane coordinates) and you want to know its location in another frame (for example, tower coordinates). How do you do it? We will start with a very simple case, and then move into more complex examples. Let’s begin by considering the two coordinate systems in Figure 15, world coordinates and robot coordinates. Notice that the only difference between the two coordinate frames is that the robot frame has been translated by 3 units along the y axis from the world coordinate frame. Figure 16 is a table of some sample points in world coordinates, and their corresponding values in robot coordi- nates. For the moment, ignore the third column of Figure 16, and just look at the first two columns. Notice that any point in world coordinates is the same as the point in robot coordinates. Simi- larly, any point in robot coordinates is the same as the point in world coordinates. We have seen that one way to convert world coordinates to robot coordinates for the system in Figure 15 is to subtract 3 from the y value in world coordinates. Surprisingly, another way to convert points from the world coordinate frame of Figure 15 to the robot coordinate frame of Figure 15 is to pre-multiply the point by the matrix . The third column of Figure 16 does exactly this and results in the same answer! a b c 1 T a b 3–( ) c 1 T d e f 1 T d e 3+( ) f 1 T xw yw zw xr yr zr Figure 15. Two coordinate frames, world (w) and robot (r). The origin of the robot frame is located at the point (0,3,0) in world coordinates. Using our new notation, we can repre- sent this as . The two x axes are parallel, as are the two y and the two z axes. Note that the origin of the world coordinate axes is at in robot coordinates. 0 3 0 1 T 0 3– 0 1 T 3 units 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 Page 10 of 25 Location of a Point in World Coordinates of Figure 15 Location of the Same Point in Robot Coordinates of Figure 15 Pre-multiplying the point in world coordinates by 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 0 0 0 1 0 3– 0 1 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 0 0 0 1 0 3– 0 1 = 0 3 0 1 0 0 0 1 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 0 3 0 1 0 0 0 1 = 5 10 15 1 5 7 15 1 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 5 10 15 1 5 7 15 1 = 84 84 84 1 84 81 84 1 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 84 84 84 1 84 81 84 1 = 4 4– 4 1 4 7– 4 1 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 4 4– 4 1 4 7– 4 1 = a b c 1 a b 3– c 1 1 0 0 0 0 1 0 3– 0 0 1 0 0 0 0 1 a b c 1 a b 3– c 1 = Figure 16. Converting between world and robot coordinates as depicted in Figure 15. Page 11 of 25 It should be clear that there is nothing magic about the number -3 in our 4x4 matrix other than the fact that we moved 3 units along the y axis between the two frames. Indeed, if we had moved 63 units, then pre-multiplying by the matrix would convert points from world coordinates to robot coordinates. Similarly, there’s nothing magic about the fact that we did this move along the y axis. We could come up with similar matrices for changes in frames that occurred along the y or z axis. Or indeed any combination of moves along all three matrices. Suppose that to get from the coordinate frame p to the coordinate frame q you need to move a units along p’s x axis, b units along p’s y axis, and c units along p’s z axis. Then to take a point from q coordinates to p coordi- nates, you need to premultiply it by the matrix For example, consider the two coordinate frames of Figure 17. To transform the robot coordinate frame into the world coordinate frame, you need to translate 5 units along the robot’s x axis, -4 units along the robot’s y axis, and -1 unit along the robot’s z axis. Thus to take a point in world coordinates and transform that into a point in robot coordinates, you need to premultiply that point by the matrix . Some examples of this can be found in Figure 18 . 1 0 0 0 0 1 0 63– 0 0 1 0 0 0 0 1 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 xm ym zm xc yc zc Figure 17. Two coordinate frames that differ by only a translation. To get from car coordi- nates (c) to mountain coordinates (m) you must translate 5 units along the car’s x axis, -4 units along the car’s y axis, and -1 unit along the car’s z axis. 5 u nit s 4 units 1 unit Page 12 of 25 Figure 18. Converting points from the mountain coordinate frame of Figure 17 to the car coordinate frame of Figure 17. Location of a point in Figure 17’s mountain coordinates Location of the same point in Figure 17’s car coordinates Pre-multiplying the point in mountain coordinates by 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 0 0 0 1 5 4– 1– 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 0 0 0 1 5 4– 1– 1 = 0 3 0 1 5 1– 1– 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 0 3 0 1 5 1– 1– 1 = 5 10 15 1 10 6 14 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 5 10 15 1 10 6 14 1 = 84 84 84 1 89 80 83 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 84 84 84 1 89 80 83 1 = 4 4– 4 1 9 8– 3 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 4 4– 4 1 9 8– 3 1 = a b c 1 a 5+ b 4– c 1– 1 1 0 0 5 0 1 0 4– 0 0 1 1– 0 0 0 1 a b c 1 a 5+ b 4– c 1– 1 = Page 13 of 25 We call the matrix that converts a point from j coordinates to k coordinates the homogeneous transformation from j coordinates to k coordinates, and we abbreviate this as . Figure 19 summarizes how to compute the matrix that converts between two frames that only differ by a translation. 6.4 Review: Determining the Homogeneous Transformation when Frames Differ only by Translation It is very important to note that we have been considering two distinct concepts in our previous discussion: 1. How do we take a point that is in frame a-coordinates and convert it to frame b-coordinates? (In other words, what is 2. How do we compute the transformation between frame a and frame b (i.e. how would we move frame a to line it up with frame b)? (We don’t have an abbreviation for this yet). The two concepts are closely related, but not the same. If we want to know how to take a point in frame a coordi- nates and convert it to frame b coordinates, the easiest way to do that is to first compute how to move frame b so that it lines up with frame a. T k j If the coordinate frames j and k only differ by a translation and to get from k coordinates to j coordinates you translate (a, b, c) along k’s x, y, and z axes, then , the matrix that takes a point in j coordinates to a point in k coordinates is We can summarize this with the equation Trans (a,b,c) = T k j 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 Figure 19. Converting points between coordinate frames that only differ by a translation. T b a Page 14 of 25 Now go look at Figure 19 again! Make sure you understand the TWO concepts before you continue on. In order to remind ourselves that there are TWO concepts, we will use two different sets of notation to represent