Interpolate Rigi-Boy Motions an Robotics J.M. Selig Faculty of Business, Computing an Info. Management. Lonon South Bank University, Lonon SE AA, U.K. seligjm@lsbu.ac.uk Yaunquing Wu Dept. Mechanical Engineering. Shanghai Jiaotong University, Shanghai, P.R. China. Abstract This work looks at several problems concerne with interpolating rigi-boy motions an their application in robotics. In particular a new type of motion is introuce. Two recently propose interpolation methos are shown to prouce the same results. We also iscuss how it might be possible to control a robot in such a way as to follow one of these interpolate motions. Inex Terms Rigi-boy motion, path planning, interpolate motion. I. INTRODUCTION This work has several objectives, the first of which is to correct a small mistake in [9]. This concerns the relations which gives the acceleration of points on the eneffector of a serial robot. The correction is given in the next section together with a brief introuction to the geometrical methos use in the paper. The secon objective is to introuce a new type of special rigi boy motion which we call the Bishop s motion. This is moele on the Frenet-Serret motion given in [3]. These motions coul be use for robot path planning. They shoul be particularly useful when human operators specify the esire path of the robot s en-effector since they are base on curves in 3 imensions an are hence easy to visualize. The Frenet-Serret motion is reviewe in section III an in the following section the Bishop s motion is introuce. Next we look at some connections with robotics. Specifically, we stuy how it is possible to guie a serial robot arm along a particular esire motion. Two answers are sketche, a numerical metho for the inverse kinematics an a non-linear control metho. Finally, we look at two recently propose interpolation schemes, [] an [4]. Although these methos approach the problem ifferently, the first focuses on the Lie groups while the secon concentrates on the paths of points in space, we are able to show that the two methos prouce ientical curves. II. SCREW THEORY Screw theory concerns the group of rigi boy motions SE3 an its Lie algebra se3. Elements of the groups, that is rigi boy motions, can be written as 4 4 matrices, R t g with R a 3 3 rotation matrix an t a translation vector. In this representation of the group it action on points can be written as follows. We enote a point in space by a vector p x, y, z T, then we exten this vector by aing an extra, p x y. z After the rigi transformation the position of the new point will be given by, p R t p Rp + t. Elements of the Lie algebra of the group correspon to velocities or small motions, they are calle twists or screws. In general, if we think of a path in the group as a continuously parameterise sequence of group elements gt, then for any parameter value we have a Lie algebra element given by, S t gt gt. If the group elements gt are given as 4 4 matrices then the Lie algebra element will have the form, Ω v S, where Ω is a 3 3 anti-symmetric matrix corresponing to the angular velocity of the motion. The vector v is a characteristic linear velocity of the motion. The action of the group on its Lie algebra elements is given by the conjugation, S gsg. This correspons to a left translation in the group. To see this suppose gt is a path in the group as above, now lefttranslate this by a constant group element h to get hgt. The Lie algebra elements of this path are now, h t gt gt h hsh. Clearly a right translation has no effect on the screw.
It is often convenient to write screws as 6-imensional Ω v vectors, corresponing to a screw S we can write a vector, ω s, v where the vector ω correspons to the anti-symmetric matrix Ω in the following way, for any vector u we have Ωu ω u. This 6-imensional representation of screws is useful in many cases. In particular it allows us to efine elements of the ual Lie algebra. Elements of this ual vector space are calle wrenches an written, M W, F where F is a force an M is a moment. These coul also be linear an angular momenta, if we were consiering ynamics. If a rigi boy is moving with instantaneous velocity given by the screw s an is acte on by a total force an moment given by the wrench W then the instantaneous power exerte on the boy is given by, power W T s M ω + F v. In robotics we can associate a screw to each joint in of the robot. The possible motions about the joint are given by the exponential, gθ e θs, where θ is the joint parameter, an angle if the joint is revolute. The superscript in the above refers to the position of the joint screw in the home position. The exponential of a matrix is given by the stanar formula, e S I + S + 2! S2 + 3! S3 + In general the position of a point p attache to the eneffector of the robot will be given by, pt e θs e θ 2S 2 e θ 3S 3 e θ 4S 4 e θ 5S 5 e θ 6S 6 p. Although the home positions of the joint screws Si are constants the current positions of these joint screws change, the kinematics of these are given by, S S S 2 e θs S 2 e θs S 3 e θ S e θ 2 S 2 S 3 e θ 2S 2 e θ S. S 6 e θ S e θ 2 S 2 e θ 5 S 5 S 6 e θ 5S 5 e θ S So the time erivatives of these screw are, t S since S is constant. t S 2 θ e θ S S S2 S2S e θ S θ [S, S 2 ] the last equality follows because S commutes with e θs. t S 3 θ e θ S S e θ 2S 2 S 3 e θ 2S 2 e θ S an so forth until, θ e θ S e θ 2 S 2 S 3 e θ 2S 2 S e θ S + θ 2 e θs e θ 2S 2 S 2 S 3e θ2s 2 e θ S θ 2 e θs e θ 2S 2 S 3 S 2e θ2s 2 e θ S θ [S, S 3 ] + θ 2 [S 2, S 3 ] t S 6 θ [S, S 6 ] + θ 2 [S 2, S 6 ] + θ 3 [S 3, S 6 ] + θ 4 [S 4, S 6 ] + θ 5 [S 5, S 6 ]. In [9, Chap 3.] a formula is given for the acceleration of a point attache to the en-effector of a serial robot. This formula contains an error, so we present a correcte version here. Differentiating the position of a point on the en-effector gives, ṗt θ Se θ S e θ 6 S 6 p + θ 6 e θs S 6 e θ6s 6 + p an this simplifies in the usual way to, ṗt θ S + + θ pt 6 S 6 This gives the velocity of the point. If we ifferentiate again to get the acceleration we have, { pt 6 θ i i S i + θ 6 i<j 6 i θj [S i, S j ] + θ 2 } pt i i S i We can simplify the above a little by expaning the square term, remembering that S i an S j o not generally commute. We get, pt { 6 i θi S i + θ i 2S2 i + 2 i<j 6 θ i θj S i S j } pt The terms θ i S i + θ i 2S2 i are the accelerations woul expect from motion about a single joint. A more comprehensive account of this view of screw theory can be foun in []. III. FRENET-SERRET MOTION In [3] Bottema an Roth stuy a number of special motions, one of which is the Frenet-Serret motion. Such a motion is etermine by a space-curve pt. Now in a Frenet-Serret motion a point in the moving boy moves along the curve an the coorinate frame in the moving boy remains aligne with the tangent t, normal n, an
binormal b, of the curve. Using the 4-imensional representation of SE3 the motion can be specifie as, Rt pt gt, where pt is the curve an the rotation matrix has the unit vectors t, n an b as columns, Rt t n b. 2 The famous Frenet-Serret relations are, ṫ vκn, 3 ṅ vκt + vτb, 4 ḃ vτn, 5 where v, κ an τ are respectively the spee, curvature an torsion of the curve. Our work here will be simplifie by introucing the Darboux vector ω vτt + vκb which has the properties that, ṫ ω t, ṅ ω n, ḃ ω b, 6 see [7, II.4] for example. This means that we can write, Ṙ ΩR, 7 where Ω is the 3 3 anti-symmetric matrix corresponing to ω. Hence we have that, Q t g g Ω vt ω p, 8 remember that ṗ vt. Using the Frenet-Serret relations 3 5 above the erivative of the velocity is, Q Ω vt ω p. 9 Here, ω vτ + v τt + vκ + v κb. In the 6-vector representation this is, ω q. vt ω p IV. BISHOP S MOTION In [2] Bishop gave an alternative metho to associate a moving frame to points on a curve in 3 imensions. In the same way that the Frenet-Serret frame etermines a special rigi boy motion etermine by a curve, the Bishop frame can also be use to efine a special motion. A point in the rigi boy follows a curve an an orthonormal frame in the boy stays aligne with the Bishop frame. Such a motion will be calle a Bishop s motion. There are some applications of the Bishop frame in Computer graphics to thicken curves. The Bishop frame is use because it oesn t twist about the curve, see Fig. b. In the Frenet-Serret frame the torsion cannot be efine at points where the curvature vanishes, this is not a problem for Bishop s frame. Another feature of the Bishop frame is that it is not unique, however once the initial frame has been selecte the frames for the rest of the curve are uniquely etermine. This suggests that the Bishop s motion efine above may be useful for robot path planning. The frame equations for Bishop s frame are: ṫ vk n + vk 2 n 2, ṅ vk t, 2 ṅ 2 vk 2 t. 3 As usual v is the spee of the curve an ṗ vt, where t is the unit tangent vector. The vectors n an n 2 are unit normal vectors an together with the tangent vector t, they form an orthonormal frame. So for example n n 2 t an so forth. The parameters k an k 2 are curvature-like functions. In terms of the Frenet-Serret frame the normal vectors n an n 2 are given by, n cos φn sin φb, 4 n 2 sin φn + cos φb. 5 where the angle φ is given by the inefinite integral, φ vτ t. 6 So, as mentione above, a curve oes not uniquely etermine a Bishop frame, there is a single rotational freeom in efining the Bishop frame, given by the constant of integration. But if we choose the unit normal vectors n an n 2 at t then the Bishop frame for the rest of the curve is unique. The path in the group etermine by a Bishop s motion will be the same as for the Frenet-Serret motion, see above, but now the rotation matrix will be given by, Rt t n n 2. 7 To compute the velocity of a Bishop s motion we nee an analogue of the Darboux vector. This is given by the vector, It is simple to verify that, a vk 2 n + vk n 2. 8 ṫ a t, ṅ a n, an ṅ 2 a n 2. Note that it is possible to show that a vκb. The velocity is, Q t g g A vt a p, 9 where, as usual, capital A represents the 3 3 antisymmetric matrix corresponing to the vector a. Proceeing as in the previous section we can compute the erivative of the velocity screw, as a 6-imensional vector this is ȧ q. 2 vt ȧ p
a b Fig.. a a Frenet-Serret motion an b a Bishop s motion. Both these motions were base on the same cubic spline curve which is approximate by the black tangent lines in either case. The initial frame for the Bishop s motion was chosen to coincie with the initial Frenet-Serret frame. So the frame at the bottom left of each iagram are the same. Notice how the blue binormal line in a rotates through 8 egrees as we move along the curve. However, in b the blue normal only turns through 9 egrees. V. ROBOT KINEMATICS AND CONTROL In this section we relate the esire motions to a couple of common problems in robotics. In general we want to rive the robot so that its en-effector follows the esire path. In the first case we look at computing the inverse kinematics along such a path. Let us write z 6, for the velocity screw of the en-effector of a robot. In terms of the joint variables this velocity screw is, 6 θ i s i z 6. 2 i This equation shows that the columns of the robot s Jacobian are given by the robot s joint screws s i. Now we can think of the inverse Jacobian of the manipulator as compose of six wrenches W, W 2,..., W 6 such that, { Wi T, if i j s j, if i j. 22 For many commercially available esigns of robot these quantities can be compute symbolically, see [, 6.7]. Setting the velocity of the robot s en-effector to the esire velocity gives, θ s + θ 2 s 2 + + θ 6 s 6 q. 23 Pairing this equation with the wrenches W i, the rows of the inverse Jacobian of the robot, yiels, θ i W T i q, i, 2,... 6. 24 These ifferentially equations shoul be straightforwar to set up an solve numerically using the Runge-Kutta metho for example. Notice that we nee to know the starting configuration of the robot but we only nee the velocity of the esire motion. Of course, like most methos in robotics, this metho will fail near singularities. As an alternative approach to guiing the robot along a esire path we can esign a close-loop controller for the robot. The scheme here is base on a non-linear feeback control scheme introuce in [8]. The iea of this is to control the en-effector of the robot along a specifie path without having to perform any inverse kinematic computations. The key is to arrange for the feeback to be such that the close loop ynamics of the system are ifferential equations in the robot s joint variables that are satisfie by the esire path. This iea is similar to the Passive Velocity Fiel Control metho introuce by Li an Horowitz[5]. However the metho presente here is somewhat simpler, since we o not introuce any extra egrees of freeom. Equation2 above can be ifferentiate to give, 6 θ i s i + i j<k 6 θ j θk [s j, s k ] ż 6. 25 Away from singularities we can pair this equation with the wrenches W i, we get, θ i + Γ ijk θj θk W T i ż6, i, 2,..., 6 26 where summing over repeate inices is assume. The quantities Γ ijk are given by, { Γ ijk 2 WT i [s j, s k ] j k 2 WT i [s 27 j, s k ] j k. If we replace the velocity of the en-effector with the velocity of the esire motion we obtain an equation for the esire motion in the joint space variables of the robot, θ i + Γ ijk θj θk W T i q, i, 2,..., 6. 28 Notice, the time erivative of a velocity screw is not exactly an acceleration. If the close-loop ynamics of the robot take this form then the en-effector will follow the esire motion, subject to consierations of stability an errors of course. Depening on whether we want to follow a Frenet-Serret motion of a Bishop s motion we must use q as given by either or 2. Now we turn to the ynamics of the robot. These can be represente by the equations, A ij θj + B ijk θj θj τ i, i, 2,..., 6 29
again summation over repeate inices is assume, see [, Chap. 3] for example. Here A ij is the generalise mass matrix an B ijk represents the coupling an Coriolis terms. The joint torques, which we assume that the control system can apply to robot s motors, are represente by τ i. For simplicity gravity will be ignore. Now suppose that our control system measures the joint variables an their rates, the motor torques can be set to, τ i B ijk A il Γ ljk θ j θk + A il W T l q, i,..., 6 3 This is a non-linear feeback control law. The close-loop equations of the system are foun by substituting for τ i in the robot ynamics equation 29 above. The result will be the equation for the esire motion 28 above multiplie by the positive-efinite mass matrix A ij. Notice that the evelopment of the control law above is base on the task space equation, ż 6 q. 3 For a stable controller it is probably better to base the metho on the equation, ż 6 q + λz 6 q, 32 where λ is a positive constant. Clearly, z 6 q is a solution to this equation. Moreover, if we assume that z 6 q + e, where e is an error vector, then the ynamics of the error vector obey, ė λe. 33 So the errors in the velocity ecrease exponentially. The joint space version of equation 32 is, θ i + Γ ijk θj θk W T i q + λw T i q θ i, i,..., 6 34 The corresponing close loop control law will be, τ i B ijk A il Γ ljk θ j θk + A il Wl T q + λa il Wl T q θ l, i,..., 6 35 VI. PROJECTION BASED INTERPOLATION Recently, two new methos of proucing rigi boy motions have been propose, see [] an [4]. Both these methos rely on projecting affine motions to rigi-boy motions. Belta an Kumar s metho interpolates the motions irectly to prouce a curve in the group of affine motions GA + 3 an then projects these motions to the nearest rigi-boy motion. On the other han, the metho escribe by, Hofer, Pottmann an Ravani is base on the paths of points in space. In this metho several points on the rigi boy are selecte an interpolation curves are efine for each of the points. Now at some particular instant the positions of the interpolate points may not be a rigi transformation of the original points. To prouce a rigi boy motion a least squares problem is solve to fin the positions of the points subject to the constraint that they are rigily relate to the original points. We show here that these methos are essentially the same. We begin with a number of rigi boy motions we wish to interpolate or approximate, assume these are given by, Ri t i, i,..., n. Now choose a number, say k, of points a j, at least 4 non-coplanar points accoring to [4]. The knot-points for the interpolation are then, b j i R i a j + t i, i,..., n. 36 So we get a set of interpolate curve, n p j t f i tb j i, j,..., k 37 i where f i t are the interpolating functions. In Belta an Kumar s metho we woul interpolate in the space of matrices to get, Mt t Xt Then clearly we have, p j t a j Xt n i f n itr i i f itt i. 38, j,..., k. 39 In general the matrix Xt will lie in the group GA + 3, this is because the 3 3 block Mt n i f itr i will generally be an element of GL + 3. However, it may happen that for some particular values of t, etmt, some conitions for this are stuie in []. The corresponing problem for points, that is values of t where the set of points p j t may be coplanar, oes not seem to have been anticipate in [4]. In Hofer et al s metho we fin the rigi motion at some time t by minimising the quantity k j pj t Ra j t 2. That is we seek a rotation matrix R an a translation vector t such that this expression is minimal. A solution to this problem can be foun in []. The first step is to choose the origin of coorinates to be at the centroi of the a j points, this is so that k j aj. In these coorinates the minimal translation is given by, t k k p j. 4 j To simplify notation we have roppe the explicit epenence on t, but we are assuming some efinite value of t. Now using the fact that p j Ma j + an the fact that the sum of the a j s is zero we have, t. 4 The minimal rotation can be foun from the polar ecomposition of the matrix, P k p j a j T. 42 j
a b Fig. 2. a a cubic projecte interpolate motion base on the en-points of the Bishop s motion given in Fig. b an b the Bishop s motion from Fig. b for comparison. This simplifies to, P M k a j a j T. 43 j The solution is the rotation R such that P RK where K is symmetric. Details on computing this an its relation to the singular value ecomposition of P can be foun in almost any numerical methos text. Belta an Kumar s metho is not really a single metho but several epening on a positive efinite symmetric matrix W. The metho requires us to fin the polar ecomposition of the matrix prouct MW. So if we ientify W with the matrix, k W a j a j T, 44 j the curves in SE3 given by the two approaches will be ientical. On the other han, Belta an Kumar give particular attention to the case where W I the ientity matrix. Now if we choose the points a j carefully then we can make the matrix k j aj a j T I. A suitable choice here woul be the vertices of a regular tetraheron, a 2/ 6 /2, a 2 / 6 / 2 3 /2, 3 a 3 / 6 / 2 /2, an a 4. 45 3 3/2 In fact, from Schur s lemma, any collection of points a j which are symmetrical with respect to some finite subgroup of rotation crystallographic point groups, will give a matrix W that is a scalar multiple of the ientity. An example of such a motion is shown in Fig. 2a. VII. CONCLUSIONS In sections III an IV above we introuce two new types of rigi motion paths. The first of these, the Frenet- Serret motion, has been known in the mechanisms community for many years but oes not seem to have been applie to robots. The Bishop s motion oes not seem to be familiar to the mechanisms community. Neither of these motions is really an interpolate motion since once the path of a point is chosen the rotational motion is etermine. In the Bishop s motion we also have the freeom to choose an initial rotation. Nevertheless, these simple motions may be useful in practical circumstances. The purpose of section V was really to show that for applications to robotics we nee to know the velocity of the rigi motion curve. An sometimes also the erivative of the velocity. The control metho presente was rather simple an not to be taken too seriously. However, the iea of esigning the control system of a robot in such a way that it follows a esire trajectory using a knowlege of the geometry of the esire path rather than just employing inverse kinematics is a worthwhile goal. This means that interpolation techniques which result in curves whose velocities are ifficult to calculate are less esirable for applications in robotics. At the moment it is ifficult to see how to compute the velocity of a curve given by the two schemes stuie in section VI. REFERENCES [] C. Belta an V. Kumar, An SVD-projection metho for interpolation on SE3, IEEE Trans. Robotics an Automation, vol. 8, pp. 334-345, 22. [2] R.L. Bishop, There is more than one way to frame a curve, Am. Math. Monthly, vol. 82 pp. 246 25, 975. [3] O. Bottema an B. Roth, Theoretical Kinematics, Dover Publications, New York, 99. [4] M. Hofer, H. Pottmann, an B. Ravani. From curve esign algorithms to the esign of rigi boy motions. The Visual Computer vol. 2, pp. 279 297, 24. [5] P.Y. Li an R. Horowitz, Passive velocity fiel control of mechanical manipulators, IEEE Trans. Robotics an Automation, vol. 5, pp. 75-763, 999. [6] D. Marsh, Applie Geometry for Computer Graphics an CAD 2n. e., Springer Verlag, Lonon, 25. [7] B. O Neill, Elementary Differential Geometry, Acaemic Press, New York, 966. [8] J.M. Selig an A.I. Ovseevitch, Manipulating robots along helical trajectories, Robotica vol. 4 pp. 26 267, 996. [9] J.M. Selig. e. Geometric founations of robotics Worl Scientific, Singapore, 2. [] J.M. Selig. Three Problems in Robotics, Proc. Inst. Mech. Eng. part C: J. Mechanical Engineering Science, vol 26, pp.73 8, 22. [] J.M. Selig. Geometric Funamentals of Robotics. Springer Verlag, New York, 25.