Trajectory Planning Algorithm Based on the Continuity of Jerk

7

3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ &RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH

Trajectory planning algorithm based on the continuity of jerk K. Petrinec* and Z. Kovacic* *

University of Zagreb, Faculty of Electrical Engineering and Computing, Zagreb, Croatia

Abstract— This paper presents a trajectory planning method that provides a continuity of position, velocity, acceleration and jerk. The method combines fifth-order and fourth-order polynomials in order to satisfy the continuity of jerk and give smooth accelerations on all segments of the planned trajectory. The proposed method was tested for movements of the three-axes planar articulated robot. The method includes limits of the velocities, accelerations, and jerks of each robot joint. Simulation results are presented and compared with the results obtained by the original Ho and Cook spline-interpolation method.

I. INTRODUCTION Trajectory planning is an important part of every industrial or mobile robot control system. A problem of planning trajectories has to be solved precisely in order to provide efficacious execution of various manipulation or transportation tasks. In case of planning trajectories for an industrial robot equipped with different tools (e.g. for welding, assembly, product packaging, or quality inspection), the operator normally defines a set of waypoints which must be visited by a robot tool and a type of connection between the waypoints (straight-line, circular or elliptical arc, etc). Then, the trajectory planning is performed off-line or on-line by adding the arrays of points that best fit the curves connecting the given waypoints. Further on, all points are transformed from the Cartesian space to a corresponding joint space and a smooth trajectory through the points in the joint space is planned with respect to imposed velocity and acceleration constraints. For most of robot tasks, the trajectory planning method should provide continuity of positions, velocities and accelerations in all waypoints. For example, the Ho and Cook spline-interpolation algorithm that uses cubic and fourth-order spline-functions [1] meets that requirement. The first and the last segments of the planned trajectory are described with fourth-order polynomials, while all other (intermediate) segments are described with thirdorder polynomials. The velocity and acceleration at the start point and the end point are equal to zero (robot starts and stops in these terminal points). Since the intermediate segments are described with third-order polynomials, the jerk is kept bounded within the segments but it changes abruptly at the waypoints. As a result, the acceleration is not smooth. The method was originally developed as an off-line trajectory planning method for industrial robots. Compared to on-line methods, off-line methods have an advantage of allowing the knot points to be specified more closely in time, thus increasing the accuracy of a trajectory and allowing a shorter sampling period [2]. This method

was successfully used in the off-line programming tool for robotized plants [3], as well as for trajectory planning of automated guided vehicles [4]. The problem with the method is that velocity and acceleration constraints are met only on the “most critical” segment, and in order to find a time-optimal trajectory, the method has to be iteratively applied until the constraints are reached on all segments. However, as mentioned above, the Ho and Cook spline-interpolation algorithm does not ensure the continuity of jerk. Limiting jerk in robot trajectories contributes to extended life of manipulator and more precise tracking accuracy. Since the control of jerk coincides with the torque rate control, jerk-bounded trajectories result in smoothed actuator loads [5]. A good review of motion planning methods that are focused on jerk bounding can be found in [6]. The method described therein that provides a smooth, controlled near time optimal trajectory for point-to-point motion with jerk limits uses a concatenation of fifth-order polynomials to provide a smooth trajectory between two waypoints. The trajectory approximates a linear segment with parabolic blends trajectory, and a sine wave template is used to calculate the end conditions (control points) for ramps from zero acceleration to nonzero acceleration. In [7] a methodology of time-optimal trajectory planning for compliant sheet metal parts is described by discretizing the part transfer path into N segments that have equal horizontal distance and by approximating the trajectory as having piecewise constant acceleration that can only change its value at the end of each segment. Herein we present a trajectory planning method developed by modifying the Ho and Cook trajectory planning algorithm [1], which guarantees not only the continuity of acceleration and velocity, but also the continuity of jerk at all trajectory segments. Moreover, the velocities and accelerations at the terminal points can be other than zero. The paper is organized in the following way. First we describe a planning algorithm “445” which represents a modified form of the Ho and Cook interpolation algorithm “434” (the numbers in the names of algorithms indicate the orders of the used polynomials). Then we demonstrate the effectiveness of the proposed method applied to a planar articulated robot. Simulation results obtained with aforementioned interpolation algorithms are compared and discussed. II.

TRAJECTORY PLANNING

We assume that a planned trajectory has N-1 segments determined by N given points, P1… PN. Let us use fourth-

7

3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ &RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH

