13 Path Planning Cubic Path P 2 P 1. θ 2

Transcription

1 13 Path Planning Path planning includes three tasks: 1 Defining a geometric curve for the end-effector between two points. 2 Defining a rotational motion between two orientations. 3 Defining a time function for variation of a coordinate between two given values. All of these three definitions are called path planning. Figure 13.1 illustrates a path of the tip point of a 2R manipulator between points P 1 and P 2 to avoid two obstacles. Y θ 2 P 2 P 1 θ 1 X FIGURE A path of the tip point of a 2R manipulator to avoid two obstacles Cubic Path A cubic function is the simplest polynomial to determine the time behavior of a variable between two given values, rest-to-rest. A cubic path in joint space for the joint variable q(t), or in Cartesian space for a Cartesian coordinate q(t), between two points q(t ) and q(t f ) is q(t) =a + a 1 t + a 2 t 2 + a 3 t 3 (13.1) where a = q 1t 2 (t 3t f )+q t 2 f (3t t f ) (t f t ) 3 t t f q t f + q 1 t (t f t ) 2 (13.2) R.N. Jazar, Theory of Applied Robotics, 2nd ed., DOI 1.17/ _13, Springer Science+Business Media, LLC 21

2 Path Planning q q 1 a 1 = 6t t f (t f t ) 3 q t f + ³ t 2 f + t t f 2t 2 + q1 t ³2t 2 f t2 t t f (t f t ) 3 (13.3) and a 2 = q (3t +3t f )+q 1 ( 3t 3t f ) (t f t ) 3 ³ q1 t t f 2t 2 + t2 f + q ³2t 2 f t2 t t f (t f t ) 3 (13.4) a 3 = 2q 2q 1 + q (t f t )+q 1 (t f t ) (t f t ) 3 (13.5) q(t ) = q q(t )=q q(t f ) = q f q(t f )=qf. (13.6) Proof. A cubic polynomial has four coefficients. Therefore, it can satisfy the position and velocity constraints at the initial and final points. For simplicity, we call the value of the variable, the position, andtherateofthe variable, the velocity. Assume that the position and velocity of a variable at the initial time t and at the final time t f are given as (13.6). Substituting the boundary conditions in the position and velocity equations of the joint variable q(t) = a + a 1 t + a 2 t 2 + a 3 t 3 (13.7) q(t) = a 1 +2a 2 t +3a 3 t 2 (13.8) generates four equations for the coefficients of the path. 1 t t 2 t 3 1 2t 3t 2 1 t f t 2 f t 3 f 1 2t f 3t 2 f a a 1 a 2 a 3 = q q q f q f (13.9) Their solutions are given in (13.2) to (13.5). In case that t =,thecoefficients simplify to a = q (13.1) a 1 = q (13.11) ³ 3(q f q ) 2q + q f t f a 2 = (13.12) a 3 = t 2 f 2(q f q )+ t 3 f ³ q + qf t f. (13.13)

3 13. Path Planning 731 q. q/2.. q/1 t FIGURE Kinematics of a rest-to-rest cubic path. It is also possible to employ a time shift and search for a cubic polynomial of the form q(t) =a + a 1 (t t )+a 2 t (t t ) 2 + a 3 (t t ) 3. (13.14) Now, the boundary conditions (13.6) generate a set of equations 1 a 1 1 (t f t ) (t f t ) 2 (t f t ) 3 a 1 a 2 = 1 2(t f t ) 3(t f t ) 2 a 3 q q q f q f (13.15) with the following solutions: q a a 1 a 2 = ³ q (t f t ) 2 3q 3q f 2t q t qf +2t fq + t f qf a ³ 3 (t f t ) 3 2q 2q f t q t qf + t fq + t f qf (13.16) A disadvantage of cubic paths is the acceleration jump at boundaries that introduces infinite jerks. Example 348 Rest-to-rest cubic path. Assume q() = 1 deg, q(1) = 45 deg, and q() = q(1) =. The coefficients of the cubic path are a =1 a 1 = a 2 = 15 a 3 = 7 (13.17) that generate a path for the variable as q(t) = t 2 7t 3 deg. (13.18)

4 Path Planning q. q.. q/2 t FIGURE Kinematics of a to-rest cubic path in joint space. The path information is shown in Figure Example 349 To-rest cubic path. Assume the angle of a joint starts from θ() = 1 deg, θ() = 12 deg / s and ends at θ(2) = 45 deg, θ() =. Thecoefficients of a cubic path for this motion are: a =1 a 1 =12 a 2 = 81 2 Thekinematicsofthispathare a 3 = 29 2 (13.19) θ(t) = 1+12t +4.5t t 3 deg (13.2) θ(t) = 81t 43.5t 2 +12deg/ s (13.21) θ(t) = 81 87t deg / s 2 (13.22) and are shown graphically in Figure Example 35 Rest-to-rest path with a constant velocity in the middle. Assume we need a rest-to-rest path with a constant given velocity q = q c for t 1 <t<t 2 where t <t 1 <t 2 <t f. We show the boundary conditions to be satisfied as q(t ) = q q(t )=q q(t) = qc t 1 <t<t 2 q(t f ) = q f q(t f )=qf. (13.23) The path has three parts: rest-to, constant-velocity, and to-rest. We need an equation for the rest-to part of the motion to achieve the given velocity.

5 13. Path Planning 733 A quadratic path has three coefficients and can be utilized to satisfy three conditions. q 1 (t) = a + a 1 t + a 2 t 2 (13.24) q 1 (t) = a 1 +2a 2 t (13.25) The conditions are the initial position and velocity, and the final constant velocity. Assuming t =the conditions for the rest-to path are q 1 () = q q 1 () = q 1 (t 1 )=q c (13.26) that generate the following equations: Therefore, the rest-to path is: q = a (13.27) = a 1 (13.28) qc = 2a 2 t 1. (13.29) q 1 (t) =q + q c t 2 <t<t 1 (13.3) 2t 1 Given the specific constant velocity qc shows that the path in the middle part is: q 2 (t) = qc (13.31) q 2 (t) = qc t + C 1 t 1 <t<t 2 (13.32) The constant of integration can be found by utilizing the position condition at t = t 1. q + q c 2t 1 t 2 1 = q ct 1 + C 1 (13.33) C 1 = q 1 2 t 1q c (13.34) There are four conditions for the to-rest part of the path. Therefore, it can be calculated utilizing a cubic equation and the following boundary conditions: q 3 (t) = b + b 1 t + b 2 t 2 + b 3 t 3 (13.35) q 3 (t) = b 1 +2b 2 t +3b 3 t 2 (13.36) q 3 (t f ) = q f (13.37) q 3 (t f ) = (13.38) q 3 (t 2 ) = q 2 (t 2 )=q 2 = q ct 2 + q 1 2 t 1q c (13.39) q(t 2 ) = q 2 (t 2 )=q c (13.4)

