10. Cartesian Trajectory Planning for Robot Manipulators

Transcription

1 V. Kumar 0. Cartesian rajectory Planning for obot Manipulators 0.. Introduction Given a starting end effector position and orientation and a goal position and orientation we want to generate a smooth trajectory that taes the end effector from the start to the goal. Since a ired end effector motion is generated by controlling the joint motion, it is necessary to determine the appropriate joint movements that correspond to the ired smooth end effector motion. his can be done by computing, at each instant, the ired end effector velocity, and from that, the ired joint velocities. he latter step involves inverting the manipulator Jacobian matrix. his approach is called the resolved motion rate control (MC) scheme and is due to Whitney. he presentation here is a variation of Whitney s scheme. Our geometric approach to constructing the Jacobian in an intermediate reference frame can be used to simplify the computations. Frame 6 at time t=0 z E {F} E 2 E(t) Frame 6 at time t= x O Frame 0 y Figure : Generation of a smooth trajectory for the end effector D.E. Whitney. esolved motion rate control of manipulators and human prosthesis. IEEE rans. Man-Machine Systems. Vol. MMS-0, 969.

2 0.2. Nomenclature Let the tas be to tae the end effector from the starting position and orientation given by frame E to the goal position and orientation given by frame E 2 in seconds. he following symbols are used in the development of the MC algorithm: 0 A 6 the homogeneous transform that cribes the position and orientation of the end effector frame with respect to the base frame E the end effector frame (reference frame 6) at the start position E 2 the end effector frame (reference frame 6) at the goal position F the fixed reference frame (reference frame 0) Q i the homogeneous transformation matrix for the displacement F A Ei given by F d F 2 d 2 A E = Q =, A = 2 = E Q t the time variable that parameterizes the entire motion, t [0, ] E(t) the end effector frame at an arbitrary time t : E(0)=E, E()=E 2. H(t) the homogeneous transformation matrix at an intermediate time: F () t d() t A E() t = H() t = rajectory generation We first consider the problem of generating a smooth trajectory between E and E 2 without considering the geometry of the manipulator. he main idea is simple. If the object of interest is a point (for example, a reference point on the end effector) in Euclidean space 3, it is meaningful to follow the shortest distance path. hus the optimal trajectory is the straight line between the starting position and the goal position of the point. Unfortunately, the object of interest is a reference frame E. hus we need the generalization of a straight line to a more complicated space (the set of all reference frames). In geometry such generalizations are called geoics 2. We will show how to generate such a shortest distance trajectory between the start and goal position. 2 An airplane flying between two cities generally follows the geoic between the two cities. he geoic will be the shortest distance path and will appear curved. For example, an airplane flying from oyo to Los Angeles will -2-

3 Let H(t) be the transformation matrix that relates the position and orientation of the end effector at time t with respect to the initial reference frame E. Note that E is a fixed reference frame. he end effector frame is coincident with E at t=0. Clearly, H(Τ) relates the goal position and orientation, E 2, with respect to E. Let M = H(). Since the start and goal frames are cribed by Q and Q 2, M = (Q ) - Q 2, M = 0 2 ( d d ) 2 () where: Q = 0 d, Q 2 2 = 0 d 2 hus the trajectory of the end effector in reference frame E is given by: () t d() t where the following boundary conditions must be satisfied: H() t = (2) 0 H(0) = I 4 4, (0)=I 3 3, d(0)=0 3 (3) H() = M, ()= 2, d()= ( d 2 d ) (4) It turns out that this shortest distance trajectory 3 can be obtained by treating the rotation and translation parts of the trajectory independently. If we consider the rotation part of the trajectory, it is clear that the simplest trajectory from a given initial position, (0)=I to a new position, ()= 2, is a uniform rotation of the end effector about a fixed axis. he existence of this fly north before turning south although oyo is north of Los Angeles. If the earth had been flat, the geoic would simply be the straight line between the two cities. 3 It is clear that our discussion here is quite superficial. We have not defined our measure of distance (a suitable metric) and therefore the notion of a shortest distance trajectory is not at all obvious. It turns out there is a natural way of defining a metric on the set of all rigid body displacements and it is this metric we refer to in this presentation. -3-

4 axis is guaranteed by Euler s theorem. A simple solution for the translation part of this trajectory is the uniform translation along a straight line from d(0)=0 to d()= ( d 2 d ) in three dimensional space. It turns out that these solutions do give the shortest distance trajectory (for a class of metrics) between E and E 2. Let the angle of rotation and the axis of rotation be given by φ and u respectively. From the notes, we can easily derive the following formulae: ( ) φ=cos 2 trace ( 2 ) ( ) uˆ = 2 2 (6) 2sin φ where û is the 3 3 sew symmetric matrix corresponding to the 3 vector u. Note that there are many solutions for the angle of rotation 4 because the inverse cosine function is multivalued. If we find φ from (5) restricting the range of the inverse cosine function to the interval [0,π], we can find the axis of rotation from (6) provided φ is not either 0 or π. If 2 = I, φ=0 and (τ)=i. If the trace of the rotation matrix is -, φ=π, and from the Euler-Lexell formula: 2 = 2uu I from which u can be solved. We want the end effector to perform a uniform rotation of φ about u in τ seconds. hus the velocity of rotation must be given by the angular velocity vector: (5) ω = φ u and the rotation matrix is given by: φt φt φt () t = I cos + uu cos + uˆ sin (7) 4 For every rotation of angle φ about the axis u, we can also obtain an equivalent axis-angle representation with a rotation -φ about the axis -u. Also for every solution (u, φ), we have other solutions (u, φ+2π) for all integer values of. -4-

