Institute of Geometry, Graz, University of Technology Mobile Robots. Lecture notes of the kinematic part of the lecture

Transcription

1 Institute of Geometry, Graz, University of Technology Institute of Geometry Mobile Robots Lecture notes of the kinematic part of the lecture Anton Gfrerrer nd Edition 4

2 .

3 Contents Introduction to planar kinematics. The Euclidean plane and its transformations Proper orthogonal ( )-matrices Converting position vectors and direction vectors from one frame to another Homogeneous representation of a planar displacement Motions in the plane Velocities of points in the planar case Instantaneous motion in the planar case The centrodes of a planar motion Examples The rolling wheel The Frenet motion along a planar curve Motion of a mobile robot with two coaxial individually drivable non-steerable wheels Introduction to space kinematics 5. Rotations in 3-space The rotation group SO(3) and its standard parameterizations Rotation parameters Euler parameters Euler angles Cardan angles Quaternions Using quaternions for representing rotations in 3-space General displacements in the Euclidean 3-space Determination of a spatial displacement Homogeneous representation of a spatial displacement Motions in space Velocitiy behavior in the spatial case Composition of motions; kinematic chains 3 3. Example: Motion of a mobile robot with two coaxial individually drivable non-steerable wheels (ctd.) The velocity operator of a motion The velocity behavior of composed motions Example: A planar serial manipulator with two revolute joints Example: A vehicle Generalization to kinematic chains with an arbitrary number of systems Kinematic chains with solely revolute joints

4 4 Wheeled mobile robots Wheel architectures The fixed standard wheel The steerable standard wheel The castor wheel Mecanum wheels Ball wheels Velocity representation in the moving system Kinematics of standard and castor wheels The inverse kinematics of the fixed standard wheel The inverse kinematics of the steerable standard wheel The inverse kinematics of the castor wheel Kinematics of the Mecanum wheel A vehicle with three Mecanum wheels

5 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, Introduction to planar kinematics. The Euclidean plane and its transformations We start with two given right-handed Cartesian frames F : {O, e,x, e,y } and F : {O, e,x, e,y } in the plane (see Fig. ). Such a pair defines a Euclidean displacement in the plane. Let O X : x e,x + y e,y, O X : x e,x + y e,y be the position vectors of a point X w.r.t. F and F. This means that x x x :, x y : y are the coordinate columns of X w.r.t. F and F. x X y x x e,y x y e,x e,y d O ϕ O e,x Figure : Transformation in the plane What is the relation between the two vectors O X and O X? Let ϕ : (e,x, e,x ) denote the angle between the x-axis of F and the one of F. (By the way, ϕ is also the angle between the y-axes of the two frames.) Then we have: e,x cos ϕ e,x + sin ϕ e,y e,y sin ϕ e,x + cos ϕ e,y } () If O O d x e,x + d y e,y () denotes the vector from O to O then O X O O + O X d x e,x + d y e,y + x (cos ϕ e,x + sin ϕ e,y ) + y ( sin ϕ e,x + cos ϕ e,y ) (d x + cos ϕ x sin ϕ y ) e,x + (d y + sin ϕ x + cos ϕ y ) e,y As a convention we always measure angles in radians. Moreover an angle is oriented (has a sign): A positive (negative) angle represents a counterclockwise (clockwise) rotation.

6 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots which means that x d x + cos ϕ x sin ϕ y, y d y + sin ϕ x + cos ϕ y. } (3) In the notation of coordinate columns this reads as x dx + y d y cos ϕ sin ϕ sin ϕ cos ϕ x y. (4) If we put A : cos ϕ sin ϕ sin ϕ cos ϕ (5) and denote the coordinate column of O (w.r.t. F ) by d : dx d y (6) then we can write (4) in the form: x d + A x (7) The vector d is called the translational part of the displacement (7) whereas the matrix A is called its rotation matrix. If we compose two planar Euclidean displacements of the kind (7) we obtain another displacement of that type. Additionally, the inverse of a planar Euclidean displacement is again a transformation of that kind. Summarizing, we can say that the set of all planar displacements establishes a (non-commutative) group, w.r.t. the compositionoperation: Definition.. The group of all planar Euclidean displacements (7) is called special Euclidean group SE(). Note, that the group SE() depends on three parameters, namely d x, d y and ϕ. We say: SE() is a threeparametric group. Often, this fact is also formulated in this way: In the plane there are three degrees of freedom (3 dof)... Proper orthogonal ( )-matrices The rotation matrix A (see eq. (5)) has the following properties: A A I det A (9) (8) A ( )-matrix A for which (8) holds is called an orthogonal matrix. Note that the inverse of an orthogonal matrix is its transpose: If additionally (9) holds for A then we say that A is a proper orthogonal matrix. A A ()

7 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 3.. Converting position vectors and direction vectors from one frame to another a) Converting position vectors. Let X be a point and x, x its coordinate columns w.r.t. F, F, respectively. Then, according to (7), () we have x d + A x, x A (x d). b) Converting direction vectors. Let now X and Y be two points whose coordinates columns w.r.t. coordinate column of the vector { } { F y x XY w.r.t. is F y x { } { F x, y are F x, y }. For the conversion we get y x d + A y (d + A x ) A (y x ) and y x A (y x ). }. Then the From that we can see that the translational part d of the transformation does not play any role in the conversion of direction vectors. This is for instance the case if we deal with velocity vectors...3 Homogeneous representation of a planar displacement Often eq. (4) is written in the more compact homogeneous notation : x cos ϕ sin ϕ d x y sin ϕ cos ϕ d y x y () If we put X : whith o : x x y, X : x we can rewrite () as x y, B : A d o cos ϕ sin ϕ d x sin ϕ cos ϕ d y X B X. () In particular, this notation is advantageous if we compose transformations: In that case we only have to multiply the respective matrices B. Remark.. In some sources the of the homogeneous notation () is written as first entry, i.e., x d x cos ϕ sin ϕ x. y d y sin ϕ cos ϕ y. Motions in the plane A one-parameter planar motion can be understood as a continuous series of planar displacements (eq. (4)). In this case we imagine that the plane is represented by two copies the fixed system (or fixed plane ) Σ and the moving system (or moving plane ) Σ (see Fig. ). The one-parameter motion itself is then denoted by Σ \Σ. Often the time t is used as the motion parameter. Aiming at a mathematical description of the motion Σ \Σ we choose a coordinate frame F : {O, e,x, e,y } in the fixed plane Σ and another one F : {O, e,x, e,y } in the moving plane Σ.

8 4 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots Σ v X, X c X x (t) e,y d(t) O e,y ϕ(t) x e,x Σ O e,x Figure : Planar motion The motion Σ \Σ is then represented by the equation x d(t) + A(t) x (3) Note, that here as opposed to (7) the translational part d and the rotation matrix A depend on the motion parameter t (the time ), i.e., d d(t) dx (t) d y (t) cos ϕ(t) sin ϕ(t), A A(t) sin ϕ(t) cos ϕ(t) Conversely, if we choose three arbitrary functions d x d x (t), d y d y (t) and ϕ ϕ(t) a particular motion Σ \Σ is defined via (3). Let now X be a point fixed in the moving plane Σ ; then its coordinate column x w.r.t. F is a constant vector. Contrarily, the coordinate column x of X varies with the motion parameter t (see eq. (3)): x x (t). Throughout the motion Σ \Σ the point X describes a certain path curve c X lying in the fixed plane Σ. A parametric representation x x (t) of this curve c X is already available by equation (3): x (t) dx (t) + cos ϕ(t) x x (t) sin ϕ(t) y (4) y (t) d y (t) + sin ϕ(t) x + cos ϕ(t) y... Velocities of points in the planar case To obtain the (coordinate column of the) velocity vector v X, of the point X w.r.t. the motion Σ \Σ we have to differentiate (3): v X, ẋ ḋ + Ȧ x (5) (For better readability we suppress the notation of the parameter t in the above and the following equations.) From (3) we get x A (x d) Derivatives with respect to t are denoted by dots: f df dt, f d f (dt),...