6 Path Planning These conditions generate three equations 1 t f t 2 f t 3 f b q f 1 2t f 3t 2 f b 1 1 t 2 t 2 2 t 3 2 b 2 = qct 2 + q 1 1 2t 2 3t 2 2 t 1qc 2 b 3 qc with the following solutions: (13.41) q c b = t 2 t 2 f 2t 2 t f + t t2 f + q 2 t 3 f 3t 2t 2 f t t3 f 3t 2t 2 f +3t2 2 t f +q f t t 2 2t f t t3 f 3t 2t 2 f +3t2 2 t f (13.42) b 1 = qc 2t 2 t f + t 2 f 2t 2 t f + t t2 f +6q 2 t 2 t f t t3 f 3t 2t 2 f +3t2 2 t f t f 6t 2 q f t t3 f 3t 2t 2 f +3t2 2 t f (13.43) b 2 = qc t 2 2t f 2t 2 t f + t t2 f + q 2 3t 2 3t f t t3 f 3t 2t 2 f +3t2 2 t f +q f 3t 2 +3t f t t3 f 3t 2t 2 f +3t2 2 t f (13.44) b 3 = for q c 2t 2 t f + t t2 f q f 2 t t3 f 3t 2t 2 f +3t2 2 t f q 2 +2 t t3 f 3t 2t 2 f +3t2 2 t f (13.45) t 2 <t<t f. (13.46) A graph of the path for the following values is illustrated in Figure t 1 =.4s t 2 =.7s t f =1s q = q f =6deg qc =5deg/ s (13.47) Example 351 A quadratic path through three points. A quadratic path passing through three points (q 1,t 1 ), (q 2,t 2 ),and(q 3,t 3 ) is: q(t) = (t t 2)(t t 3 ) (t 1 t 2 )(t 1 t 3 ) q 1 + (t t 3)(t t 1 ) (t 2 t 3 )(t 2 t 1 ) q 2 + (t t 1)(t t 2 ) (t 3 t 1 )(t 3 t 1 ) q 3 (13.48)

7 13. Path Planning 735 q t FIGURE A piecewise rest-to-rest path with a constant velocity in the middle. As an example, the path passing through (1 deg, ), (25 deg,.5), and (45 deg, 1) is: q(t) = (t.5) (t 1) (t 1) t t (t.5) ( 1) (.5) (.5) 1 = t 2 19.t 4. (13.49) The velocity of the path at both ends are: q() = 47.5deg/ s (13.5) q(1) = 22.5deg/ s (13.51) 13.2 Polynomial Path The number of required conditions determines the degree of the polynomial for q = q(t). In general, a polynomial path of degree n, q(t) =a + a 1 t + a 2 t a n t n (13.52) needs n +1 conditions. The conditions may be of two types: positions at a series of points, so that the trajectory will pass through all specified points; or position, velocity, acceleration, and jerk at two points, so that the smoothness of the path can be controlled. The problem of searching for the coefficients of a polynomial reduces to a set of linear algebraic equations and may be solved numerically. However, the path planning can be simplified by splitting the whole path into a

8 Path Planning series of segments and utilizing combinations of lower order polynomials for different segments of the path. The polynomials must then be joined together to satisfy all the required boundary conditions. Example 352 Quintic path. Forcing a variable to have specific position, velocity, and acceleration at boundaries introduces six conditions: q(t ) = q q(t )=q q(t )=q q(t f ) = q f q(t f )=qf q(t f )=qf (13.53) A five degree polynomial can satisfy these conditions q(t) =a + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 (13.54) and generates a set of six equations: 1 t t 2 t 3 t 4 t 5 1 2t 3t 2 4t 3 5t 4 2 6t 12t 2 2t 3 1 t f t 2 f t 3 f t 4 f t 5 f 1 2t f 3t 2 f 4t 3 f 5t 4 f 2 6t f 12t 2 f 2t 3 f a a 1 a 2 a 3 a 4 a 5 = q q q q f q f q f (13.55) A rest-to-rest path with no acceleration at the rest positions with the following conditions: q() = 1 deg q() = q() = q(1) = 45 deg q(1) = q(1) = (13.56) can be found by solving a set of equations for the coefficients of the polynomial which shows a a 1 a 2 a 3 a 4 a 5 = a a 1 a 2 a 3 a 4 a = 1 45 (13.57). (13.58) The path equation is then equal to q(t) = t 3 525t t 5. (13.59) which is shown in Figure 13.5.

9 13. Path Planning 737 q. q/2... q/1.. q/1 t FIGURE A quintic rest-to-rest path. Example 353 A jerk zero at a start-stop path. Tomakeapathstartandstopwithzerojerk,asevendegreepolynomial q(t) =a + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 + a 6 t 6 + a 7 t 7 (13.6) and eight boundary conditions must be employed. q() = q q() = q() = q(1) = q f q(1) = q(1) =... q () =... q (1) = (13.61) Suchazerojerkstart-stoppathforq() = 1 deg and q(1) = 45 deg, can be found by solving the following set of equations for the unknown coefficients a,a 1,,a 7 which provides a a 1 a 2 a 3 a 4 a 5 a 6 a 7 = 1 45 (13.62) q(t) = t 4 294t t 6 7t 7. (13.63) A graph of this path is illustrated in Figure Figures 13.2, depicts the path of a rest-to-rest motion with no condition on the acceleration and jerk. Figure 13.5 shows an improvement by forcing

10 Path Planning q. q/2... q/1.. q/1 t FIGURE A jerk zero at start-stop path. the motion to have zero accelerations at start and stop. In Figure 13.6, the motion is forced to have zero acceleration and zero jerk at start and stop. Hence, it shows the smoothest start and stop. However, increasing the smoothness of the start and stop increases the peak value of acceleration. Example 354 Constant acceleration path. A constant acceleration path has two segments with positive and negative accelerations. Let s assume the absolute value of the positive and negative accelerations are given. q(t ) = a c (13.64) The first half of the motion has a positive acceleration that needs a second degree polynomial q 1 (t ) = a c t (13.65) q 1 (t ) = 1 2 a ct 2 + q (13.66) for <t< 1 2 t f. (13.67) The constants of integration are found based on the initial conditions. q 1 () = q q 1 () = (13.68) For the second half of the path, we may start with a second degree polynomial q 2 (t) =a + a 1 t + a 2 t t f <t<t f (13.69)

11 13. Path Planning 739 q. q/2.. q/1 t FIGURE A constant acceleration path. and impose the following boundary conditions: q 2 (t f ) = q f q 2 (t f ) = q 1 ( t f 2 ) = q 2( t f 2 )=1 8 a ct 2 f + q (13.7) These conditions generate three equations for the unknown coefficients 1 t f t 2 f 1 2t f 1 t f a a 1 = a 2 q f 1 8 a ct 2 f + q (13.71) with the following solution: a q f t f ³q a ct 2 f a 1 = 2q a ct 2 f (13.72) a 2 1 t f ³q a ct 2 f A constant acceleration path is shown in Figure 13.7 for the conditions q =1deg, q f =45deg, t f =1,anda c = 2 deg / s 2. Example 355 Point sequence path. A path can be assigned via a series of points that the variable must attain at specific times.thepointsmayalsobedefinedtoapproximateatrajectory. Consider an example path specified by four points q, q 1, q 2,andq 3,such that the points are reached at times t, t 1, t 2,andt 3 respectively. In addition to positions, we usually impose constraint on initial and final velocities and