order polynomials to describe the first segment and all intermediate segments. Then the path between two adjacent waypoints Fk and Fk+1, 1 ≤ k ≤ N-1, can be written as: Fk (t ) = Bk ,1 + Bk ,2t + Bk ,3t 2 + Bk ,4t 3 + Bk ,5 t 4 (1) where F(t) represents a position of a single robot joint. Letting t in (1) to run from zero to Tk+1, the boundary conditions for this segment can be expressed as: Fk (0) = Pk Fk (Tk +1 ) = Pk +1 vk (0) = vk vk (Tk +1 ) = vk +1 (2) ak (0) = ak where vk and vk+1 represent joint velocities, and ak and ak+1 represent joint accelerations at points Pk and Pk+1, respectively (see Fig. 1). Substitution of (2) into (1) yields: Fk (0) = Bk ,1 = Pk (3) vk (0) = Bk ,2 = vk

(4)

ak (0) = 2 Bk ,3 = ak 2 k ,3 k +1

Fk (Tk +1 ) = Bk ,1 + Bk ,2Tk +1 + B T

+ Bk ,5Tk +1 = Pk +1

(5) (6)

+ 4 Bk ,5Tk3+1 = vk +1

(7)

3 k ,4 k +1

+B T 2 k ,4 k +1

vk (Tk +1 ) = Bk ,2 + 2Bk ,3Tk +1 + 3B T

4

Solving (6) and (7) for Bk,4 and Bk,5, and substituting (3), (4) and (5) for Bk,1, Bk,2 and Bk,3 yields: Pk +1 − Pk v a v − 3 2k − k − k2+1 Tk3+1 Tk +1 Tk +1 Tk +1

(8)

Pk − Pk +1 v a v + 2 3k + k2 − k3+1 4 Tk +1 Tk +1 2Tk +1 Tk +1

(9)

Bk ,4 = 4 Bk ,5 = 3

For 1 ≤ k ≤ N-2, (3), (4), (5), (8) and (9) may be expressed in a matrix form as: 0 0 0 0   1  0 1 0 0 0  P  Bk ,1    k   B   1  0 0 0   vk   k ,2   0 2   a  (10)  Bk ,3  =  3 1 4 1  k     4 − − − − P  Bk ,4   T 3 Tk2+1 Tk +1 Tk3+1 Tk2+1   k +1   v   B   k +1  k ,5   3 2 1 3 1   k +1  − 4 3 2 4  T 2Tk +1 Tk +1 Tk3+1   k +1 Tk +1 In order to calculate the spline coefficients, the values of Tk+1, vk, ak, vk+1 and ak+1 should be known. Let us select the value Tk+1 to be the length of the chord between the waypoints Pk and Pk+1 in the joint coordinates:

Tk +1 =

n

∑ (q j =1

j ,k +1

− q j ,k ) 2

(11)

where Tk+1 is expressed as a parametric unit of time which will be determined later, n is the number of joints of the manipulator, and qj,k represents the value of the jth joint at the kth waypoint. The last trajectory segment is characterized with an additional constraint: aN −1 (TN ) = aN (12) To describe the last segment, this requires the use of a fifth-order polynomial of the form:

FN −1 (t ) = BN −1,1 + BN −1,2 t + BN −1,3t 2 + BN −1,4 t 3 + BN −1,5t 4 + BN −1,6 t 5 (13)

For the last segment, we let 0 ≤ t ≤ TN and have the following boundary conditions: FN −1 (0) = BN −1,1 = PN −1 (14) vN −1 (0) = BN −1,2 = vN −1

(15)

aN −1 (0) = 2 BN −1,3 = aN −1 2 N −1,3 N

FN −1 (TN ) = BN −1,1 + BN −1,2TN + B +B

4 N −1,5 N

(16) 3 N −1,4 N

T +B

T +

5 N −1,6 N

T +B

T = PN

vN −1 (TN ) = BN −1,2 + 2 BN −1,3TN + 3BN −1,4TN2 + +4BN −1,5TN3 + 5BN −1,6TN4 = vN aN −1 (TN ) = 2 BN −1,3 + 6 BN −1,4TN + 12 BN −1,5TN2 + +20 BN −1,6TN3 = aN

(17) (18) (19)

Solving (17), (18) and (19) for BN-1,4, BN-1,5 and BN-1,6, and substituting (14), (15) and (16) for BN-1,1, B N-1,2 and BN-1,3 yields: P −P v v 3 aN −1 1 aN BN −1,4 = 10 N 3 N −1 − 6 N 2−1 − − 4 N2 + (20) TN TN 2 TN TN 2 TN PN −1 − PN v v a 3 aN −1 + 8 N 3−1 + + 7 N3 − N2 2 TN2 TN4 TN TN TN