9 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 5 which by substitution into (5) yields v X, ḋ + Ȧ A (x d) ḋ + W (x d). (6) The occurring matrix W : Ȧ A, the so-called angular velocity matrix, is skew symmetric: 3 sin ϕ cos ϕ W Ȧ A cos ϕ sin ϕ ϕ ϕ cos ϕ sin ϕ sin ϕ cos ϕ ϕ Here, ω ω (7) denotes the instantaneous angular velocity of the motion Σ \Σ. Due to (6), (7) we have v X, ω : ϕ (8) d x ω(y d y ) d y + ω(x d x ). (9) Particularly, if X is chosen as the origin O of the moving system s coordinate frame F, i.e., x d x, y d y, we obtain d v O, x d. () y dx (t) This is obvious, as d(t) is a parameterization of the path c d y (t) O of O... Instantaneous motion in the planar case At a time instant t t each point X(x, y ) has a velocity vector v X, represented by (9). What can we say about the distribution of those vectors? Is there any point X P, whose velocity is zero at t t? To answer these questions, we have to distinguish the two cases, ω (case ) and ω (case ). Case : ω ϕ. In this case, there is exactly one point P X whose velocity vector v X, is equal to zero. This is due to the fact that the equation d v X, x ω(y d y ) d y + ω(x d x ) has a unique solution in the variables x, y, namely p,x d x d y ω, p,y d y + d x ω. Definition.3. The point P with the coordinate column p p,x p,y d d x y ω d d y + x ω is called instantaneous center of revolution (ICR) at the time instant t t. () 3 A square matrix W w ij i,j,...,n is called skew symmetric if W W. This means that w i,j w j,i for i, j,..., n. This also implies that the main diagonal entries of a skew symmetric matrix are all zero.

10 6 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots If X... x x y is an arbitrary point the vector P X has the representation x p x d x + d y ω y d y d x ω w.r.t. the frame F. Multiplication with ω yields ω (x p ) ω d y + ω(x d x ) d x + ω(y d y ) d y + ω(x d x ) d x + ω(y d y ) () Comparing this with (9) shows that we obtain the velocity vector v X, of the point X by a positive 9 - rotation of the vector ω (x p ) (Fig. 3(a)). Z v X, ω (x p ) v Z, v Y, X v X, Y x p X P (a) Velocity vector v X, of a point X P (b) Velocity distribution Figure 3: Velocities in case : ω By computing the norm of both sides of () we see that Summarizing we have the following v X, ω P X. (3) Result.4. Instantaneous velocity behavior of a planar motion with non-zero instantaneous angular velocity. Let Σ \Σ be a planar motion (eq. (3)) with a non-vanishing angular velocity ω at the time instant t t, i.e., ω. Then we have: a) There is exactly one point P in Σ whose velocity vector is zero at this moment, the so-called instantaneous center of revolution (ICR). b) The velocity vectors v X, of the points X Σ behave just like in case of a pure rotation with angular velocity ω ϕ around P (Fig. 3(b)). This motivates the following Definition.5. In Case (ω ) we say that the instantaneous motion of the motion Σ \Σ at the time instant t t is an instantaneous rotation. Remark {.6. From (9) } and { () } we can see that the instantaneous rotation is positive (counterclockwise) ω > if. negative (clockwise) ω <

11 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 7 Remark.7. The coordinate column of the ICR w.r.t. the moving frame is p,x p A cos ϕ sin ϕ p,x d (p p d) x,y sin ϕ cos ϕ p,y d y d x d y cos ϕ sin ϕ ω d x sin ϕ cos ϕ d d y +. x ω d y Hence, we have Case : ω ϕ. p p,x p,y cos ϕ ω d y + sin ϕ sin ϕ d y + cos ϕ d x d x. (4) According to (9) the velocity vector v X, of an arbitrary point X is d v X, x d ḋ (5) y Hence, we have the Result.8. Instantaneous velocity behavior of a planar motion with zero instantaneous angular velocity. Let Σ \Σ be a planar motion (eq. (3)) with a zero angular velocity ω at the time instant t t, i.e., ω. Then, the instantaneous velocity vector of a point X does not depend on X: At the respective time instant t t each point X has the same instantaneous velocity vector v X, ḋ. At the time instant t t the motion Σ \Σ behaves like a pure translation with velocity ḋ. Definition.9. In Case (ω ) we say that the instantaneous motion of the motion Σ \Σ at the time instant t t is an instantaneous translation. If additionally ḋ o holds then each point has zero velocity and we speak of an instantaneous standstill...3 The centrodes of a planar motion Let us now study the general case (Case : ω ϕ ) in a global context: We assume that ω for all t t, t. Then we have an ICR for each time instant t t, t. The locus of the ICRs in the fixed system Σ is a certain curve p whose parameterization is given by (). Analogously, the locus of the ICRs in the moving system Σ is another curve p with parameterization (4). Definition.. The curves p and p are called fixed centrode and the moving centrode of the motion Σ \Σ. The following fundamental result can be shown: Result.. Fundamental Theorem of planar kinematics. (Fig. 4) Within a planar motion Σ \Σ with non-vanishing angular velocity ω the moving centrode p rolls without gliding on the fixed centrode p. At any time-instant the contact point of the two centrodes p and p is the respective instantaneous center P of revolution (ICR). A proof of this fundamental theorem can be found in or in 4, pp. 6.

12 8 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots Σ Σ p p P Figure 4: Centrodes p, p of a planar motion Σ \Σ.3 Examples.3. The rolling wheel Let a circle c with radius r be rolling on a straight line g (Fig. 5). The fixed system Σ is represented by g whereas the moving system Σ is represented by c. Two positions of the wheel c are depicted in Fig. 5. After choosing the coordinate frames F {O, e,x, e,x } and F {O, e,x, e,x } in Σ and Σ as shown in Fig. 5 we obtain the following representation of the motion Σ \Σ : x y rϕ r + cos ϕ sin ϕ sin ϕ cos ϕ x y. (6) For this particular motion Σ \Σ the fixed centrode is the line g p whereas the circle c p is the moving centrode. Their contact point P is the ICR. If again ω ϕ denotes the angular velocity of the motion we easily check that the velocity vector v O, of the wheel center O is rω v O, which gives us the scalar velocity of O as Hence, we have: v O, r ω. ω v O, r Due to the results in Section.., p. 5, (Case ) we see that for the velocity vector v H, of the wheel top point H we have: rω v H, v O, (7)

13 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 9 v H, v O, H c e,x e,y v O, O rϕ O e,x e,y ϕ e,y P rϕ O e,x g Figure 5: A wheel c rolling along a straight line g Remark.. Let us consider the rolling of a circle c on an arbitrary planar curve k (Fig. 6). Note that in this case Σ \Σ is not a planar motion anymore because Σ (the supporting plane of k) and the wheel plane Σ are not identical! We can choose the (spatial!) coordinate systems F {O, e,x, e,y, e,z } and F {O, e,x, e,y, e,z } of Σ and Σ as shown in Fig. 6. Let the curve k be parameterized by d(t) d x (t) d y (t). Because the path c O of the wheel center O is congruent to k by means of a translation into z-direction with magnitude r ( wheel radius), this path c O has the parameterization d x (t) d (t) d y (t) r. Hence, the velocity vector v O, of O is v O, d x d y and has the magnitude v O, d x + d y. Because the rolling of c along k is up to differential order the same as the rolling of c along the tangent g of k equation (7) is still valid in this more general case: ω v O, r d x + d y. (8) r

14 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots c e,z e,x e,z d e,x O e,y Σ v O, O r e,y d g c O P k Figure 6: A wheel c rolling along a curve k.3. The Frenet motion along a planar curve Recall the notions curvature, radius of curvature and osculating circle of a planar curve: 4 If k is a planar curve given by the parameterization d(t) dx (t) d y (t) then the curvature κ of k is given by d κ : x d y d x d y ( d x + d y ) 3 (9) The sign of κ has the following meaning: A positive (negative) value of κ means that the curve makes a left (right) turn in the sense of its parameterization. In case of κ we have an inflection point on k. In non-inflection points the (oriented!) radius ρ of curvature is defined as the reciprocal value of κ: ρ : κ ( d x + d y ) 3 (3) d x d y d x d y The osculating circle c P of a point P on k has the radius ρ and its center P on the curve normal n (Fig. 7). P is the point with the coordinate vector d dx ρ : +. (3) d y d x + d y d Note that d y x + d y d represents the unit direction vector of the curve normal n, obtained by a positive x quarter-turn of the unit tangent vector of k! The curve k which is the locus of the centers of curvature is called the evolute of the curve k. It is well-known that the normals n of k are the tangents of the evolute k. A parameterization of the evolute k is given by eq. (3). We now consider the so-called Frenet motion Σ \Σ along k which is defined as follows (Fig. 7): The fixed system Σ is represented by the curve k whereas the moving system Σ is represented by the lines g and n. The path of the point O : P g n throughout the motion Σ \Σ is the given curve k. The line g (line n) is always tangent (perpendicular) to k. 4 See, for instance, 3, pp. 77. d y d x