12 Path Planning accelerations. The conditions for such a sequence of points can be q(t ) = q q(t )= q(t )= q(t 1 ) = q 1 q(t 2 ) = q 2 q(t 3 ) = q 3 q(t 3 )= q(t 3 )=. (13.73) A seven degree polynomial can be utilized to satisfy these eight conditions. q(t) = a + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 +a 5 t 5 + a 6 t 6 + a 7 t 7 (13.74) The set of equations for the unknown coefficients is 1 t t 2 t 3 t 4 t 5 t 6 t 7 1 2t 3t 2 4t 3 5t 4 6t 5 7t 6 2 6t 12t 2 2t 3 3t 4 42t 5 1 t 1 t 2 1 t 3 1 t 4 1 t 5 1 t 6 1 t t 2 t 2 2 t 3 2 t 4 2 t 5 2 t 6 2 t t 3 t 2 3 t 3 3 t 4 3 t 5 3 t 6 3 t t 3 3t 2 3 4t 3 3 5t 4 3 6t 5 3 7t t 3 12t 2 3 2t 3 3 3t t 5 3 that can be simplified to for the following example data. a a 1 a 2 a 3 a 4 a 5 a 6 a 7 a a 1 a 2 a 3 a 4 a 5 a 6 a 7 = = q q 1 q 2 q (13.75) (13.76) q() = 1 deg q() = q() = q(.4) = 2 deg q(.7) = 3 deg q(1) = 45 deg q(1) = q(1) = (13.77)

13 13. Path Planning 741. q/2 q..... q/5 q/2 t FIGURE A point sequence path. The solution for the coefficients is: a a 1 a 2 a 3 a 4 a 5 a 6 a 7 = (13.78) These coefficients generate a path as shown in Figure This method provides a continuous and differentiable function for the q variable. Continuity and differentiability of q = q(t) is an advantage that provides a continuous velocity, acceleration, and jerk. However, the number of equations increases by increasing the number of points, which needs larger data storage and increases the calculating time. Example 356 Splitting a path into a series of segments. Instead of using a single high degree polynomial for the entire trajectory, we may prefer to split the trajectory into some segments and use a series of low degree polynomials. Consider a path for the following boundary conditions: q(t ) = q q(t )= q(t )= q(t 4 ) = q 3 q(t 4 )= q(t 4 )= (13.79)

14 Path Planning whichmustalsopassthroughthreemiddlepointsgivenbelow. q(t 1 ) = q 1 q(t 2 ) = q 2 q(t 3 ) = q 3 (13.8) Let s split the entire path into four segments, namely q 1 (t), q 2 (t), q 3 (t), and q 4 (t). q 1 (t) for q(t ) <q 1 (t) <q(t 1 ) and t <t<t 1 q 2 (t) for q(t 1 ) <q 2 (t) <q(t 2 ) and t 1 <t<t 2 q 3 (t) for q(t 2 ) <q 3 (t) <q(t 3 ) and t 2 <t<t 3 q 4 (t) for q(t 3 ) <q 4 (t) <q(t 4 ) and t 3 <t<t 4 The boundary conditions for the first segment are q 1 (t ) = q q 1 (t )= q 1 (t )= q 1 (t 1 ) = q 1 (13.81) which can be satisfied by a cubic function. q 1 (t) =a + a 1 (t t )+a 2 (t t ) 2 + a 3 (t t ) 3 (13.82) The coefficients can be calculated by solving a set of equations 1 a q 1 a 1 2 a 2 = (13.83) 1 (t 1 t ) (t 1 t ) 2 (t 1 t ) 3 a 3 q 1 that provides a = q a 1 = a 2 = a 3 = q 1 q (t 1 t ) 3. (13.84) The path in the second segment must satisfy the following boundary conditions: q 2 (t 1 ) = q 1 q 2 (t 1 ) = q 1 (t 1 )=a 1 +2a 2 (t 1 t ) 2 +3a 3 (t 1 t ) 2 = q +3 q 1 q t 1 t q 2 (t 2 ) = q 2 (13.85) A quadratic polynomial will satisfy these conditions: q 2 (t) =b + b 1 t + b 2 t 2 (13.86)

15 13. Path Planning 743 The coefficients are the solutions of 1 t 1 t t 1 b b 1 1 t 2 t 2 2 b 2 = q 1 q +3 q1 q t 1 t q 2 (13.87) that provide t 2 1 2t 1 t 2 + t 2 2 b = q 2 2t 1 t 2 + t q 1 t2 2 2t 1 t 2 + t t2 2 µ t 2 t 1 q +3 q + q 1 t 1 + t 2 t + t 1 (13.88) t 1 t 1 b 1 = 2q 1 2t 1 t 2 + t q 2 t2 2 2t 1 t 2 + t t2 2 + t µ 1 + t 2 q +3 q + q 1 t 1 + t 2 t + t 1 (13.89) q 1 b 2 = 2t 1 t 2 + t t2 2 2t 1 t 2 + t t2 2 µ 1 q +3 q + q 1. (13.9) t 1 + t 2 t + t 1 The boundary conditions in the third segment are: q 3 (t 2 ) = q 2 q 3 (t 2 )= q 2 (t 2 )=b 1 +2b 2 t 2 q 3 (t 3 ) = q 3 (13.91) We can satisfy these conditions with a quadratic equation q 3 (t) =c + c 1 t + c 2 t 2 (13.92) that provides three equations for the unknown coefficients. 1 t 2 t t 2 c q 2 c 1 = b 1 +2b 2 t 2 (13.93) 1 t 3 t 2 3 c 2 q 3 The coefficients are: c = t 2 t 3 t 2 + t 3 (b 1 +2b 2 t 2 )+q 3 t 2 2 2t 2 t 3 + t t2 3 q 2 +q 2 2t 2 t 3 + t 2 3 2t 2 t 3 + t t2 3 (13.94)

16 Path Planning c 1 = t 2 + t 3 t 2 + t 3 (b 1 +2b 2 t 2 )+2q 2 t 2 2t 2 t 3 + t t2 3 2q 3 t 2 2t 2 t 3 + t t2 3 (13.95) 1 q 2 c 2 = (b 1 +2b 2 t 2 ) t 2 + t 3 2t 2 t 3 + t t2 3 q 3 + 2t 2 t 3 + t (13.96) t2 3 The boundary conditions for the fourth segment are q 4 (t 3 ) = q 3 q 4 (t 3 )= q 3 (t 3 )=c 1 +2c 2 t 3 q 4 (t 4 ) = q 4 q 4 (t 4 )= q 4 (t 4 )= (13.97) which needs a fourth degree polynomial to be satisfied. q 4 (t) =d + d 1 t + d 2 t 2 + d 3 t 3 + d 4 t 4 (13.98) Substituting the boundary conditions generates a set of four equations for the coefficient. 1 t 3 t 2 3 t 3 3 t 4 3 d q 3 1 2t 2 3t 2 2 4t 3 2 d 1 1 t 4 t 2 4 t 3 4 t 4 4 d 2 1 2t 4 3t 2 4 4t 3 4 d 3 = c 1 +2c 2 t 3 q 4 (13.99) 2 6t 4 12t 2 4 d 4 As an example, a set of conditions given by t = t 1 =.4 t 2 =.7 t 3 =.9 t 4 =1 (13.1) q() = 1 deg q() = q() = q(.4) = 2 deg q(.7) = 3 deg q(.9) = 35 deg q(1) = 45 deg q i (1) = q(1) = (13.11) provides q 1 (t) = t 3 (13.12) q 2 (t) = t 172.2t 2 (13.13) q 3 (t) = t t 2 (13.14) q 4 (t) = t +1295t t t 4 (13.15)