(21)

PN − PN −1 v v 1 aN −1 1 aN − 3 N −41 − − 3 N4 + 5 3 TN TN 2 TN TN 2 TN3

(22)

BN −1,5 = 15 BN −1,6 = 6

Equations (14), (15), (16), (20), (21) and (22) may be expressed in a matrix form as:  1  0   BN −1,1   B   0  N −1,2    BN −1,3   10   = − 3  BN −1,4   TN  BN −1,5   15    4 T  BN −1,6   N  6 − 5  TN

0

0

0

0

1

0 1 2 3 − 2TN

0

0

0

0

0 −

6 TN2

8 TN3 −

3 TN4

3 2TN2 −

1 2TN3

10 TN3 −

15 TN4

6 TN5

−

4 TN2

7 TN3 −

3 TN4

0  0    PN −1  0  v    N −1  1   a N −1  2TN   PN   1  v  − 2  N  TN  aN   1   2TN3 

(23)

The velocities vk and accelerations ak can be determined using the continuity constraint on the acceleration and jerk at the waypoints. Given the three waypoints Fk, Fk+1 and Fk+2 (1 ≤ k ≤ N-4) with two trajectory segments connecting them with times 0 ≤ t ≤ Tk+1 and 0 ≤ t ≤ Tk+2, the acceleration at the first segment can be determined as: ak (Tk +1 ) = 2Bk ,3 + 6Bk ,4Tk +1 + 12 Bk ,5Tk2+1

(24)

Jerk is defined as the third derivative of position over time. The jerk at the end of the first segment can be determined as: jk ( Tk +1 ) = 6 Bk ,4 + 24 Bk ,5Tk +1

(25)

Similarly, the acceleration and the jerk at the beginning of the second segment can be calculated as: ak +1 ( 0 ) = 2Bk +1,3 jk +1 ( 0 ) = 6 Bk +1,4

(26) (27)

3

U

R

F

H

H

G

&

R

Q

W

U

R

O

L 

Q

J 

V $



R

I



X

W

R

P

W

K D

W

7H







L

R

Q



Figure 1. Boundary conditions (dashed boxes) and continuity conditions (below the boxes) for spline segments of an N-point trajectory.

The continuity of acceleration requires (24) and (26) to be equal. Equating them and rearranging them gives: 6 6 12 vk + ak + vk +1 − ak +1 = 2 ( Pk +1 − Pk ) (28) Tk +1 Tk +1 Tk +1 Equally, the continuity of jerk requires (25) and (27) to be equal. Equating them and rearranging them gives:  18 30 6 18  6 6 vk + ak +  2 + 2  vk +1 + ak + 2 + 2 v k + 2 = 2 Tk +1 Tk +1 Tk +2 Tk +2  Tk +1 Tk +2  (29) 48 24 = 3 ( Pk +1 − Pk ) + 3 ( Pk +2 − Pk +1 ) Tk +1 Tk +2

The continuity constraint on the acceleration and jerk at the waypoints for the last two segments is as follows: aN −2 ( TN −1 ) = aN −1 ( 0 ) jN −2 (TN −1 ) = jN −1 ( 0 )

Equations (30) and (31) yield with: 3 1 3 1 6 vk + ak + vk +1 − ak +1 = 2 ( Pk +1 − Pk ) 2 2 Tk +1 Tk +1 Tk +1  3 5 1 6  3 vN − 2 + aN −2 +  2 + 2  v N −1 + a N −1 = 2 TN2−1 TN −1 T T T  N −1 N  N 8 10 4 1 = 3 ( PN −1 − PN −2 ) + 3 ( PN − PN −1 ) − 2 vN + aN 2TN TN −1 TN TN

(30) (31)

(32)

(33)

Combining the results of (28) and (29) with (32) and (33) allows us to express the matrix equation (34) shown below that can be used for solving for N-2 unknown velocities and N-2 unknown accelerations. Dimensions of [M] are (2N-4)×(2N-4), dimensions of [D] are (2N-4)×1 and dimensions of [H] are (2N-4)×1 where: 6 M 1,1 = , M 1,2 = −1 T2 18 18 6 6 M 2,1 = 2 + 2 , M 2,2 = , M 2,3 = 2 T2 T3 T3 T3 12 6 h1 = 2 ( P2 − P1 ) − v1 − a1 T2 T2 48 24 30 6 h2 = 3 ( P2 − P1 ) + 3 ( P3 − P2 ) − 2 v1 − a1 T2 T3 T2 T2 h2k -1 = 12 ( Pk +1 − Pk ) , k = 2,… , N − 3