15 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, We choose F : {O P, e,x, e,y } as coordinate frame in Σ with e,x and e,y being unit direction vectors of the curve tangent g and the curve normal n: e,x : e,y : F is called the Frenet 5 frame of the curve k. d x + d y d x + d y ( ) d x e,x + d y e,y ( ) d y e,x + d x e,y Then the Frenet motion Σ \Σ is described by x y dx d y + cos ϕ sin ϕ sin ϕ cos ϕ x y with cos ϕ d x d, sin ϕ y. (3) d x + d y d x + d y What is the fixed centrode p and what is the moving centrode p of the Frenet motion? From (3) we obtain c P k g k e,x ρ e,y P O n d P e,y ϕ O e,x Figure 7: Frenet motion along a curve k d tan ϕ y, d x and by differentiation 5 Jean Frédéric Frenet, French mathematician (86 9) ϕ cos ϕ d x d y d x d x d y.

16 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots Hence, after substitution of (3) we get d ω ϕ x d y d x d y. (33) d x + d y Substitution of eq. (33) into eq. (), p. 5 yields the parameterization of the fixed centrode p, dx p : + d y ω d y dx ρ d d + y x d y d x + d y d x which shows that p is identical with the evolute k of k! (Compare with the parameterization 3, p. ) of the evolute!) Analogously, if we substitute eq. (33) into eq. (4), p. 7 we obtain the parameterization of the moving centrode p : p : cos ϕ ω d y + sin ϕ d x sin ϕ d y + cos ϕ ρ Hence, the moving centrode is a straight line, namely the curve normal n! Result.3. The fixed centrode of the Frenet motion Σ \Σ along a curve k is the evolute k of k. The moving centrode of Σ \Σ is the normal n of k. d x

17 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, Motion of a mobile robot with two coaxial individually drivable non-steerable wheels Fig. 8 shows a mobile robot with two non-steerable standard wheels W L (left wheel), W R (right wheel), both with radius r mounted on a common axis. Each of the wheels is driven by an individual motor. An additional ball wheel W S is attached to the rear of the chassis. W S is passive and its only purpose is to support the chassis. We will neglect any influence of friction in the following analysis. The robot chassis represents the moving system Σ whereas the planar ground represents the fixed system Σ. For an analytical description of the robot motion Σ \Σ we supply the moving system Σ with a coordinate frame F {O, e,x, e,y } whose origin O is the midpoint of the wheel contact points L and R and whose x- and y-axes are parallel to the chassis centerline g and the wheel axis n, respectively (Fig. 8). v R, ρ k v O, r v L, e,x R k n L e,y P O g S d P e,y b O e,x b Figure 8: A mobile robot with two coaxial individually drivable non-steerable wheels follows a prescribed curve k. It is evident that the robot motion Σ \Σ is the Frenet motion along the path k of O. Hence, a given path k of O with the parameterization d(t) dx (t) d y (t), defines this robot motion Σ \Σ via 6 x y dx d y + cos ϕ sin ϕ sin ϕ cos ϕ x y with cos ϕ d x d, sin ϕ y. d x + d y d x + d y 6 Compare with the previous Section.3.!

18 4 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots How do we have to choose the angular velocity functions ω L (t) and ω R (t) of the left and the right wheel so that the robot will indeed follow the prescribed path k? Let again v O, d x d y denote the velocity vector of O. Its magnitude is v O, d x + d y. The ICR of the Frenet motion Σ \Σ is the center of curvature P of k lying on the curve normal n (see Fig. 8). Thus, the velocity vectors v L, and v R, of the wheel contact points L and R are linearly dependent of v O,, i.e., 7 v L, v L Note that v L v L,, v R v R,! v O, v, v v O, R, v R O, v. O, As revealed by Fig. 8 the yet unknown values v L and v R have to satisfy the two linear equations v R + v L v O,, v R v L b v O,. ρ (34) where b : dist(l, R) is the wheel center distance and ρ denotes the oriented radius of curvature (eq. (3), p. ). Solving (eq. 34) for v L, v R yields v L v O, ( b ρ ) v O, ( b κ), (35) v R v O, ( + b ρ ) v O, ( + b κ). Keeping in mind eq. (8), p. 9, we obtain the desired wheel velocities: ω L v L r ω R v (36) R r Of course, v L und v R have to be substituted according to (35). Note that the signs of ω L and ω R are always correct, if the wheel axis n is oriented in the direction of e,y (to the left side of the vehicle)! 7 Note that the paths c L and c R of L and R are offset curves of the path k of O at distance b : The three curves k, c L and c R have the same set of normals.

19 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 5 Introduction to space kinematics. Rotations in 3-space Let two right-handed spatial Cartesian frames with a common origin O be (Fig. 9): F : {O, e,x, e,y, e,z }; F : {O, e,x, e,y, e,z } If X is an arbitrary point it has a representation w.r.t. each of the two coordinate frames, i.e., OX x e,x + y e,y + z e,z, (37) OX x e,x + y e,y + z e,z. (38) This means that the coordinate column of X w.r.t. F and F are x : What is the relation between these two vectors x and x? x y z, x : x y z. e,z e,z e,y O e,x e,x e,y Figure 9: Transformation of a right-handed frame F : {O, e,x, e,y, e,z } into another one F : {O, e,x, e,y, e,z }. The frames have the same origin O. Let then by substitution into (38) we obtain which means that e,x a e,x + a e,y + a 3 e,z e,y a e,x + a e,y + a 3 e,z e,z a 3 e,x + a 3 e,y + a 33 e,z OX (a x + a y + a 3 z ) e,x + (a x + a y + a 3 z ) e,y + (a 3 x + a 3 y + a 33 z ) e,z, ; (39) x A x (4) with A : a a a 3 a a a 3 a 3 a 3 a 33. (4) What are the properties of the (3 3)-matrix A?

20 6 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots Because e,x is a unit vector we know that e,x. Analogously, we have e,y e,z. This means that the column vectors of the matrix A are normalized: e,x a + a + a 3 e,y a + a + a 3 (4) e,z a3 + a 3 + a 33 Each pair of the vectors e,x, e,y, e,z is orthogonal and so are the column pairs of A: e,x, e,y a a + a a + a 3 a 3 e,x, e,z a a 3 + a a 3 + a 3 a 33 e,y, e,z a a 3 + a a 3 + a 3 a 33 (43) Because e,x, e,y, e,z is an orthonormal right-handed vector basis we additionally have det A. (44) In a more compact form the conditions (4) and (43) can be notated as A A I 3 : (45) Definition.. An (n n)-matrix A which satisfies A A I n where I n is the (n n)-identity matrix is called an orthogonal matrix. Note that the inverse of an orthogonal matrix is its transpose: Moreover, the determinant of an orthogonal matrix is either 8 + or. A A (46) Definition.. If det A + holds for an orthogonal matrix A, then A is called a proper orthogonal matrix or rotation matrix. Summarizing we have the following Result.3. Spatial Rotations. A transformation of the Euclidean 3-space which leaves the origin O fixed and moves a right-handed Cartesian frame F : {O, e,x, e,y, e,z } to another right-handed Cartesian frame F : {O, e,x, e,y, e,z } is described by a proper orthogonal (3 3)-matrix That means A A A A A I 3 a a a 3 a a a 3 a 3 a 3 a 33., det A. Remark.4. The following two items reveal the nature of the transformations discussed above: 9. If ρ is a transformation according to (4), where A is a proper orthogonal (3 3)-matrix then ρ is a rotation about an (oriented) axis a through O and by some rotation angle ϕ, π. We denote this rotation by ρ(a, ϕ).. Conversely, if ρ ρ(a, ϕ) is a rotation about an axis a through O by some rotation angle ϕ, π, then ρ can always be described by an equation of the type (4) with a proper orthogonal (3 3)-matrix A. 8 This follows from det I n det(a A) (det A ) (det A) (det A) (det A) (det A). 9 For a proof see for instance.