17 13. Path Planning 745 q t FIGURE Spliting a path into a series of segments. which is shown in Figure 13.9 graphically. The disadvantage of the segment method is the lack of a smooth overall path and having a discontinuous acceleration. To increase the smoothness of the path, we need to use higher degree polynomials and put constraints on acceleration and possibly jerk. Equations (13.12)-(13.15) indicate that: q 1 (t 1 ) = 375 q 2 (t 1 )= (13.16) q 2 (t 2 ) = q 3 (t 2 ) = (13.17) q 3 (t 3 ) = q 4 (t 3 ) = 79 (13.18) q 1 (t 1 ) 6= q 2 (t 1 ) q 2 (t 2 ) 6= q 3 (t 2 ) q 3 (t 3 ) 6= q 4 (t 3 ) (13.19) Therefore, the acceleration of the path is not continuos at the connection points and show a finite jump. A jump in acceleration introduces an infinity jerk. Having continuos acceleration is the minimum requirement for smoothness of a path. A piecewise path with continuous acceleration is called spline. Example 357 F Least-squares polynomial. When the number of points to approximate a trajectory is too large, we may use a low degree polynomial to pass close to the points. Least-squares is an applied method to determine the coefficients of a selected polynomial to approximate the path. Consider a path with N given points, p i = p(t i ) i =1, 2, 3,,N (13.11) and a polynomial of degree n that is supposed to approximate the path. If N = n +1 then the polynomial passes exactly through all given points. To

18 Path Planning work with low degree polynomials, we choose n<n+1. q = a + a 1 t + a 2 t a n t n (13.111) Having the N points (13.11) and the polynomial (13.111), we define an error e i at t i. e i = p i q i = p i a a 1 t i a 2 t 2 i a n t n i (13.112) Sum of e 2 i for all points p i is the total error e. NX NX e = e 2 i = pi a a 1 t i a 2 t 2 i a n t n 2 i (13.113) i=1 i=1 The minimum error e provides the best approximate polynomial (13.111). At the minimum, all the partial derivatives e/ a, e/ a 1,, e/ a n vanish. These conditions generate n +1 equations: e a = 2 e a 1 = 2 NX pi a a 1 t i a 2 t 2 i a n t n i = i=1 NX t i pi a a 1 t i a 2 t 2 i a n t n i = i=1 e NX = 2 a n i=1 t n i pi a a 1 t i a 2 t 2 i a n t n i = (13.114) Dividing each equation by 2 and rearrangement gives n + 1 equations to be simultaneously solved for the coefficients a i, i =1, 2,,n. X N a i=1 X N a i=1 a N + a 1 t i + a 1 t n i + a 1 N X i=1 X N i=1 N X i=1 t i + + a n t 2 i + + a n t n+1 i + + a n N X i=1 X N i=1 NX i=1 t n i = t n+1 i = t 2n i = NX i=1 p i NX t i p i i=1 NX t n i p i (13.115) i=1 Rearrangement makes a set of linear equations to be solved for a i, i = 1, 2,,n [A] a = b (13.116)

19 13. Path Planning 747 where, [A] = P N N i=1 t P N P N i i=1 t2 i i=1 tn i P N i=1 t P N P N P i i=1 t2 i i=1 t3 i N i=1 tn+1 i (13.117) P N P N P N P i=1 tn i i=1 tn+1 i i=1 tn+2 i N i=1 t2n i P N a i=1 p i P a 1 N i=1 a = a 2 b = t ip i. (13.118) a n P N i=1 t2 i p i P N i=1 tn i p i 13.3 F Non-Polynomial Path Planning A path of motion in either joint or Cartesian spaces may be defined based on different mathematical functions. Harmonic and cycloid functions are the most common paths. q(t) = a + a 1 cos a 2 t + a 3 sin a 2 t (13.119) q(t) = a + a 1 t a 2 sin a 3 t (13.12) However, we may also use other function approximate methods such as Fourier, q(t) = A 2 + X [A n cos (nx)+b n sin (nx)] (13.121) Legendre, A = 1 π A n = 1 π B n = 1 π n=1 Z π π Z π π Z π π q(t)dt (13.122) q(t)cos(nx) dt (13.123) q(t)sin(nx) dt (13.124) q n (t) = L i (t) = nx L i (t)q(t i ) (13.125) i= ny j=,j6=i t t j t i t j i =, 1, 2,,n (13.126)

20 Path Planning q. q/2.. q/1 t FIGURE A harmonic path. Chebyshev. q n+1 (t) = 2tq n (t) q n 1 (t) (13.127) q (t) = 1 q 1 (t) =t (13.128) Example 358 Harmonic path. Consider a harmonic path between two points q(t ) and q(t f ) with the rest-to-rest boundary conditions. q(t) =a + a 1 cos a 2 t + a 3 sin a 2 t (13.129) q(t ) = q q(t )= q(t f ) = q f q(t f )= (13.13) Applying the conditions to the harmonic equation (13.129) provides the following solution: q(t) = 1 µ q f + q (q f q )cos π (t t ). (13.131) 2 t f t A plot of the solution is depicted in Figure 13.1 for the following numerical values: t = t f =1 q = 1deg q = q f = 45deg qf =. (13.132)

21 13. Path Planning 749 q. q/2.. q/1 t FIGURE A cycloid path. Example 359 Acycloidpath. A cycloid path between two points q(t ) and q(t f ) with rest-to-rest boundary conditions is: q(t) =q + q f q π q(t ) = q q(t )= q(t f ) = q f q(t f )= (13.133) µ π (t t ) 1 t f t 2 sin 2π (t t ) t f t (13.134) A plot of the cycloid path is illustrated in Figure for the following numerical values: t = t f =1 q = 1deg q = q f = 45deg qf =. (13.135) Comparing Figure with 13.5 indicates that the main kinematic characteristics of a cycloid path are similar to quintic rest-to-rest path Manipulator Motion by Joint Path Having the joint variables as functions of time, and employing the forward kinematics of manipulators, allows us to calculate the path of motion for the end-effector.

22 Path Planning Example 36 2R manipulator motion based on joints path. Assume that we have calculated the paths of the two joints of a 2R planar manipulator according to cubic functions, and they are: θ 1 (t) = t 2 7t 3 deg (13.136) θ 2 (t) = t 3 525t t 5 deg (13.137) The joints paths satisfy the following conditions: θ 1 () = 1 deg θ1 () = θ 1 (1) = 45 deg θ1 (1) = (13.138) θ 2 () = 1 deg θ2 () = θ2 () = θ 2 (1) = 45 deg θ2 (1) = θ2 (1) = (13.139) The forward kinematics of a 2R manipulator are found in Example 141 as below. T 2 = T 1 1 T 2 = c (θ 1 + θ 2 ) s (θ 1 + θ 2 ) l 1 cθ 1 + l 2 c (θ 1 + θ 2 ) s (θ 1 + θ 2 ) c (θ 1 + θ 2 ) l 1 sθ 1 + l 2 s (θ 1 + θ 2 ) 1 1 (13.14). The fourth column of T 2 indicates the Cartesian position of the tip point of the manipulator in the base frame. Therefore, the X and Y components of the tip point are: X = l 1 cos θ 1 + l 2 cos (θ 1 + θ 2 ) (13.141) Y = l 1 sin θ 1 + l 2 sin (θ 1 + θ 2 ) (13.142) Substituting θ 1 and θ 2 from (13.136) and (13.137) provides the time variation of the position of the tip point. These variations, for l 1 = l 2 =1m are shown in Figure 13.12, while the configurations of the manipulator at initial and final positions are shown in Figure As long as the joint variables are defined and given as functions of time, it is immaterial which joint turns first. The joint variables are relative coordinates and the final configuration of the robot would be the same. They caneventurntogether. Moving a robot by applying a set of joint paths is not always a proper method. In case the joint variables are not monotonic in time and are fluctuating, defining a joint path is more complicated. Furthermore, it is not easy to move the end-effector of a robot on a desired geometric path by defining joint paths.