h2k = 8Tk3+2 ( Pk +1 − Pk ) + 4Tk3+1 ( Pk +2 − Pk +1 ) h2 N -5 = h2 N -4 =

6 ( PN -1 − PN -2 ) TN2-1

8 10 4 1 aN ( PN -1 − PN -2 ) + 3 ( PN − PN -1 ) − 2 vN + TN3-1 TN TN 2TN

M×D = H = 0 0  M1,1 M 1, 2 M M M 0 0 2, 2 2,3  2,1  6T3 T32 6T3 -T32 0 0  3 2 3 3 3 3 2 3 T3 T4 0 0 5T3T4 T3 T4 3T3T4 + 3T3 T4 T3 T4  0 0    0 6TN -2 TN2-2 6TN -2 -TN2-2  3 2 3 3 3 0 5TN -2TN -1 TN -2TN -1 3TN -2TN -1 + 3TN -2TN -1 TN3 -2TN2 -1   3 1  0 0 TN -1 2   5 1  0 0 0 TN2-1 TN -1 

0

0 TN3 -2TN -1 3 TN -1 3 2 N -1

T

+

6 TN2

0  0   v2   h1      a2   h2    (34)   v3   h3    a3         ×  =   0       0  vN -2       1  a    -   N -2   2  vN -1 h2 N -5       3   a N -1   h2 N -4  2TN 

7

3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ &RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH

Joint1

-1

2 1

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5 Time

3

3.5

4

4.5

5

2

1.5

Joint3

-1.5 -2 -2.5 -3

Figure 3. The position of robot joints in joint space (angles).

The velocity of robot joints (Fig. 4) is smooth and stays within given constraints. The velocity at the start and the end of trajectories equals to the requested velocities. Velocity 2 Trajectory 434 Trajectory 445

Joint1

1 0 -1 -2

2 2

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5 Time

3

3.5

4

4.5

5

2 1

) )

 d1 + d 2 cos ( P2,k ) ⋅ Yk − d 2 sin ( P2,k ) ⋅ X k P1,k = arctan   d1 + d 2 cos ( P2,k ) ⋅ X k + d2 sin ( P2,k ) ⋅ Yk  P3,k = Ok − P1,k − P2,k

 ,  

0 -1 -2 2

where Xk and Yk represent x- and y-coordinate of the waypoints and Ok is the orientation of robot tool at the waypoints (k = 1, 2,…, 12). d2

Joint2

2 k

0

2.5

-1

 X +Y − d − d  P2,k = arccos   2d1d2  