21 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 7. The rotation group SO(3) and its standard parameterizations If we compose two rotations ρ (a, ϕ ) and ρ (a, ϕ ) with axes a and a through O we get again a rotation ρ(a, ϕ) with an axis a through O. This is obvious because the multiplication of two proper orthogonal (3 3)-matrices yields again a matrix of that type. Moreover, the inverse of the rotation ρ ρ(a, ϕ) is the rotation ρ ρ(a, ϕ) and the identity transformation is a rotation about an arbitrary axis a and the angle ϕ. Summarizing, we can say that the set of all rotations with axis through O builds up a (non-commutative) group, w.r.t. the composition-operation: Definition.5. The group of spatial rotations whose axes contain a fixed point O is called special orthogonal group SO(3). A rotation ρ with an oriented axis a through O is determined if we choose the rotation angle ϕ and the normalized direction vector of the rotation axis a. Note that due to the normalization the direction vector of the axis depends on two parameters only. So together with the rotation angle ϕ we have 3 parameters: Result.6. The special orthogonal group SO(3) is a 3-parametric group. As we already know, such a rotation can be described by an equation of the form x A x with A a a a 3 a a a 3 a 3 a 3 a 33 ; A A I 3 ; det A. (47).. Rotation parameters (For a derivation of the Results.7 and. see.) Result.7. Let ϕ be the rotation angle and let a : a x a y a z be the coordinate column of the normalized direction vector of the rotation axis a. Then the corresponding rotation matrix is a x( cos ϕ) + cos ϕ a x a y ( cos ϕ) a z sin ϕ a x a z ( cos ϕ) + a y sin ϕ A a x a y ( cos ϕ) + a z sin ϕ ay ( cos ϕ) + cos ϕ a y a z ( cos ϕ) a x sin ϕ. (48) a x a z ( cos ϕ) a y sin ϕ a y a z ( cos ϕ) + a x sin ϕ az( cos ϕ) + cos ϕ Definition.8. The parameters a x, a y, a z and ϕ are called rotation parameters. Example.9. If a,,, i.e., a x, a y a z, then we have a rotation about the x-axis. In this case A R x (ϕ) : cos ϕ sin ϕ. (49) sin ϕ cos ϕ For a rotation about the y-axis or the z-axis (a,, and a,,, respectively) we obtain cos ϕ sin ϕ A R y (ϕ) : (5) sin ϕ cos ϕ Because ρ(a, ϕ) fixes the axis a the coordinate columns of the normalized direction vector of the axis a w.r.t. the two frames F and F are identical.

22 8 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots and A R z (ϕ) : cos ϕ sin ϕ sin ϕ cos ϕ. (5) The following recipe allows us to compute the rotation parameters from a given rotation matrix A: Result.. If a proper orthogonal (3 3)-matrix A I 3 (eq. (47)) is given one can determine the rotation parameters a x, a y, a z, ϕ as follows:. Determine ϕ (, π from where is the so-called trace of the given matrix A. cos ϕ ( tr A ) (5) tr A a + a + a 33. For the computation of the direction vector a a x, a y, a z one has to distinguish the two cases ϕ π and ϕ π: If ϕ π then a a x a y a z sin ϕ a 3 a 3 a 3 a 3 a a If ϕ π then at least one of the expressions +a a a 33, a +a a 33 and a a +a 33 is non-zero and a can be computed either by a x + a a a y a a 33 a + a + a a a a 33 z a 3 + a 3 or by a a x a y a z a + a a 33 a + a a + a a 33 a 3 + a 3 or by a a x a y a z a a + a 33 a 3 + a 3 a 3 + a 3 a a + a Euler parameters A rational parametrization of the group SO(3) by means of four parameters q, q, q, q 3 ( Euler parameters ) is the following: A q + q + q + q 3 q + q q q 3 (q q q q 3 ) (q q 3 + q q ) (q q + q q 3 ) q q + q q 3 (q q 3 q q ) (q q 3 q q ) (q q 3 + q q ) q q q + q 3 (53)

23 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 9 The Euler parameters are homogeneous. Due to this fact we can normalize them via q + q + q + q 3. The following nice relation between normalized Euler parameters q, q, q, q 3 and rotation parameters a x, a y, a z, ϕ is worth mentioning: q cos ϕ q q sin ϕ a x sin ϕ a (54) y q 3 sin ϕ a z Eq. (53) tells us how to compute the rotation matrix A belonging to a quadruple q, q, q, q 3 of Euler parameters. Conversely, the Euler parameters of a given rotation matrix A (eq. (47)) can be extracted from the entries of A according to q : q : q : q 3 + a + a + a 33 : a 3 a 3 : a 3 a 3 : a a a 3 a 3 : + a a a 33 : a + a : a 3 + a 3 a 3 a 3 : a + a : a + a a 33 : a 3 + a 3 a a : a 3 + a 3 : a 3 + a 3 : a a + a 33. (55) It should be mentioned that each of the equations (55) can be important as there are cases where some of the ratios on the right-hand side are of the form : : :. For instance, if a, a, a 33 and a ij for all other entries of A this specifies a half-turn (rotation angle π) about the z-axis, then all but the last of the four given ratios on the right-hand side of (55) are equal to : : :. So in this example only the last one is well defined...3 Euler angles An arbitrary rotation ρ SO(3) can be decomposed into a rotation about the z-axis, followed by a rotation about the x-axis and again followed by a rotation about the z-axis (Fig. ). Hence, the corresponding rotation matrix A is the product of the three matrices R z (θ), R x (χ), R z (ψ) (compare with (49), (5)): A R z (ψ) R x (χ) R z (θ) cos ψ sin ψ sin ψ cos ψ cos χ sin χ sin χ cos χ cos θ sin θ sin θ cos θ cos ψ cos θ sin ψ cos χ sin θ cos ψ sin θ sin θ cos χ cos ψ sin ψ sin χ sin ψ cos θ + cos ψ cos χ sin θ sin ψ sin θ + cos ψ cos χ cos θ cos ψ sin χ sin χ sin θ sin χ cos θ cos χ (56) Note that the order of transformations is important! Applying this transformation to a vector x means that one has to proceed from right to left: First the multiplication of R z (θ) with x has to be applied, followed by the multiplication of R x (χ) with the outcome of the first operation and finally followed by the multiplication of R z (ψ) with the outcome of the second operation! Definition.. The angles θ, χ, ψ are called the Euler angles of the given rotation ρ. How can we determine the Euler angles θ, χ, ψ to a given rotation matrix A (eq. (47))? We have to distinguish two cases Case : a 3 + a 3 sin χ. This means that χ, π. This means that the parameters are only determined up to a common non-zero factor. This is also the reason why in this case we need four parameters to parameterize a three-parametric set.

24 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots e,z e,z e,x e,z e,z e,x e,x e,y e,y e,x e,y e,y e,z e,z e,z e,x e,z e,x e,x e,y e,y e,x e,y e,y Figure : Euler angles θ, χ, ψ. Home position: θ χ ψ (top, left). A rotation about z by angle θ (top, right), followed by a rotation about x by angle χ (bottom, left) and by a rotation about z by angle ψ (bottom, right).

25 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, From the third column of A we get sin ψ a 3 a, cos ψ 3, (57) a 3 + a3 a 3 + a3 and from this we can uniquely determine the angle ψ in, π). Similarly, we get sin θ a 3 a, cos ψ 3, (58) a 3 + a3 a 3 + a3 from the third row of A, which gives us the angle θ uniquely in, π). We know that cos χ a 33 and sin χ a3 + a 3 from which we obtain a unique value of χ in the interval (, π). Case : a 3 + a 3 sin χ. This means that χ (in this case a 33 ) or χ π (then a 33 ). Here we obtain a one-parametric set of solutions for either of the other two Euler angles θ and ψ. We can, for example, choose θ arbitrarily; then we obtain two linear equations for the yet unknown values cos ψ and sin ψ from the entries a and a of A: } cos θ cos ψ sin θ sin ψ a (59) ± sin θ cos ψ + cos θ sin ψ a from which we get cos ψ cos θ a ± sin θ a sin ψ cos θ a sin θ a }. (6) From that we can determine the angle ψ uniquely in the interval, π) to any arbitrarily chosen value θ! Note that the singular Case occurs when the first and the third rotation axis are concurrent which is, for instance, the case in the home position (Fig., top, left)!..4 Cardan angles A rotation ρ SO(3) can always be written as a composition of three rotations about the x-axis, the y-axis and the z-axis. (Fig. ). Hence, the corresponding rotation matrix A is the product of the three matrices R x (α), R y (β), R z (γ) (compare with (49), (5), (5)): A R z (γ) R y (β) R x (α) cos γ sin γ sin γ cos γ cos β sin β sin β cos β cos α sin α sin α cos α cos γ cos β sin γ cos α + cos γ sin β sin α sin γ sin α + cos γ sin β cos α sin γ cos β cos γ cos α + sin γ sin β sin α cos γ sin α + sin γ sin β cos α sin β cos β sin α cos β cos α (6) Note again that the order of transformations matters! In the matrix product above you have to read from right to left. Definition.. The three rotations about the x-, y-, and z-axis are called roll, pitch and yaw. The three corresponding angles α, β, γ are the Cardan angles, sometimes also referred to as nautical angles or Tait-Bryan angles.