23 13. Path Planning 751 X Y t FIGURE X and Y components of the tip point position of a 2R planar manipulator Y θ 1 θ 2 θ 1 θ X FIGURE Configuration of a 2R manipulator at initial and final positions.

24 l Path Planning Y P 2 R 2 R 1 θ 2 l 2 P 1 ϕ l θ 1 X FIGURE A 2R robot moveing along a given line. Example 361 A 2R robot moving along a line. Let us consider a 2R manipulator with l 1 = l 2 =.25 (13.143) that its tip point is supposed to move on a given line Y = f(x) as is shown in Figure Y =.25998X (13.144) Assume that the first angle is moving between 45 deg and 135 deg in 1 sec based on a cubic path. 45 deg <θ 1 < 135 deg (13.145) θ 1 = π 4 + 3π 2 t2 π 1 t3 <t<1 sec (13.146) The elbow joint R will move on a circle and at the beginning is at: X R1 =.25 cos π 4 Y R1 =.25 sin π 4 = (13.147) = (13.148) Point P 1 must be on the line (13.144) at a distance d =.25 from R 1. q d = (X ) 2 +(Y ) 2 q = (X ) 2 +(.25998X ) 2 =.25 (13.149)

25 13. Path Planning 753 Therefore, P 1 is at: X P1 = Y P1 = (13.15) and initial values of angles ϕ and θ 2 are: ϕ = arctan Y P 1 Y R =arctan X P1 X R = rad deg (13.151) θ 2 = θ 1 ϕ = π = rad deg (13.152) The elbow joint R at the final position is at: X R2 =.25 cos 3π 4 Y R2 =.25 sin 3π 4 = (13.153) = (13.154) Point P 2 must be on the line (13.144) at a distance d =.25 from R 2. q d = (X ) 2 +(Y ) 2 q = (X ) 2 +(.25998X ) 2 =.25 (13.155) Therefore, P 2 is at: X P2 = Y P2 = (13.156) and final values of angles ϕ and θ 2 are: ϕ = arctan Y P 2 Y R =arctan X P2 X R = rad deg (13.157) θ 2 = θ 1 ϕ = 3π = rad deg (13.158) To determine θ 2 during the motion, we should follow the same procedure. Let us find the position of the elbow joint R as a function of θ 1. X R =.25 cos θ 1 Y R =.25 sin θ 1 (13.159)

26 Path Planning The tip point P must be on the line (13.144) at a distance d =.25 from the elbow joint R. d = = q (X P.25 cos θ 1 ) 2 +(Y P.25 sin θ 1 ) 2 q (X P.25 cos θ 1 ) 2 +(.25998X P sin θ 1 ) 2 =.25 (13.16) Solution of this equation for X P and substitution in (13.144) provides the coordinates (X P,Y P ) of the tip point P during the motion. Then, the angle ϕ and θ 2 would be: ϕ = arctan Y P Y R =arctan Y P.25 sin θ 1 X P X R X P.25 cos θ 1 (13.161) θ 2 = θ 1 ϕ = θ 1 arctan Y P.25 sin θ 1 X P.25 cos θ 1 (13.162) Therefore, to make the point P moving along the line (13.144), while θ 1 is varying as (13.146), the angle θ 2 must vary according to (13.162) Cartesian Path Cartesian path planning is mathematically similar to joint space path planning. Having the coordinates of the start and stop point of the end-effector as P = P (X,Y,Z ) P 1 = P 1 (X 1,Y 1,Z 1 ) (13.163) we can connect the points by a geometric space curve Z = Z (X) Y = Y (X) (13.164) where, X (t )=X X (t f )=X f. (13.165) Then, we may define a time path for one of the coordinates, say X, between P and P f to determine the kinematic behavior of the other coordinates on the geometric path (13.164). A point-to-point path can also be planned by connecting the points, or designing a path to pass close to but not necessarily through the points. A practical method is to design a path utilizing straight lines with constant velocity, and deform the corners to have a smooth transition. The path connecting points r to r 2, and passing close to the corner r 1

27 13. Path Planning 755 r 1 r(t 1 -t ) r(t 1 +t ) r 2 r FIGURE Transition parabola between two line segments as a path in Cartesian space. on a transition curve, can be designed by a piecewise motion. r(t) =r 1 t 1 t (r 1 r ) t t t 1 t t 1 t r(t) =r 1 (t t t 1 ) 2 4t (t 1 t ) (r 1 r ) + (t + t t 1 ) 2 4t (t 2 t 1 ) (r 2 r 1 ) t 1 t t t 1 + t r(t) =r 1 t 1 t (r 2 r 1 ) t 2 t 1 t 1 + t t t 2 (13.166) The path starts from r at time t and moves with constant velocity v 1 = r 1 r t 1 t along a line until a point at switching time t 1 t.atthistime,the path switches to a constant acceleration parabola. At another switching pointattimet 1 + t, the path switches to the second line and moves with constant velocity v 2 = r 2 r 1 t 2 t 1 toward the destination at point r 2.Thetime t 1 t istherequiredtimetomovefromr to r 1 and t 2 t 1 is the required time to move from r 1 to r 2, if there were no transition path. The path is shown in Figure schematically. Proof. The first line segment starts from a point r at time t and, without any deformation, it arrives at point r 1 at time t 1 via a constant velocity. The second line ends with a constant velocity at point r 2 at time t 2 and, without deformation, it would start from point r 1 at time t 1. r 1 t 1 t (r 1 r ) t t t 1 t r(t) = 1 t r 1 t (13.167) 1 t (r 2 r 1 ) t 1 t t 2 t 2 t 1

28 Path Planning We introduce an interval time t before arriving at r 1 to switch from the line to a transition curve. The transition curve is then between times t 1 t and t 1 + t. The simplest transition curve is a parabola, which, at the end points, has the same speed as the lines. The boundary positions of the transition curve on the first and second lines are respectively at where, r(t 1 t ) = r 1 t δ 1 t 1 t (13.168) r(t 1 + t ) = r 1 + t δ 2 t 2 t 1 (13.169) δ 1 = r 1 r (13.17) δ 2 = r 2 r 1. (13.171) The velocity at the beginning and final points of the transition curve are respectively equal to: ṙ(t 1 t ) = ṙ(t 1 + t ) = 1 δ 1 t 1 t (13.172) 1 δ 2 t 2 t 1 (13.173) Assume the acceleration of motion along the transition curve is constant r(t) = r c = const (13.174) and therefore, the transition curve after integration is equal to r(t) =r(t 1 t )+(t t 1 + t )ṙ(t 1 t )+ 1 2 (t t 1 + t ) 2 r c. (13.175) Substituting (13.168) and (13.172) provides r(t) =r 1 + t t 1 t 1 t δ r c(t t 1 + t ) 2. (13.176) The transition curve r(t) must be at the end point when t = t 1 + t r(t 1 + t )=r 1 + t δ 2 = r 1 + t δ 1 +2 r c t 2 1 (13.177) t 2 t 1 t 1 t therefore, the acceleration on the curve must be r c = 1 2t µ δ2 t 2 t 1 δ 1 t 1 t. (13.178)