( (

0 -0.5

SIMULATION RESULTS

Let us apply the proposed smooth trajectory planning method to a 3 DOF planar robot (Fig. 2). The lengths of robot segments are d1 = 1.1, d2 = 0.9. Twelve waypoints form a triangle (1,0), (0,1) and (1,1). Velocity and acceleration vectors at both ends of the trajectory are set to v1T = [-0.1, 0.4, 0], a1T = [1, 3, 0]. Being convenient, maximal velocities and accelerations of all joints are set to 2 rad/s and 10 rad/s2, respectively. For the sake of comparison, two trajectories were planned using the original “434” and the modified “445” interpolation algorithms. As mentioned before, the first and the last segment of “Trajectory 434” have the sets of fourth-order polynomials, while intermediate segments have the sets of third-order polynomials. On the other hand, the first and all intermediate segments of “Trajectory 445” have the sets of fourth-order polynomials, while the last segment has a set of fifth-order polynomials. For both trajectories, a set of waypoints in the robot joint space is found by using inverse kinematics equations: 2 k

Trajectory 434 Trajectory 445

0.5

F 3(t)

1 Joint3

III.

Position 1

Joint2

Solving (34) for [D] allows us to find polynomials with the desired properties. To extend these results to multiple robot joints, the described procedure for one joint can be repeated for each joint, using the same values of Tk. Thus, a path through N points for a robot with n joints will consist of (N-1)× n unique spline functions. Once the spline functions have been defined for all robot joints, the parametric variable Tk+1 must assume some initial time value (e.g. by using (11) and assuming that robot joint speeds have unity values), and then it must be scaled iteratively until maximum velocities and accelerations of all robot joints drop within the imposed physical constraints.

0 -1 -2

F 2(t ) Figure 4. The velocity of robot joints.

d1 y F1(t ) x Figure 2. A three degree of freedom robot.

Planned trajectories have a similar shape as both pass through the same waypoints. However, the total time for “Trajectory 445” is longer than the time for “Trajectory 434” (see Fig. 3).

The acceleration of the trajectories remains within given constraints as well. The acceleration at the start and the end of trajectories equals to the requested accelerations. While “Trajectory 434” has a triangular shape on the intermediate segments, “Trajectory 445” is smooth, as shown in Fig. 5. Moreover, while “Trajectory 434” has abrupt changes of jerk on the intermediate segments, “Trajectory 445” is jerk-continuous (Fig. 6). Using the following equations for forward kinematics, the path (x- and y-coordinates) and orientation of the robot tool can be determined:

7

3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ &RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH

X k = d1 cos ( F1 ) + d 2 cos ( F1 + F2 )

1

Yk = d1 sin ( F1 ) + d2 sin ( F1 + F2 ) .

0.9

Ok = F1 + F2 + F3

0.8

The path of the robot tool obtained with “Trajectory 445” exhibits slightly larger deviations from the ideal path than with “Trajectory 434”. That was expected due to the use of fifth order polynomials. This implies the addition of new waypoints, for example, by using a Taylor’s bounded deviations method [8]. It can be notice that both paths pass through the given set of waypoints (Fig. 7).

Latitude

0.7 0.6 0.5 0.4 0.3 0.2

Acceleration

0.1

10 Trajectory 434 Trajectory 445

Joint1

5

Path 434 Path 445 Waypoint Locations

0 0

0

0.2

0.4

0.6 Longitude

0.8

1

Figure 7. Planned trajectories.

-5 -10

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0

0.5

1

1.5

2

2.5 Time

3

3.5

4

4.5

5

20

Joint2

10 0 -10 -20 10

Joint3

5 0 -5 -10

Figure 5. The acceleration of robot joints.

IV. CONCLUSION There are many robot applications which require smooth robot motion. Limiting jerk in robot trajectories coincides with limiting the torque rate which extends the life of manipulator and contributes to more precise tracking accuracy. The trajectory planning algorithm described in this paper combines fourth-order and fifth-order polynomials to ensure the continuity of velocity, acceleration, and jerk at all trajectory segments. Moreover, the velocities and accelerations at the terminal points can assume different values. The developed smooth trajectory planning method is suitable for planning not only point-to-point, but also the continuous path movements of the robot. REFERENCES [1]

Jerk 200 Trajectory 434 Trajectory 445

Joint1

100

[2]

0 -100 -200

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

[3]

200

Joint2

100

[4]

0 -100 -200

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

[5]

Joint3

400 200

[6]

0 -200

0

0.5

1

1.5

2

2.5 Time

3

3.5

Figure 6. The jerk of robot joints.

4

4.5

5

[7]

[8]

C.Y. Ho and C.C. Cook, “The application of spline functions to trajectory generation for computer controlled manipulators”, Digital Systems for Industrial Automation, 1 (4): 325-333, 1982. P.G. Ranky and C.Y. Ho, Robot Modelling – Control and Applications with Software, Springer-Verlag, IFS (Publications) Lts, UK, 1985. Z. Kovacic, S. Bogdan, K. Petrinec, T. Reichenbach, M. Puncec, "Leonardo - The Off-line Programming Tool for Robotized Plants", CD-ROM Proceedings of the 9th Mediterranean Conference on Control and Automation, Dubrovnik, Croatia, June 27-29, 2001. K. Petrinec, Z. Kovacic, ”The application of spline functions and Bézier curves to AGV path planning”, CD-ROM Proceedings of the IEEE International Symposium on Industrial Electronics ISIE 2005, Dubrovnik, Croatia, June 20-23, 2005. K. J. Kyriakopoulos and G. N. Saridis, “Minimum jerk path generation,” Proceedings of the IEEE International Conference on Robotics and Automation, 364–369, 1988. S. Macfarlane and E. A. Croft, “Jerk-Bounded Manipulator Trajectory Planning: Design for Real-Time Applications”, IEEE Transactions on Robotics and Automation, Vol.19, No. 1, 42-52, 2003. H. Li and D. Ceglarek, “Optimal Trajectory Planning for Material Handling of Compliant Sheet Metal Parts”, ASME Journal of Mechanical Design, Vol. 124, 213-222, June 2002. R.H. Taylor, “Planning and execution of straight line manipulator trajectories”, IBM J. Research and Development, Vol. 23, 424436, 1979.