26 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots e,z e,z e,z e,z e,y e,y e,x e,x e,y e,y e,x e,x e,y e,z e,z e,y e,x e,y e,z e,z e,y e,x e,x e,y e,x Figure : Cardan angles α, β, γ. Home position: α β γ (top, left). A rotation about x by angle α (top, right), followed by a rotation about y by angle β (bottom, left) and by a rotation about z by angle γ (bottom, right).

27 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 3 The inverse problem to find the three Cardan angles α, β, γ for a given rotation matrix A (eq. (47)) is solved in a similar way as the inverse problem for Euler angles: Case : a + a cos β. This means that β ± π. From the first column of A we sin γ get and from this we can uniquely determine the angle γ in, π). Similarly, we get sin α from the third row of A, which gives us the angle α uniquely in, π). a a, cos ψ, (6) a + a a + a a 3 a, cos α 33, (63) a + a a + a We know that sin β a 3 and cos β a + a from which we obtain a unique value of β in the interval ( π, + π ). Case : a + a cos β. This means that β π (in this case a 3 ) or β π (then a 3 ). Now we get a one-parametric set of solutions for either of the other two Cardan angles α and γ. We can, for example, choose α arbitrarily; then we obtain two linear equations for the yet unknown values cos γ and sin γ from the entries a and a of A: from which we get ± sin α cos γ cos α sin γ a cos α cos γ ± sin α sin γ a cos γ cos α a ± sin α a sin γ cos α a ± sin α a } (64) }. (65) From that we can determine the angle γ uniquely in the interval, π) to any arbitrarily chosen value α! Here again the singular Case occurs when the first and the third rotation axis are concurrent!.3 Quaternions Another frequently used tool to represent rotations in 3-space are the so-called quaternions which were independently discovered by O. Rodrigues in 84 and W.R. Hamilton 3 in 843. To introduce quaternions we start with the 4-dimensional column vector space q R 4 {q q q q, q, q, q 3 R}. q 3 Olinde Rodrigues (795 85), French mathematician, banker and social reformer, 3 Sir William Rowan Hamilton (85 865), Irish physicist, astronomer and mathematician. Hamilton first tried to generalize the concept of complex numbers (which were already known as a tool for describing planar Euclidean transformations) to 3 dimensions but could not find a suitable multiplication-rule for 3-dimensional numbers. Later Hamilton wrote in a letter to his son Archibald: Every morning in the early part of October 843, on my coming down to breakfast, your brother William Edward and yourself used to ask me: Well, Papa, can you multiply triples? Whereto I was always obliged to reply, with a sad shake of the head, No, I can only add and subtract them. The solution to the problem suddenly occurred to him on a walk along the Royal Canal in Dublin, on October, 6th, 843: He recognized that one has to use 4 instead of 3 dimensions. Glad about his success Hamilton promptly carved the fundamental equation rules for his quaternions into the side of the nearby Broom Bridge: i j k i j k. William Hamilton devoted the remainder of his life to the study and teaching of quaternions.

28 4 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots The product 4 is defined via q q r : q q q q 3, r r r r r 3 q r q r q r q 3 r 3 q r + q r + q r 3 q 3 r q r + q r + q 3 r q r 3 q r 3 + q 3 r + q r q r. (66) By means of the usual addition and this new multiplication the set R 4 becomes a so-called skew field, called the skew field of quaternions and denoted by H. This means that all usual calculation rules for addition and multiplication (like for instance in R) are valid except the commutativity of the multiplication: In general we have as one can easily check. The set R of real numbers can be embedded into H via r R q r r q r H, which allows us to identify the real number r with the quaternion r Note that the (quaternion-)product of a real number r with q the vector q: For a given quaternion q q q q q 3 r q r we define q q q q 3 q q q q 3 r q r q r q r q 3 r. is simply the product of the scalar r with its norm q : q + q + q + q 3, (67) its conjugate q : q q q q 3, (68) 4 This product of two vectors yields a vector again as opposed to the dot product of two vectors which yields a scalar!

29 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 5 and in case of q its inverse q : q. (69) q One easily verifies that q q q q q q. (last equality according to the embedding of R in H; see above!). Due to this we also have q q q q q q q q q q q q. After having introduced the three quaternion units i :, j :, k : (7) we can easily check that i j k j k i k i j i j k : i j k. (7) By means of this notation an arbitrary quaternion q q q q q 3 can now be written in the form q q + q i + q j + q 3 k. (7).3. Using quaternions for representing rotations in 3-space Let a rotation ρ SO(3) be given. As we know from Section.., pp. 8, we can describe this transformation by means of normalized Euler parameters: x y z with q + q + q + q 3. q + q q q 3 (q q q q 3 ) (q q 3 + q q ) (q q + q q 3 ) q q + q q 3 (q q 3 q q ) (q q 3 q q ) (q q 3 + q q ) q q q + q 3 x y z (73)

30 6 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots With the quaternions x : x y z x i + y j + z k, x : x y z x i + y j + z k, and q : q q q q 3 q + q i + q j + q 3 k, the rotation ρ (73) can also be written this way: x q x q. (74) For a verification one only has to multiply out the right-hand side 5 and compare with the right-hand side of (73). Note that q is a normalized quaternion: q q + q + q + q 3!.4 General displacements in the Euclidean 3-space If we are given two right-handed Cartesian frames F : {O, e,x, e,y, e,z }; F : {O, e,x, e,y, e,z } in the Euclidean 3-space then an arbitrary point X has a representation w.r.t. each of the two coordinate frames, i.e., This means that x : and F, respectively. x y z O X x e,x + y e,y + z e,z, (75) O X x e,x + y e,y + z e,z. (76) and x : In order to bring the first frame F into the second one F we x y z are the coordinate columns of the point X w.r.t. the frames F first apply a rotation ρ around a suitable axis through the origin O of F so that F is transformed in a frame F whose axes are parallel to and equally oriented as those of F (see Fig. ) and subsequently apply the translation τ with the translation vector O O. Let A : a a a 3 a a a 3 a 3 a 3 a 33 denote the rotation matrix belonging to ρ and let d : d x d y d z 5 Check that! This is useful for exercising quaternion multiplication.

31 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 7 e,z e,z e,z e,y e,y O e,x O e,x e,x e,y Figure : Transformation of a right-handed frame F : {O, e,x, e,y, e,z } into another one F : {O, e,x, e,y, e,z }. be the coordinate column of the translation vector O O w.r.t. F. Then, is the relation between x and x, i.e., x y z : x : d + A x, (77) d x d y d z + a a a 3 a a a 3 a 3 a 3 a 33 x y z. (78) Result.3. A general displacement in the Euclidean 3-space has an analytical description of the form (77), with a translation vector d and a proper orthogonal 3 3 matrix A. a a a 3 Note that the 3 columns a, a and a 3 of the rotation matrix A are the coordinate columns of a 3 the vectors e,x, e,y and e,z w.r.t. the vector base {e,x, e,y, e,z }. a 3 a 33 In analogy to the planar case the Euclidean displacements (77) establish a continuous group SE(3), the so-called special Euclidean group of the 3-space. The group SE(3) is 6-parametric: Three parameters are contained in the translation vector d and the remaining three parameters are packed in the rotation matrix A. (Particular parameterizations of A are available from Section., p. 7 3.) We can also put it this way: In 3-space there are 6 degrees of freedom (6 dofs)..4. Determination of a spatial displacement A general spatial displacement F {O, e,x, e,y, e,z } F {O, e,x, e,y, e,z } can for instance be determined by the following input (all given coordinate representations are w.r.t. the frame F ): the coordinate column d : d x d y d z of the point O,

32 8 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots the coordinate column f : f,x f,y f,z of a (not necessarily normalized) direction vector of the x- axis of F and the coordinate column f : of another vector parallel to the xy-plane of F so that f, f determine the same orientation as e,x, e,y. The mathematical description of the displacement is then given by (77) where the columns a, a and a 3 of the matrix A are f,x f,y f,z a : f f, a 3 : f f f f, a : a 3 a. Example.4. Let ABCDEF GH be a cuboid (Fig. 3) with edge lengths 3 dist(a, B) dist(c, D) dist(e, F ) dist(g, H), dist(b, C) dist(a, D) dist(f, G) dist(e, H), dist(a, E) dist(b, F ) dist(c, G) dist(d, H). (79) G H C D f F a E B f a 3 A a a e,z e,y O e,x Figure 3: Determination of a general spatial displacement Let moreover a coordinate frame F {O, e,x, e,y, e,z } be given and let a : 3, 4 f :, f : be the coordinate columns of