29 13. Path Planning 757 Hence, the curve equation becomes (t t t 1 ) 2 r(t) =r 1 δ 1 4t (t 1 t ) + δ (t + t t 1 ) 2 2 4t (t 2 t 1 ) (13.179) showing that the path between r and r 2 has a piecewise character given in (13.166). A Cartesian path followed by the manipulator, plus the time profile along the path, specify the position and orientation of the end frame. Issues in Cartesian path planning include attaining a specific target from an initial starting point, avoiding obstacles, and staying within manipulator capabilities. A path is modeled by n points called control points. The control points are connected via straight lines and the transient parabolas will be implemented to exclude the sharp corners. An alternative method is applying an interpolating or approximating method, such as least-squared, to design a continuous path over the control points, or close to them. Example 362 A path in 2D Cartesian space. Consider a line in the XY plane connecting (1, ) and (1, 1), and another line connecting (1, 1) and (, 1). Assume that the time is zero at (1, ), is t =1secat (1, 1), andist =2secat (, 1). For an interval time t =.1sec, the position vector at control points are where r = î (13.18) r 1 = î +ĵ (13.181) r 2 = ĵ (13.182) r(t 1 t ) = r 1 t δ 1 =î + µ1 t t 1 t 1 r(t 1 + t ) = r 1 + t δ 2 = µ1 t t 2 t 2 ĵ (13.183) î +ĵ (13.184) δ 1 = r 1 r =ĵ (13.185) δ 2 = r 2 r 1 = î. (13.186) The path of motion is then expressed by the following piecewise function as shown in Figure 13.16: î + tĵ t.9 r(t) = ³1 (t.9)2.4 î + ³1 (t 1.1)2.4 ĵ.9 t 1.1 (13.187) (2 t)î +ĵ 1.1 t 2

30 Path Planning Y X FIGURE A transition parabola connecting two lines. The velocity of motion along the path is also a piecewise function given below. ĵ t.9 t.9 ṙ(t) =.2 î t ĵ.9 t 1.1 (13.188) î 1.1 t 2 Example 363 A 2R manipulator following a line. Assume the 2R manipulator in Figure has l 1 = l 2 =.25 (13.189) and its tip point is supposed to move on a given line Y = f(x). The manipulator moves form P 1 to P 2 in 1 sec. Y =.25998X (13.19) X P1 = Y P1 = (13.191) X P2 = Y P2 = (13.192) Let us define a rest-to-rest cubic path for X. X = t t 3 (13.193) We determine the equation of Y as a function of t by substituting X = X(t) in the line equation (13.19). Y = t t (13.194)

31 13. Path Planning F Rotational Path Consider an end-effector frame to have a rotation matrix G R at an initial orientation at time t. The end-effector must be at a final orientation G R f at time t f.therotational path is defined by the angle-axis rotation matrix Rû,φ R û,φ = R f = G R T G R f (13.195) that transforms the end-effector frame from the final orientation G R f to the initial orientation G R. The axis of rotation û is defined by a unit vector expressed in the initial frame. Therefore, the desired rotation matrix for going from initial to the final orientation, would be R Tû,φ = G R T f G R. (13.196) Keeping û constant, we can define an angular path for ϕ to vary R Tû,φ from G R to G R f at t f. To control a rotation, we may define a series of control orientations G R 1, G R 2,, G R n between the initial and final orientations, and rotate the end-effector frame through the control orientations. When there is a control orientation G R 1 between the initial and final orientations, then the initial orientation G R transforms to the control orientation G R 1 using an angleaxis rotation R û,φ, and then it transforms from the control orientation G R 1 to the final orientation using a second-angle axis rotation R 1û,φ 1. R û,φ = G R T G R 1 (13.197) R 1û,φ 1 = G R1 T G R f (13.198) Proof. According to the Rodriguez rotation formula (3.4), R f = R û,φ = I cos φ + û û T vers φ + ũ sin φ (13.199) the angle and axis that transforms a frame B f to another frame B are found from cos φ = 1 tr R f 1 (13.2) 2 ũ 1 = R f R T f. (13.21) 2sinφ If G R is the rotation matrix from B to the global frame G, and G R f is the rotation matrix from B f to G, then G R f = G R R f (13.22) and therefore, R f = R û,φ = G R T G R f. (13.23)

32 Path Planning We define a linearly time dependent rotation matrix by varying the angle of rotation about the axis of rotation R f (t) = R û,( t t (13.24) t f t )φ = r 11(t) r 12 (t) r 13 (t) r 21 (t) r 22 (t) r 23 (t) t t t f r 31 (t) r 32 (t) r 33 (t) where, t is the time when the end-effector frame is at orientation G R and t f is the time at which the end-effector frame is at orientation G R f,and µ µ t r 11 (t) = u 2 t t t 1 vers φ +cos φ t f t t f t µ µ t t t t r 21 (t) = u 1 u 2 vers φ + u 3 sin φ t f t t f t µ t t r 31 (t) = u 1 u 3 vers t f t µ t t r 12 (t) = u 1 u 2 vers t f t µ t r 22 (t) = u 2 t 2 vers t f t µ t t r 32 (t) = u 2 u 3 vers t f t µ t t r 13 (t) = u 1 u 3 vers t f t µ t t r 23 (t) = u 2 u 3 vers t f t µ t r 33 (t) = u 2 t 3 vers t f t φ u 2 sin µ t t t f t µ t t φ u 3 sin t f t µ t t φ +cos φ + u 1 sin φ t f t µ t t t f t µ t t φ + u 2 sin t f t φ u 1 sin φ +cos µ t t µ t t t f t t f t φ (13.25) φ φ (13.26) φ φ φ. (13.27) The matrix R f (t) can turn the final frame about the axis of rotation û onto the initial frame, and therefore, G R f = G R R f (t). (13.28) If there is a control orientation frame G R 1 between the initial and final orientations, then G R 1 = G R R 1 (13.29) G R f = G R 1 1 R f (13.21)