5 Since the translation ( d 2 d ) must be uniform and performed in the same time interval, we get: d t () t ( d ) = (8) 2 d Equations (7) and (8) cribe the displacement of the end effector from its initial position at E to the final position E 2 in the fixed reference frame E Cartesian velocities he end effector velocities can be obtained either in the end effector reference frame E (reference frame 6) or the fixed reference frame F (reference frame 0). We will determine the velocities in the end effector reference frame. he twist in the moving reference frame at any time t is given by: E t()= t H() t dh() t (9) dt A simple computation verifies that: E () t d φ ω () t = () t = U (0) dt () t () t () t ( d ) E v dd 2 d = = () t () dt he twist in the end effector frame can be transformed to any other reference frame. In the th reference frame: t = E ω, t = 6 E (2) P6 6 6 P6 6 6 v where P 6 is the sew-symmetric matrix corresponding to p 6 and A 6 = 0 6 p 6-5-

6 0.5. Joint velocities o obtain the joint velocities for the ired end effector twist given by equations (0-) or (2), one needs to premultiply the twist by the inverse of Jacobian. We have seen that letting =3 allows us to develop analytical expressions for the joint rates. Once the ired joint rates are calculated, the joints can be commanded to move at the ired rates and the end effector moves in the ired trajectory. Control algorithm. Given the starting and goal positions and orientations find the required displacement from Equation (). 2. Compute the ired trajectory from Equations (7) and (8) 3. Find the expressions for the joint rates as analytical functions of the end effector velocities in a suitable reference frame (generally, frame 3 will wor well). 4. For each time step: 4. Compute the end effector velocity (0-) 4.2 ransform the velocity to frame Obtain joint rates from the analytical expressions in Step Send the ired joint rates to the motor controllers 4.5 Wait for t seconds (sampling rate) 4.6 Go to Closed loop feedbac control he method presented above suffers from a fundamental difficulty. If there are errors in the trajectory, the control algorithm above offers no mechanism to correct for the errors. We now incorporate feedbac terms in the control algorithm to correct for errors in the trajectory. Before we do this, it is useful to first consider a simple, one-dimensional problem. Consider a single joint manipulator whose joint displacement is denoted by q, and the Cartesian displacement is denoted by x. he direct inematics equations are given by: x = f(q) (3) -6-

7 while the velocity equations are: dx df dq = dt dq dt x& = Jq& (4) We assume (as in the preceding sections) it is possible to command an arbitrary joint velocity. herefore we ignate u, our control input, to be the joint velocity: u = q& (5) We assume (as in the preceding sections) a ired trajectory that will be denoted by the superscript. In other words, the ired Cartesian trajectory x (t) and the corresponding joint trajectory q (t) are assumed to be given: x = f(q ) (6) We define the error to be the discrepancy between the actual Cartesian position, x(t), and the ired Cartesian position x (t). e(t) = x(t) - x (t) (7) We want to select the control input and drive the joints so that this error is as close to zero as possible. We would lie the error to asymptotically (in fact, exponentially) to zero. In other words, we would lie the error, e(t), to satisfy the ordinary differential equation e& + e = 0 (8) where is a positive constant. he solution to this differential equation is nown to be an exponential response whose rate of decay depends only on : e(t) = e 0 exp(- t ), where e 0 is the initial error at t=0. Differentiating (7) and substituting into (8) we get (dropping the notation denoting explicit dependence on time): ( x & x& ) + ( x x ) = 0-7-

8 Substituting from (4) and (5), we get: ( x ) + ( x x ) = 0 Ju & (9) In order to get the exponential attenuation of a constant error as guaranteed by (8), it is sufficient to select u so that (9) is satisfied. his can be done by the control law: u = J & ( x ( x x ) ) (20) If the joint velocities are driven by this control law, any initial Cartesian error, e(0), will driven to zero exponentially. he generalization of this to the multidimensional (six degree of freedom) case is not too difficult. he main thing is to define a reasonable measure of the error e(t). Following the development in (-6) above, we define the error between two frames given by X and X, E = (X) - X - I, E = 0 ( d d) I where: X = 0 d, X = 0 d. he error transform E is a 4 4 homogeneous transformation matrix that expresses the position and orientation of the ired end effector frame in the actual end effector frame.. However, the measure of the error can be captured by an orientation error vector σ and a translation error vector s. If the error vector is small, since, σˆ = I = ( + δ) I = δ is sew symmetric. he corresponding vector σ is a measure of the orientation error. (σ is zero only when =, and the norm of σ can be shown to be a norm on the set of rotations). Similarly, s = (d d ), is a measure of the translation error hus the 6 error vector, e(t), given by: -8-

9 σ e = s provi the six dimensional analog to e(t). Once again, we would lie the error vector to go exponentially to zero. In other words, we would lie the error vector to satisfy the ordinary differential equation e& + Ke = 0 (2) where K is a positive definite matrix (a real, symmetric matrix with positive eigenvalues). he solution to this differential equation is nown to be an exponential response: e = exp ( Kt) e 0 where exp is now the matrix exponential. Once again, let u, the control input be given by: u = q& he Cartesian velocity is given by the end effector twist, E t. he derivative of e is clearly given by: σ& e& = = s & E t E t Since the Jacobian matrix times the joint velocities gives the twist, we substitute into (2) to get: E ( Ju t ) + Ke = 0 (22) from which we get the control law: u ( E t Ke)0 (22) = J his control law guarantees the position and orientation errors for the end effector trajectory go exponentially to zero. -9-

10 -0-