33 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 9 the cuboid s vertex A, a direction vector of the cuboid s edge AB with the same direction as AB a vector parallel to the cuboid s face ABCD so that f, f establish the same orientation as AB, AC. What is the coordinate column g of the cuboid s vertex G w.r.t. F? To solve the problem we establish a second coordinate frame F with origin O A and coordinate axes along the cuboid s edges AB, AD, AE. The transformation (77) from F back to F is then given by (77) where the translation vector is d a. Moreover, the first, the third and the second column of the rotation matrix A is a : f f 3 4, a 3 : f f f f, a : a 3 a 3, respectively; i.e., A Note that a, a and a 3 are unit vectors parallel to the cuboid s edges. Thus, according to (79) the coordinate column of the cuboid s vertex G w.r.t. to F is 3,,. We compute the coordinate column g of G w.r.t. F : g Homogeneous representation of a spatial displacement Like in the planar case (cf. Section..3, p. 3) we can rewrite the transformation (78) in a more condensed form: x y z a a a 3 d x a a a 3 d y a 3 a 3 a 33 d z x y z (8) With X : x where o : x y z, X : x we can rewrite (8) as x y z A, B : d o a a a 3 d x a a a 3 d y a 3 a 3 a 33 d z X B X. (8) This notation is convenient in case of composed transformations: The respective matrices B simply have to be multiplied.

34 3 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots.5 Motions in space Analogously to the planar case we define a motion in space as a continuous one-parametric series of spatial displacements (77), p. 7. The 3-space is represented by two copies Σ, the so called fixed system and Σ, the moving system (Fig. 4). We denote the motion by Σ \Σ. As motion-parameter we will take the time t. c X X x z x e,y x e,z O e,x e,y d e,z O e,x y y z x Figure 4: A spatial motion. After having chosen a coordinate frame F {O ; e,x, e,y, e,z } in the fixed system Σ and another one F {O ; e,x, e,y, e,z } in the moving system Σ the one-parametric motion Σ \Σ is represented by an equation of the form x : d(t) + A(t) x, (8) where as opposed to (77), p. 7 the translation vector d as well as the rotation matrix A are functions of a variable, namely the motion parameter t: d x (t) a (t) a (t) a 3 (t) d d(t) d y (t), A A(t) a (t) a (t) a 3 (t). d z (t) a 3 (t) a 3 (t) a 33 (t) If a particular point X which is fixed in the moving system Σ is subjected to the motion Σ \Σ then X traces out a path c X. If x denotes the coordinate column of X w.r.t. the moving system s coordinate frame F then a parameterization w.r.t. to the fixed system s coordinate frame F of the path c X is already available via (8)..5. Velocitiy behavior in the spatial case We obtain the velocity vector v X, of a point X by differentiation of (8) w.r.t. t: v X, ẋ ḋ + Ȧ x (83)

35 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 3 (For brevity we suppress the notation of t.) From (8) we get and by substitution into (83): ẋ A (x d) v X, ḋ + Ȧ A (x d) ḋ + W (x d). (84) The matrix W Ȧ A the so-called angular velocity matrix is skew symmetric as in the planar case. The skew symmetry of W can be proved as follows: From the orthogonality property of A we have: A A I 3, and by differentiation (product rule!) Ȧ A + A Ȧ O 3. Hence, W Ȧ A A Ȧ (Ȧ A ) W qu.e.d. Due to its skew symmetry the angular velocity matrix W has the following shape: w z w y W w z w x (85) w y w x If we pack the three essential components w x, w y, w z of W into a 3-vector we can write (84) this way: 6 w : w x w y w z v X, ḋ + w (x d) (86) The vector w is called the angular velocity vector of the motion Σ \Σ at the instant t. From this equation the velocity distribution of the motion Σ \Σ at a particular time instant t can be easily derived. We list the possible cases: 7 Case : w, ḋ. At a time instant t where this is the case the velocity vector field of the motion Σ \Σ behaves like that of a screw motion around a particular axis a parallel to w. The screw parameter is p w, ḋ w. We speak of an instantaneous screw motion in this case. 6 denotes the cross-product of two vectors! 7 For the proofs see.

36 3 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots Case : w, ḋ but w,,. If this is the case for a time instant t then concerning the velocities the motion Σ \Σ behaves like a rotation around an axis a which is parallel to w. We have the case of an instantaneous rotation. Case 3: w,,. Due to (86) we have v X, ḋ which means that all points X have the same velocity vector ḋ at this instant t: In this case we have an instantaneous translation. If additionally ḋ,, then the motion is in an instantaneous standstill. 3 Composition of motions; kinematic chains Consider a rigid body Σ which is moved w.r.t. to another one Σ. At the same time a third rigid body Σ is moved w.r.t. Σ. So altogether we have three motions, namely Σ \Σ, Σ \Σ and Σ \Σ. In this case we call the motion Σ \Σ the composition of the motions Σ \Σ and Σ \Σ and speak of a kinematic chain with three links Σ, Σ, Σ. An example of a planar kinematic chain with three systems Σ, Σ, Σ (a planar serial manipulator with two revolute joints) is shown in Fig. 7, left, p. 38, whereas Fig. 5, p. 33, depicts a spatial kinematic chain with three links Σ (ground), Σ (chassis), Σ (wheel). As for kinematic chains it is advantageous to use the homogeneous notation as introduced in Section..3 p. 3 (planar case) and in Section.4., p. 9 (spatial case) for the mathematical representation: Let the two motions Σ \Σ and Σ \Σ be described by and X B X (87) X B X (88) where X : x x, X : x, X : A d, B : o A d, B : o. Here x, x and x are the coordinate columns of a point X w.r.t. coordinate frames F, F and F which have to be chosen in Σ, Σ and Σ. Note that in case of planar motions the coordinate vectors x, x, x as well as the vectors d, d and o (the zero vector) are -vectors whereas in case of spatial motions they are 3-vectors. A and A are proper orthogonal matrices ( -matrices in the planar case and 3 3-matrices in the spatial case). As we have a one-parametric motions the translation vectors d, d and the matrices A and A depend on the time parameter, let s say the time t: d d (t), d d (t), A A (t), A A (t) The composed motion Σ \Σ can now easily be parameterized by substitution of (88) into (87): X ( B X ) B B X. (89)

37 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 33 e,z e,x O e,y Σ b e,z Σ O e,y e,z O e,y e,y r e,x e,x Σ ḋ k Figure 5: The kinematic chain consisting of the ground Σ, a vehicle s chassis Σ and a wheel Σ. 3. Example: Motion of a mobile robot with two coaxial individually drivable nonsteerable wheels (ctd.) We once again consider the example of a mobile robot with two coaxial individually drivable non-steerable wheels (cf. Section.3.3, p. 3) and investigate the kinematic chain consisting of the planar ground Σ, the chassis Σ and the left wheel Σ. The coordinate frames F {O, e,x, e,y, e,z }, F {O, e,x, e,y, e,z } and F {O, e,x, e,y, e,z } of Σ, Σ and Σ are chosen as illustrated in Fig. 5. The motion Σ \Σ of the chassis w.r.t. the ground is the Frenet motion along a prescribed curve k lying in the plane Σ. If d(t) d x (t) d y (t) is the parameterization of k then w.r.t.. the chosen coordinate frames F, F the Frenet motion Σ \Σ is described by x y z where r denotes the wheel radius and cos ϕ cos ϕ sin ϕ d x sin ϕ cos ϕ d y r d x d, sin ϕ y. d x + d y d x + d y x y z (9)

38 34 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots According to the choice of the frame F the rotation Σ \Σ (wheel\chassis) has the representation x y z cos ψ sin ψ b sin ψ cos ψ x y z. (9) Here b denotes the distance of the wheel centers and ψ denotes the rotation angle of the left wheel W L. Recall, that we already have computed the first derivative ω L (t) of the function ψ ψ(t), i.e., the angular velocity of the wheel; see eq. (36) on page 4. Hence, ψ(t) can be obtained by integration. By substitution of (9) into (9) we finally obtain the parameterization of the motion Σ \Σ (wheel\ground): x y z cos ϕ sin ϕ d x sin ϕ cos ϕ d y r cos ψ sin ψ b sin ψ cos ψ cos ϕ cos ψ sin ϕ cos ϕ sin ψ d x b sin ϕ sin ϕ cos ψ cos ϕ sin ϕ sin ψ d y + b cos ϕ sin ψ cos ψ r x y z x y z (9) 3. The velocity operator of a motion Let us consider a planar or spatial motion Σ \Σ presented by its homogeneous notation: 8 X B X (93) Here, X x x, X A d, B o (94) where x, x are the coordinate columns of a point X w.r.t. the coordinate frames F, F in Σ, Σ. In case of planar motions the coordinate vectors x, x as well as the vectors d and o (the zero vector) are -vectors whereas in case of spatial motions they are 3-vectors. A is a proper orthogonal matrix ( -matrix in the planar case and 3 3-matrix in the spatial case). As we have a one-parametric motion the translation vector d and the rotation matrix A are functions of the time t: d d(t), A A(t) We again suppress the notation of t in the following equations. From (93) we get X B X (95) Note that the inverse matrix of B is B A A d o. To obtain the velocity vector v X, of a point X we have to differentiate (93) w.r.t. t: 8 Compare with Section..3 p. 3 and Section.4., p. 9 Ẋ Ḃ X