33 13. Path Planning 761 and therefore, R û,φ = R 1 = G R T G R 1 (13.211) R 1û,φ 1 = 1 R f = G R1 T G R f. (13.212) The rotation matrices R 1 and 1 R f may be defined as linearly time varying rotation matrices by R 1 (t) =R û,( t t t t 1 t )φ t t 1 1 (13.213) R f (t) =R 1û,( t t 1 t t f t )φ 1 t t f. 1 1 Using these variable matrices, we can turn the end-effector frame from the initial orientation G R about û to achieve the control orientation G R 1,and then turn the end-effector frame about 1 û to achieve the final orientation G R f. Following the parabola transition technique of section 13.5, we may define an orientation path connecting G R and G R f, and passing close to the corner orientation G R 1 on a transient rotation path. The path starts from G R at time t and turns with constant angular velocity along an axis until t = t 1 t. At this time, the path switches to a rotational parabolic path with constant angular acceleration. At another switching orientation at time t = t 1 + t, the path switches to the second path and turns with constant velocity toward the destination orientation G R f.thetimet 1 t istherequiredtimetomovefrom G R to G R 1,andt 2 t 1 is the required time to move from G R 1 to G R f if there were no transition path. We introduce an interval time t before arriving at orientation G R 1 to switch from the first path segment to a transition path. The transition path is then between times t 1 t and t 1 + t. At the second switching orientation, the transition path ends at the same angular velocity as the third path segment. The boundary positions of the transition path between the first and third segments are respectively G R 1 (t 1 t ) = G R R 1 (t 1 t ) = G R R û,(1 t t 1 t )φ t = t 1 t (13.214) G R f (t 1 + t ) = G R 1 1 R f (t 1 + t ) = G R 1 R 1û,( t t f t )φ t = t 1 + t. (13.215) 1 1 The transition path is then equal to µ R t (t) = G R t1 t t R 1 2t (t t t 1 ) 2 Ã! 1 (t + t t 1 ) 2 4t R f (t 1 t ) 4t (t f t 1 ) = G R R û,( t 1 t t 2t (t t t 1 ) 2 R 4t (t 1 t ) )φ 1û,( (t+t t 1) 2 4t (t f t 1) )φ 1 t 1 t t t 1 + t (13.216)

34 Path Planning and the entire path is: R(t) = R 1 (t) =R û,( t t t 1 t )φ t t t 1 t R(t) =R t (t) t 1 t t t 1 + t (13.217) R(t) = 1 R f (t) =R 1û,( t t 1 t f t )φ 1 1 t 1 + t t t 2 Example 364 Rotation about Z-axis. Consider a body B which is initially coincident with the global coordinate frame G at t =. So, its initial transformation matrix is an identity. G R 1 = I (13.218) B is suppose to be at G R 2 after 1 sec. G R 2 = 1 1 (13.219) 1 The axis of rotation 2 R 1 is the Z-axis, and the angle of rotation is π. The transformation matrix between the initial and final orientations of B 1 and B 2 is: 2 R 1 = G R1 T G R 2 = 1 1 (13.22) 1 Let us define a cubic rest-to-rest path for the angle of rotation α. α = 3π 1 t2 π 5 t3 (13.221) The angular path of B between B 1 an B 2 is: 2 R 1 = = cos α sin α sin α cos α 1 (13.222) cos 3π 1 t2 π 5 t3 sin 3π 1 t2 π 5 t3 sin 3π 1 t2 π 5 t3 cos 3π 1 t2 π 5 t3 1 Example 365 Rotation about X-axis. AbodyB is initially at 1 G R 1 = cos π sin π 1 1 sin π cos π 1 1 (13.223)

35 13. Path Planning 763 The body is supposed to be at G R 2 in 1 sec. 1 G R 2 = cos π sin π 2 2 sin π cos π 2 2 (13.224) The axis of rotation 2 R 1 is the X-axis, and the angle of rotation is 2 5 π =.Wedefine a cubic rest-to-rest path for the angle of rotation γ π 2 π 1 γ = π 1 + 3πt2 25 πt3 (13.225) 125 to determine the angular path of B between G an B 2 is: G R 2 = 1 cosγ sin γ (13.226) sinγ cos γ At any time t, the body B with respect to B 1 is at 1 R 2. 1 R 2 = 1 R G G R 2 (13.227) = cos γ.39 sin γ.39 cos γ.951 sin γ.39 cos γ sin γ.951 cos γ.39 sin γ 13.7 Manipulator Motion by End-Effector Path Cartesian path planning is the most natural application of path planning. Considering the pick and place motion as the main job of industrial robot, we have to determine a desired geometric path for the end-effector in the 3-dimenssional Cartesian space of the base frame. We may then define a time path for one of the coordinates, say X, and determine the time history of the other coordinates by using the geometric path. Having the time functions of the coordinates of the end-effector, we can determine the velocity, acceleration and jerk behavior of the end-effector. Inverse kinematics will determine the kinematics of joint variables. Substituting the joint variables position, velocity, and acceleration in the dynamic equations of motion provide the required actuators torque or force to move the end-effector on the desired path with the planned kinematics. ThegeometricCartesianpathisanappliedmethodofpathplanningin robotics, because it can control the level of force and jerk inserted by the hand of a robot to the carrying object. Path planning in Cartesian space also determines the geometric constraints of the external world. However, a Cartesian path needs inverse kinematics to determine the time history of the joint variables.

36 Path Planning Y X FIGURE Illustration of a 2R panipulator when the tip point moves on a straight line y =1.5. Example 366 Joint path for a designed Cartesian path. Consider a rest-to-rest Cartesian path from point (1, 1.5) to point ( 1, 1.5) on a straight line Y =1.5. A cubic polynomial can satisfy the position and velocity constraints at initial and final points. X() = X =1 Ẋ() = Ẋ = X(1) = X f = 1 Ẋ(1) = Ẋf = (13.228) The coefficients of the polynomial are and the Cartesian path is: a =1 a 1 = a 2 = 6 a 3 =4 (13.229) X = 1 6t 2 +4t 3 (13.23) Y = 1.5 (13.231) The inverse kinematics of a 2R planar manipulator is calculated in Example 184 as s (l 1 + l 2 ) 2 (X θ 2 = ±2atan2 2 + Y 2 ) (X 2 + Y 2 ) (l 1 l 2 ) 2 (13.232) θ 1 = atan2 X (l 1 + l 2 cos θ 2 )+Yl 2 sin θ 2 Y (l 1 + l 2 cos θ 2 ) Xl 2 sin θ 2 (13.233) where the sign (±) indicates the elbow-up and elbow-down configurations of the manipulator. Depending on the initial configuration at point (1, 1.5),

37 13. Path Planning 765 the manipulator is supposed to stay in that configuration. Let s consider an elbow-up configuration. Therefore, we accept only those values of the joint valuables that belong to the elbow-up configuration. Substituting (13.23) and (13.231) in (13.232) and (13.233) provides the path in joint space. s (l 1 + l 2 ) 2 (4t θ 2 = ±2atan2 3 6t ) 2 (4t 3 6t 2 +1) 2 (l 1 l 2 ) 2 (13.234) 1 6t 2 +4t 3 (l 1 + l 2 cos θ 2 )+1.5l 2 sin θ 2 θ 1 = atan2 1.5(l 1 + l 2 cos θ 2 ) (1 6t 2 +4t 3 (13.235) ) l 2 sin θ 2 A graphical illustration of the manipulator at every 1/3 th of the total time is shown in Figure Example 367 A 2R manipulator on a line. Consider the 2R manipulator of Figure with l 1 = l 2 =.25 m (13.236) that its tip point is supposed to move on a given line between P 1 and P 2 in 1 sec. Y =.25998X (13.237) X P1 = Y P1 = (13.238) X P2 =.282 Y P2 = (13.239) Defining a rest-to-rest cubic path for X, we determine the Cartesian path of the tip point. X = t t 3 (13.24) Y =.22848t t (13.241) The kinematics of the tip point are shown in Figures to Employing the inverse kinematics of equations (6.39) and (6.42), we find the variation of the joint angles as are shown in Figure Let us divide the total time of the motion in n =4equal intervals. The configuration of the manipulator at each time step are shown in Figure Example 368 A 2R manipulator on a line with no end acceleration. Consider the 2R manipulator of Figure with l 1 = l 2 =.25 m (13.242) that its tip point is supposed to move on a given line Y =.25998X (13.243)

38 Path Planning Y [m] X t [s] FIGURE Cartesian coordinates of the tip point versus time. [m/s] Y. t [s] X. FIGURE Components of the tip point velocity versus time.

39 13. Path Planning 767 [m/s 2 ] Ẏ. t [s] Ẋ. FIGURE Components of the tip point acceleration versus time. deg θ 1 θ 2 t FIGURE The variation of joint angles of the 2R manipulator.

40 Path Planning Y X FIGURE The configuration of the 2R manipulator at 42 equal time steps. between P 1 and P 2 in 1 sec. X P1 = Y P1 = (13.244) X P2 =.282 Y P2 = (13.245) Let us define a quintic path for X to apply a zero acceleration at both ends. X = t t t 5 (13.246) Substituting X in the line equation (13.243), we also determine the variation of Y. Y = t t t 5 (13.247) Using the Cartesian components (13.246) and (13.247), we determine the kinematics of the tip point as are shown in Figures to Using Equations (6.39) and (6.42), we find the variation of the joint angles as are shown in Figure Example 369 A 2R manipulator on a line with no end acceleration. Consider the 2R manipulator of Figure with equal arms length. l 1 = l 2 =.25 m (13.248) The tip point is supposed to move from P 1 to P 2 in 1 sec. X P1 = Y P1 = (13.249) X P2 =.282 Y P2 = (13.25)

41 13. Path Planning 769 Y [m] X t [s] FIGURE Cartesian coordinates of the tip point versus time on a no end acceleration path. [m/s] Y. t [s] X. FIGURE Components of the tip point velocity versus time on a no end acceleration path.

42 Path Planning [m/s 2 ] Ẏ. t [s] Ẋ. FIGURE Components of the tip point acceleration versus time on a no end acceleration path. deg θ 1 θ 2 t FIGURE The variation of joint angles of the 2R manipulator on a no end acceleration path.

43 l Path Planning 771 Y P 2 Forbidden zone R 2 R 1 θ 2 l 2 P 1 ϕ θ 1 X FIGURE A few circular paths between P 1 and P 2 to go around forbidden zone at P 3. However, there is a circular forbidden zone at point P 3, where the tip point cannot pass. X P3 = Y P3 =.3271 (13.251) (X X P3 ) 2 +(Y Y P3 ) 2 =.25 2 (13.252) To find a path between P 1 and P 2 to go around P 3,letuschooseacircular arc with a center on the bisector of P 1 P 2. Figure depicts a few optional paths. The arc must be in the working space of the manipulator, which is a circle ring about the base point. (l 1 l 2 ) 2 < X 2 + Y 2 < (l 1 + l 2 ) 2 (13.253) < X 2 + Y 2 <.5 2 (13.254) The center of the circular path should be on the following line. Y Y P3 = (X X P3 ) (13.255) Let us pick a point P C to be the center of the circular path at: Therefore, the equation of the path is: X C =.1 Y C =.6 (13.256) (X X C ) 2 +(Y Y C ) 2 =.45 2 (13.257)

44 Path Planning Y [m] X t [s] FIGURE Cartesian coordinates of the tip point versus time on a circular path. This path is shown in Figure with a dashed line. We use a quintic time-path for X to apply a zero acceleration at both ends. X = t t t 5 (13.258) Substituting X in the path equation (13.257), we determine the time-path of Y. q Y = Y C (X X C ) 2 (13.259) The kinematics of the tip point are shown in Figures to Equations (6.39) and (6.42), provides the joint angles as are shown in Figure The configuration of the manipulator at 42 equal time steps are shown in Figure Example 37 Articulated manipulator on a line. Figure illustrates an articulated manipulator. Assume that l 1 =.5m l 2 =1.m l 3 =1.m. (13.26) The tip point of the manipulator is supposed to move from point P 1 to P 2 in 1 sec. r P1 = 1.5. r P2 = (13.261) Using a quintic path for X, we find the following function to express the time variation of X. X =1.5.25t t 4.15t 5 (13.262)

45 13. Path Planning 773 [m/s] Y. t [s] X. FIGURE Components of the tip point velocity versus time on a circular path. [m/s 2 ] Ẏ. Ẋ. t [s] FIGURE Components of the tip point acceleration versus time on a circular path.

46 Path Planning deg θ 1 θ 2 t FIGURE Y X FIGURE The configuration of the 2R manipulator at 42 equal time steps on a circular path.

47 13. Path Planning 775 θ 1 x 4 x 3 z 3 z4 l 3 z x 2 P θ 3 z 2 x 1 l 2 l 1 θ 2 z 1 y x FIGURE An articulated manipulator. Let us connect P 1 and P 2 by a straight line and determine the time variation of Y and Z. Y = Y P1 + Y P 2 Y P1 (X X P1 ) X P2 X P1 =.1t 3.15t 4 +.6t 5 (13.263) Z = Z P1 + Z P 2 Z P1 (X X P1 ) X P2 X P1 = 1+.5t 3.75t 4 +.3t 5 (13.264) Using the inverse kinematic equations, we can determine the time history of joint variables of the manipulator as are shown in Figure µ l1 Z + l 2 sin θ 2 θ 3 =arccos θ 2 (13.265) l 3 θ 2 = 2 arctan C 2 + p C2 2 C 1C 3 C 1 (13.266) arctan Y X θ 1 = X arctan Y X + π X < (13.267)

48 Path Planning deg θ 2 θ 1 θ 3 t FIGURE The time history of joint variables of an articulated manipulator. C 1 = l1 2 2l 1 Z + l l 2X l3 2 + X2 cos θ 1 cos 2 + Z 2 (13.268) θ 1 C 2 = 2l 1 l 2 2l 2 Z (13.269) C 3 = l1 2 2l 1 Z + l2 2 2l 2X l3 2 + X2 cos θ 1 cos 2 + Z 2 (13.27) θ 1

49 13. Path Planning Summary A serial robot may be assumed as a variable geometrical chain of links that relates the configuration of its end-effector to the Cartesian coordinate frame in which the base frame is attached. Forward kinematics are mathematical-geometrical relations that provide the end-effector configuration by having the joint coordinates. On the other hand, the inverse kinematics are mathematical-geometrical relations that provide joint coordinates for a given end-effector configuration. The Cartesian path of motion for the end-effector must be expressed as a function of time to find the links velocity and acceleration. The first applied path function that can provide a rest-to-rest motion is a cubic path for a variable q i (t) betweentwogivenpointsq i (t ) and q i (t f ) q i (t) =a + a 1 t + a 2 t 2 + a 3 t 3. (13.271) By increasing the requirements, such as zero acceleration or jerk at some points on the path, we need to employ higher polynomials to satisfy the conditions. An n degree polynomial can satisfy n +1conditions. It is also possible to split a multiple conditional path into some intervals with fewer conditions. The interval paths must then be connected to satisfy their boundary conditions. A path of motion may also be defined based on different mathematical functions. Harmonic and cycloid functions are the most common paths. Non-polynomial equations introduce some advantages, due to simpler expression, and some disadvantages due to nonlinearity. When a path of motion either in joint or Cartesian coordinates space is defined, forward and inverse kinematics must be utilized to find the path of motion in the other space. Rotational maneuver of the end-effector about the wrist point needs a rotational path. A rotational path may mathematically be defined similar to a Cartesian path utilizing the Rodriguez formula and rotation matrices.