39 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 35 More detailed this is ẋ and after substitution of (95) ẋ Ȧ ḋ A A d o o So we finally have with the matrix x G Ḃ B Ȧ ḋ o ẋ, Ȧ A Ȧ A d + ḋ o x x : G Ẋ G X. (96) W ḋ W d o where W denotes the skew symmetric angular velocity matrix, i.e., ϕ ω W ϕ ω in the planar case (cf. eq. (7), p. 5) and in the spatial case (cf. eq. (85), p. 3). W w z w y w z w x w y w x Of course, In the planar case the occurring translation vector d d(t) has two components, dx d whereas in the spatial case it has three, d Definition 3.. The 4 4-matrix G Ḃ B is called the velocity operator of the motion Σ \Σ for the time instant t. In order to deal with velocities of kinematic chains we have to answer the following question: How is the velocity operator G changed if we apply a displacement? Let the displacement be given by with d y d x d y d z. (97) X T X (98) T S s o where S is a proper orthogonal - or 3 3-matrix and s is a - or 3-vector depending on whether we are in the planar or spatial case. Then we have T Ẋ Ẋ G X G T X by means of (96) and (98). From this we get by left-multiplication with T: Ẋ T G T }{{} :G X Hence, a left multiplication with the displacement matrix T and a right multiplication with the inverse T has to be applied to G: G T G T

40 36 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots 3.3 The velocity behavior of composed motions Let be the composition of the two motions and Σ \Σ... X B B X (99) Σ \Σ... X B X () Σ \Σ... X B X () as introduced on page 3. To compute the velocity operator G of the composed motion Σ \Σ we have to differentiate (99) w.r.t. t: From (99) we obtain Ẋ (Ḃ B + B Ḃ ) X () which yields X B B X Ẋ (Ḃ B + B Ḃ ) B B X Ḃ B X + B Ḃ B B X (3) after having been substituted into (). We interpret this equation: Clearly, the matrix Ḃ B in the first summand is the velocity operator of the motion Σ \Σ. More accurately, it is the representation G of this velocity operator w.r.t. coordinate frame F of the system Σ. What is the meaning of the matrix B Ḃ B B in the second summand of (3)? The block in the middle Ḃ B is the representation of the velocity operator of the motion Σ \Σ w.r.t. the coordinate frame F of Σ. The combination of left-multiplication by B and right-multiplication by B means a back-transformation into the coordinate frame F of Σ (cf. Section 3.). Hence, the matrix in the second summand of (3) is the representation G of velocity operator of the motion Σ \Σ again w.r.t. coordinate frame F of Σ : As a conclusion we see that eq. (3) can be written as G B Ḃ B B Ẋ G X G X + G X (G + G ) X (4) where G, G and G are the velocity operators of the motions Σ \Σ, Σ \Σ and Σ \Σ, respectively. (All three of them are represented w.r.t. the coordinate frame F of Σ!). We have the Result 3.. The velocity operator G of the composition Σ \Σ of two motions Σ \Σ and Σ \Σ is the sum of the velocity operators G, G of the partial motions: G G + G (5) Note that this result is valid in the planar as well as in the spatial case! Consecutively, we list some important conclusions of our result. Let Σ \Σ is the composition of the motions Σ \Σ and Σ \Σ ; then we have:. From eq. (4) follows that the instantaneous velocity vectors of points behave additively if motions are composed: If X is an arbitrary point whose instantaneous velocity vectors w.r.t. the motions Σ \Σ and Σ \Σ are v X,, v X,, respectively, then the instantaneous velocity vector v X, of X w.r.t. the composed motion Σ \Σ is v X, v X, + v X,. (6)

41 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 37. Written in detail eq. (5) has the form W ḋ W d o W ḋ W d o which means that the angular velocity matrices W ij also behave additively: W ḋ + W d o W W + W (7) Particularly, in the planar case this means that the angular velocities ω ij ϕ ij behave additively: ω ω + ω (8) This is obvious anyway, as in the planar case the rotation angles ϕ ij (t) already behave additively (see Fig. 6): ϕ ϕ + ϕ (9) Σ Σ Σ e,y e,x O e,y e,x ϕ e,y O e,x ϕ O ϕ ϕ +ϕ Figure 6: A planar kinematic chain with three systems Σ, Σ and Σ Example: A planar serial manipulator with two revolute joints We consider the device depicted in Fig. 7, left. It is a planar kinematic chain with three systems (rigid bodies) Σ, Σ and Σ. The motions Σ \Σ and Σ \Σ are rotations about the centers O and O, respectively. If we choose the coordinate frames F i : {O i, e i,x, e i,y } of the systems Σ i, i,, as shown in Fig. 7, left, we get the following representations of the rotations Σ \Σ and Σ \Σ : x y z x y z cos ϕ sin ϕ sin ϕ cos ϕ }{{} :B cos ϕ sin ϕ r sin ϕ cos ϕ } {{ } :B x y z x y z

42 38 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots v X, +v X, v X, v X, v X, X Σ Σ r e,y e,x e,y Σ O O O e,x ϕ e,y ϕ e,x O ϕ ϕ +ϕ O O θ v O, θ O Σ Σ Figure 7: A planar serial manipulator with two revolute joints. Left: The three systems and their coordinate frames. Right: Velocity vectors. Σ Here ϕ, ϕ denote the two rotation angles and r is the distance of the rotation centers O and O. The motion Σ \Σ composed of the two rotations has the representation x y z x B B y z cos ϕ cos ϕ sin ϕ sin ϕ cos ϕ sin ϕ sin ϕ cos ϕ r cos ϕ cos ϕ sin ϕ + sin ϕ cos ϕ cos ϕ cos ϕ sin ϕ sin ϕ r sin ϕ cos(ϕ + ϕ ) sin(ϕ + ϕ ) r cos ϕ sin(ϕ + ϕ ) cos(ϕ + ϕ ) r sin ϕ Fig. 7, right, illustrates how the velocity vector v X, w.r.t. the composed motion Σ \Σ of a point X can be constructed if the velocity vector v O, of the point O w.r.t. the rotation Σ \Σ and the velocity vector v X, of the point X w.r.t. the rotation Σ \Σ are given. The construction is done in two steps: O O is the instantaneous center of revolution (ICR) of the rotation Σ \Σ which allows to determine v X, from the given velocity vector v O,. According to (6) the velocity vector v X, is the sum of v X, and v X,. x y z x y z 3.3. Example: A vehicle Fig. 8 depicts a vehicle chassis (system Σ ). Two other systems are the road Σ represented by the straight line g) and the left front wheel Σ represented by the circle k. So we have a kinematic chain with three systems. Obviously the motion Σ \Σ is a translation. Hence the angular velocity ω of this motion is zero: ω () the motion Σ \Σ ist the rotation of the wheel w.r.t. the chassis about the center C of k. Thus A is the ICR of this motion: P C.

43 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 39 The composition Σ \Σ (wheel\road) of the rotation Σ \Σ and the translation Σ \Σ is the rolling of the circle k along the straight line g (cf. Section.3., p. 8). This means that g is the fixed k is the moving centrode of this motion. The ICR P of this motion is the contact point of g and k. Due to () we have according to the additivity of angular velocities (8). If the vehicles velocity v is given we have ω ω v C, v C, + v C, v + o v because C P is the ICR of the motion (rotation) Σ \Σ. (o denotes the zero vector.) Using this and the fact that the ICR P of the motion Σ \Σ is the contact point of g and k we can easily construct the velocity vector v H, of the point H on top of the wheel (see Fig. 8): The vector v H, is twice the vector v: v H, v Σ v v H, v H, v v C, v C, v H Σ P C P g k Σ P Figure 8: A vehicle. 3.4 Generalization to kinematic chains with an arbitrary number of systems The results on kinematic chains with 3 systems can easily be generalized to chains with an arbitrary number of systems. Such a chain is defined by n + systems (rigid bodies) Σ,..., Σ n with the additional assumption that the movability of Σ i+ w.r.t. to Σ i is restricted to a one parametric motion Σ i+ \Σ i for i,..., n. After having chosen a local coordinate frame F i in each of the systems Σ i each partial motion Σ i+ \Σ i is represented by its homogeneous equation X i B i,i+ X i+. By means of repeated substitution we can easily compute the representation of the motion Σ i \Σ : X B B... B i i X i. B i X i

44 4 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots g 5 Σ 5 Σ 5 Σ 6 g 6 Σ 3 Σ 4 Σ 3 Σ 4 g 3 g g 4 Σ g Σ Σ Figure 9: Left: A spatial robot with 6 revolute joints. Right: The humanoid NAO. This means that the transformation matrix B i of the motion Σ i \Σ is the product of the transformation matrices B,..., B i i of the motions Σ \Σ,..., Σ i \Σ i. Iteration of (5), (7) and (6) yields the additivity of the corresponding velocity operators, angular velocity matrices and velocity vectors: G i G + G G i i () W i W + W W i i () v X,i v X, + v X, v X,i i (3) 3.4. Kinematic chains with solely revolute joints We consider a kinematic chain with the systems Σ,..., Σ n where any partial motion Σ i+ \Σ i of two adjacent systems Σ i, Σ i+ is a rotation about an axis g i+. This means that Σ i and Σ i+ are linked by a revolute joint. Kinematic chains of that kind frequently appear in practical applications. One example are serial manipulators (Fig. 9, left), another one are the arm or leg chains of humanoid mobile robots (Fig. 9, right). The architecture of such a chain is determined by the so-called Denavit-Hartenberg parameters. To define these parameters we consider three consecutive systems Σ i, Σ i and Σ i+ of the chain (Fig. ). The motions Σ i \Σ i and Σ i+ \Σ i are rotations about the axes g i and g i+. These two straight lines have a common perpendicular p i, i.e., a line which intersects g i as well as g i+ orthogonally in points P i and O i+, respectively. Altogether we have n such perpendiculars p,..., p n. The Denavit-Hartenberg parameters of the kinematic chain are then defined as the following values a i, α i and d i :

45 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 4 a i : dist(g i, g i+ ) dist(p i, O i+ ), i,..., n α i : (g i, g i+ ), i,..., n d i : dist(o i, P i ), i,..., n p i+ O i+ P i+ g i+ O i α i a i p i p i P i d i P i g i g i Figure : Definition of the Denavit-Hartenberg parameters. e i+,z e i+,y u i+ e i+,x O i+ α i e i,z g i+ a i e i,y e i,x g i O i d i Figure : Canonical coordinate frames for a kinematic chain with solely revolute joints. Note that d is not defined so far we will catch up on this below. In order to obtain a mathematical description we attach a local coordinate frame F i {O i, e i,x, e i,x, e i,x } to each system Σ i in a canonical way (Fig. ): For i,..., n we choose O i as the pedal point of the perpendicular p i on the axis g i. The origin O of the frame F can be chosen arbitrarily on the first axis g and for the origin O of the frame F we take the same point, i.e., O O. Complementary we can now define where P is the pedal point of p on g. d : dist(o, P )

46 4 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots For i,..., n we choose e i,x as unit direction vector of the rotation axis g i. Particularly, if O i P i, we put O i P i e i,x : O i P i whereas there are two possible choices for the direction of e i,x if O i P i. Moreover we put e,x e,x. For i,..., n we choose e i,z as unit direction vector of the perpendicular p i : If P i O i+, we put P i O e i,z : i+ P i O i+ ; whereas in case of if P i O i+ there are two possible choices for the direction of e i,z. The vectors e,z and e n,z can be chosen arbitrarily but orthogonal to e,x and e n,x, respectively. Because {e i,x, e i,y, e i,z } has to be a right-handed orthonormal vector base we put e i,y : e i,z e i,x for i,..., n. As one can easily check the motion Σ i+ \Σ i this is a rotation about the axis g i+ by an angle u i+ has the following representation w.r.t. to the coordinate frames F i, F i+ chosen in Σ i, Σ i+ : x i cos α i sin α i d i x i+ y i z i sin α i cos α i a i cos u i+ sin u i+ sin u i+ cos u i+ y i+ z i+ With the notations X i : C i : x i y i z i we can write this in the form ; X i+ : x i+ y i+ z i+ cos α i sin α i d i sin α i cos α i a i,, R z(u i+ ) : cos u i+ sin u i+ sin u i+ cos u i+ X i C i R z (u i+ ) X i+. (4) Note that C i is a constant matrix that only contains the three Denavit-Hartenberg parameters a i, α i, d i whereas R z (u i+ ) is the rotation matrix about the z-axis belonging to the rotation angle u i+ which is a function of time: u i+ u i+ (t) The task to determine the position of the last system Σ n w.r.t. to the initial system Σ for given joint angles u,..., u n is called the forward kinematic problem. It is solved by the equation which we obtain by repeated substitution of (4). X R z (u ) C R z (u )... C n R z (u n ) X n (5) The inverse kinematic problem of the chain is the task to find a set of joint angles u,..., u n which belong to a prescribed position of the last system Σ n given by S s X T X n o X n where S is a given proper orthogonal 3 3-matrix and s is a given 3-vector. This means that the joint angles u,..., u n have to be to determined from the equation R z (u ) C R z (u )... C n R z (u n ) T. Note that this is a non-linear problem. Due to the fact that there are 6 degrees of freedom in space, we can in general expect a solution only if n 6. Some more details on the inverse kinematics of kinematic chains with solely revolute joints can be found in 4, pages

47 Mobile Robots A. Gfrerrer, Institute of Geometry, TU Graz, 43 b b a a a Figure : A non-steerable standard wheel. Figure 3: A steerable standard wheel. Figure 4: A castor wheel. 4 Wheeled mobile robots In this section we take a closer look on mobile robots supplied with wheels. One example of such a robot was already discussed in Section.3.3, p. 3. The robot there has two driven standard wheels and one passive ball wheel. Subsequently we list the various wheel types. 4. Wheel architectures 4.. The fixed standard wheel A fixed standard wheel (Fig. ) is a non-steerable wheel which is either driven (active wheel) or not (passive wheel). It has one degree of freedom (dof). 4.. The steerable standard wheel This -dof wheel (Fig. 3) rotates about a horizontal axis a and can be steered about a vertical axis b which meets a in the wheel center. Usually this wheel is supplied with two motors, one for driving the wheel (rotation about the axis a) and the other one for steering (rotation about b). A robot with three wheels of that kind (Fig....) allows for omnidirectional movement. This means that the robot can rotate about its center while simultaneously following a prescribed path. Note that a robot of that type has 6 degrees of freedom (3 motors for steering and 3 for driving the robot) which generates redundancy as we have have only 3 dofs in the plane! 4..3 The castor wheel Like a steerable standard wheel a castor wheel or caster wheel (Fig. 4) has degrees of freedom. As opposed to steerable standard wheels the two rotation axes a and b don t intersect. Castor wheels are frequently used as passive wheels.

48 44 A. Gfrerrer, Institute of Geometry, TU Graz, Mobile Robots b a Figure 5: A Mecanum wheel. Figure 6: A ball wheel Mecanum wheels Mecanum wheels or Swedish wheels (Fig. 5) were invented by the Swedish engineer Bengt Ilon in 973 (cf. 5). Such a wheel consists of a set of rolls arranged around the wheel axis a (see ). The roll axes b are skew to the wheel axis a. A Mecanum wheel is supplied with only one motor for driving; the rolls are passive. Like a robot with 3 steerable standard wheels a robot supplied with 3 Mecanum wheels is omnidirectional but without the disadvantage of redundant degrees of freedom. A disadvantage is that Mecanum wheels work well only on smooth ground Ball wheels A ball wheel or sphere wheel (Fig. 6) is a passive wheel with 3 dofs. 4. Velocity representation in the moving system In the next two Sections 4.3 and 4.4 we have to describe the vectorial velocities of a planar motion Σ \Σ w.r.t. the moving system Σ : If the motion Σ \Σ is given by x d(t) + A(t) x, i.e., x y dx (t) d y (t) cos ϕ(t) sin ϕ(t) + sin ϕ(t) cos ϕ(t) x y, then the velocity vector vx, of a point X is vx, ẋ ẏ d x d y sin ϕ cos ϕ + ω cos ϕ sin ϕ x y where ω ϕ. Here vx, is the representation of the velocity vector of the point X w.r.t. the fixed system s x coordinate frame F whereas is the position vector of X w.r.t. the moving system s coordinate frame F! y