Trajectory Planning

Abstract

This chapter discusses the trajectory planning of robot. First, the basic principle of trajectory planning is described, which is mainly determined by the geometric path and the motion law, and it is a motion law that defines time according to a given geometric path. Second, the characteristics of the joint space and the operational space are expounded. Then the trapezoidal trajectory and the trajectory with S velocity profile commonly used in industrial practice are introduced. Finally, point-to-point trajectories and multipoint trajectories are described. Point-to-point trajectories include a point-to-point trajectory in the joint space and a straight-line and circle trajectory in the operational space. Multipoint trajectories include Bezier and NURBS curves functions.

1. Introduction

Trajectory planning is a motion law that defines time according to a given geometric path. Therefore, the purpose of trajectory planning is to meet the needs of the tasks and generate the reference inputs required by the control system, so that the robots can perform the movements in Ref. [1]. The inputs of any trajectory planning algorithm are an expected motion path, kinematics, and dynamics system parameters of the robots, and the outputs are the positions, velocities, and accelerations of the interpolation points of each joint or the end effector in Refs. [2, 3]. This chapter will introduce trajectory planning from the perspective of industrial practice, including the basic principle of trajectory planning, the joint space and the operational space, the motion laws, and point-to-point and multipoint trajectories.

2. The basic principle of trajectory planning

The desired trajectory can be fully determined by the geometric path and the motion law. The geometry path is related to space, while the motion law is related to time.

2.1 The geometry of the trajectory

The geometric path $p=p\left(s\right)$ expresses the path of the end effector from the start point to the end point in Ref. [4]. The end point B can be reached in two different path forms from the start point A, see Figure 1. Depending on whether it is necessary to specify the geometry of the trajectory, the planning of trajectory can be carried out in the joint space or the operational space.

Figure 1.

Two different path forms from the start point A to the end point B.

2.2 The motion law

The motion law $s=s\left(t\right)$ limits velocity, acceleration, and other parameters of the end effector from the start point to the end point. The end point B can be reached from the start point A according to two different motion laws, see Figure 2. Various functions, such as linear trajectory function with constant velocity or trajectory function with S velocity profile, can be selected to specify the motion law along the geometric path.

Figure 2.

Two different motion laws from the start point A to the end point B.

Therefore, based on the geometric path $p=p\left(s\right)$ and the motion law $s=s\left(t\right)$, the trajectory can be written as

$$p=p\left(s\left(t\right)\right)$$E1

3. The joint space and the operational space

Trajectory planning is divided into the joint space and the operational space according to the different description spaces. Trajectory planning in the joint space has the characteristics of few constraints and fast calculation speed, while trajectory planning in the operational space is mainly used for scenarios where users have specified path requirements.

3.1 The joint space

Trajectory planning in the joint space is to describe the trajectory of the robot as a function of joint position with respect to time, that is, the trajectory of the end effector can be directly determined by joint variables, so it is easy and simple to carry out trajectory planning in the joint space. Since there is no continuous corresponding relationship between the joint space and the operational space, singularity, and redundant degrees of freedom in motion can be avoided. In addition, the complexity of its path depends on the specific kinematic characteristics of the robot, and the path in the operational space cannot be predicted in Ref. [5]. Therefore, trajectory planning in the joint space is only applicable to the operating scenarios without path requirements.

3.2 The operational space

Trajectory planning in the operational space is based on a function of position and orientation with respect to time to describe the trajectory of the robot, and the corresponding joint position is obtained by the inverse kinematic model.

Since the path planned in the operational space is determined, the trajectory planning in the operational space is suitable for the operating scenarios with strict requirements for the instantaneous change law of position and orientation. In addition, the planning of trajectory in the operational space is prone to problems such as unreachable workspace or singularity, and they are generally avoided by specifying the robots to pass through a set of intermediate points in Ref. [6].

4. The motion law

In order to ensure that the robots can operate efficiently and stably, it is generally required that the motion profile meet at least the continuous first derivative, and it needs to have the characteristics of fast acceleration and deceleration and constant velocity. At present, the velocity control algorithms commonly used in robot systems include the trapezoidal trajectory and the trajectory with S velocity profile.

4.1 The trapezoidal trajectory

The trapezoidal trajectory means that the velocity profile is trapezoidal, and its complete curves are shown in Figure 3. The complete trajectory consists of three phases. In the first phase, the acceleration changes from zero to a fixed acceleration, and the velocity increases linearly with a fixed acceleration from zero. In the second phase, when the velocity reaches the desired value, it enters the constant velocity phase. At this phase, the velocity is constant and the acceleration suddenly changes to zero. In the third phase, the velocity begins to decrease with a fixed acceleration until it reaches zero.

Figure 3.

Position, velocity, and acceleration curves of a trapezoidal trajectory.

Piecewise acceleration equations are expressed as

$$a\left(t\right)=\left\{\begin{array}{ll}{a}_{\max }& \left(0\le t\le t_1\right)\\ 0& \left(t_1\le t\le t_2\right)\\ -a_{\max }& \left(t_2\le t\le t_3\right)\end{array}\right.$$E2

Piecewise velocity equations are expressed as

$$v\left(t\right)=\left\{\begin{array}{ll}{a}_{\max }t& \left(0\le t\le t_1\right)\\ v_2& \left(t_1\le t\le t_2\right)\\ v_2-a_{\max }\left(t-t_2\right)& \left(t_2\le t\le t_3\right)\end{array}\right.$$E3

Piecewise position equations are expressed as

$$s\left(t\right)=\left\{\begin{array}{ll}\frac{1}{2}{a}_{\max }{t}^2& \left(0\le t\le t_1\right)\\ s_1+v_2\left(t-t_1\right)& \left(t_1\le t\le t_2\right)\\ s_2-\frac{1}{2}{a}_{\max }{\left(t-t_2\right)}^2& \left(t_2\le t\le t_3\right)\end{array}\right.$$E4

where the time instants $t_1$, $t_2$ and $t_3$ are specified. $s_1$, $s_2$ and $s_3$ are corresponding position values, and $v_1$, $v_2$ and $v_3$ are corresponding velocity values. $a_{\max }$ is a maximum acceleration value.

4.2 The trajectory with S velocity profile

The trajectory with S velocity profile means that the velocity profile is S-shaped, which presents that the jerk is constant during acceleration and deceleration, and a smoother velocity curve is generated by controlling the duration of jerk to reduce the impact on machinery. The complete trajectory with S velocity profile includes seven phases: constant positive jerk phase $T_1$ in acceleration, constant zero jerk phase $T_2$ in acceleration, constant negative jerk phase $T_3$ in acceleration, constant velocity phase $T_4$, constant negative jerk phase $T_5$ in deceleration, constant zero jerk phase $T_6$ in deceleration, and constant positive jerk phase $T_7$ in deceleration, see Figure 4. In the first three phases, the velocity increases smoothly, the acceleration presents trapezoidal profile, and the jerk changes abruptly between the desired value and zero. In the fourth phase, when the velocity is accelerated to the desired value, it enters the constant velocity phase. When the end point is approaching, the deceleration phase begins in the last phases. The transition of the entire velocity curve is smooth, and there is a continuous acceleration.

Figure 4.

Position, velocity, and acceleration curves of a trajectory with S velocity profile.

Piecewise jerk equations are expressed as

$$j\left(t\right)=\left\{\begin{array}{ll}{j}_{\max }& \left(0\le t\le t_1\right)\\ 0& \left(t_1\le t\le t_2\right)\\ -j_{\max }& \left(t_2\le t\le t_3\right)\\ 0& \left(t_3\le t\le t_4\right)\\ -j_{\max }& \left(t_4\le t\le t_5\right)\\ 0& \left(t_5\le t\le t_6\right)\\ j_{\max }& \left(t_6\le t\le t_7\right)\end{array}\right.$$E5

Piecewise acceleration equations are expressed as

$$a\left(t\right)=\left\{\begin{array}{ll}{j}_{\max }t& \left(0\le t\le t_1\right)\\ j_{\max }{T}_1& \left(t_1\le t\le t_2\right)\\ j_{\max }{T}_1-j_{\max }\left(t-t_1\right)& \left(t_2\le t\le t_3\right)\\ 0& \left(t_3\le t\le t_4\right)\\ -j_{\max }\left(t-t_4\right)& \left(t_4\le t\le t_5\right)\\ -j_{\max }{T}_4& \left(t_5\le t\le t_6\right)\\ -j_{\max }{T}_4+j_{\max }\left(t-t_6\right)& \left(t_6\le t\le t_7\right)\end{array}\right.$$E6

Piecewise velocity equations are expressed as

$$v\left(t\right)=\left\{\begin{array}{ll}{v}_s+\frac{1}{2}{j}_{\max }{t}^2& \left(0\le t\le t_1\right)\\ v_1+j_{\max }{T}_1\left(t-t_1\right)& \left(t_1\le t\le t_2\right)\\ v_2+j_{\max }{T}_1\left(t-t_2\right)-\frac{1}{2}{j}_{\max }{\left(t-t_2\right)}^2& \left(t_2\le t\le t_3\right)\\ v_3& \left(t_3\le t\le t_4\right)\\ v_4-\frac{1}{2}{j}_{\max }{\left(t-t_4\right)}^2& \left(t_4\le t\le t_5\right)\\ v_5-j_{\max }{T}_5\left(t-t_5\right)& \left(t_5\le t\le t_6\right)\\ v_6-j_{\max }{T}_5\left(t-t_6\right)+\frac{1}{2}{j}_{\max }{\left(t-t_6\right)}^2& \left(t_6\le t\le t_7\right)\end{array}\right.$$E7

Piecewise position equations are expressed as

$$s\left(t\right)=\left\{\begin{array}{ll}{v}_{s}t+\frac{1}{6}{j}_{\max }{t}^3& \left(0\le t\le t_1\right)\\ s_1+v_1\left(t-t_1\right)+\frac{1}{2}{j}_{\max }{T}_1{\left(t-t_1\right)}^2& \left(t_1\le t\le t_2\right)\\ s_2+v_2\left(t-t_2\right)+\frac{1}{2}{j}_{\max }{T}_1{\left(t-t_2\right)}^2-\frac{1}{6}{j}_{\max }{\left(t-t_2\right)}^3& \left(t_2\le t\le t_3\right)\\ s_3+v_3\left(t-t_3\right)& \left(t_3\le t\le t_4\right)\\ s_4+v_4\left(t-t_4\right)-\frac{1}{6}{j}_{\max }{\left(t-t_4\right)}^3& \left(t_4\le t\le t_5\right)\\ s_5+v_5\left(t-t_5\right)-\frac{1}{2}{j}_{\max }{T}_5{\left(t-t_5\right)}^2& \left(t_5\le t\le t_6\right)\\ s_6+v_6\left(t-t_6\right)-\frac{1}{2}{j}_{\max }{T}_5{\left(t-t_6\right)}^2+\frac{1}{6}{j}_{\max }{\left(t-t_6\right)}^3& \left(t_6\le t\le t_7\right)\end{array}\right.$$E8

Where the time instants $t_1$, $t_2$, …, and $t_7$ are specified. $s_1$, $s_2$, …, and $s_7$ are corresponding position values, and $v_1$, $v_2$,…, and $v_7$ are corresponding velocity values. $a_{\max }$ and $j_{\max }$ are maximum acceleration and jerk values.

5. Point-to-point trajectories and multipoint trajectories

5.1 Point-to-point trajectories

Point-to-point trajectories are mainly used for robotic tasks such as palletizing, cutting, press tending, and other operations. Point-to-point control first ensures the position and orientation of the teach-in points, and the geometric path between the teach-in points, such as straight-lines and circles, can be specified when programming the robot’s working program.

5.1.1 Point-to-point trajectory in the joint space

According to the requirements of the task, the end effector is required to move linearly in the joint space between point A and point B (Figure 5).

Figure 5.

A point-to-point trajectory in the joint space.

Therefore, the position of each joint at the time instant $t+\mathrm{\Delta }t$ can be expressed as

$${\theta }_{t+\mathrm{\Delta }t}=\left(1-u\right){\theta }_A+u{\theta }_B\left(0\le u\le 1\right)$$E9

5.1.2 Point-to-point trajectory in the operational space

5.1.2.1 A straight-line position trajectory

According to the requirements of the task, the position of the end effector needs to be moved in a straight-line between point A and point B. Generally, the positions of the start point A and the end point B in the operational space are known, and the positions of a set of intermediate points on the trajectory AB are calculated, see Figure 6.

Figure 6

A straight-line position trajectory in the operational space.

The position length of the trajectory AB is $L_{\mathit{AB}}$. The end effector is located at the point $P_t$ and $P_{t+\mathrm{\Delta }t}$ on the trajectory AB, corresponding to the displacement $s\left(t\right)$ and $s\left(t+\mathrm{\Delta }t\right)=s\left(t\right)+\mathrm{\Delta }s$ at the time instants $t$ and $t+\mathrm{\Delta }t$. $\mathrm{\Delta }t$ is a control period, corresponding to the displacement $\mathrm{\Delta }s$ .

Length function in normalized form can be expressed as

$$u=\frac{s\left(t+\mathrm{\Delta }t\right)}{L_{\mathit{AB}}}\left(0\le u\le 1\right)$$E10

Therefore, the position of the end effector at the time instant $t+\mathrm{\Delta }t$ can be expressed as

$$\left[\begin{array}{c}{P}_x\\ P_y\\ P_z\end{array}\right]=\left(1-u\right)\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]+u\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]$$E11

5.1.2.2 A straight-line orientation trajectory

According to the requirements of the task, the orientation of the end effector needs to be moved continuously between point A and point B. Generally, the orientations of the start point A and the end point B in the operational space are known, and the orientations of a set of intermediate points on the trajectory AB are calculated, see Figure 7. Since Euler angles and rotation matrices may have gimbal locking in rotation, and quaternions do not have this problem. Therefore, quaternions can be used to calculate the orientation.

Figure 7

A straight-line orientation trajectory in the operational space.

The orientation unit vector of the start point A is ${\overrightarrow{r}}_A$, corresponding to the quaternion $Q_A$. The orientation unit vector of the end point B is ${\overrightarrow{r}}_B$, corresponding to the quaternion $Q_B$. The angle between two orientation unit vectors is $\theta$. The end effector is located at the point $P_t$ and $P_{t+\mathrm{\Delta }t}$ on the trajectory AB, corresponding to the angle $\theta \left(t\right)$ and $\theta \left(t+\mathrm{\Delta }t\right)$ at the time instants $t$ and $t+\mathrm{\Delta }t$. Its orientation unit vector is ${\overrightarrow{r}}_{P_{t+\mathrm{\Delta }t}}$, and the corresponding quaternion is $Q_{P_{t+\mathrm{\Delta }t}}$.

Angle function in normalized form can be expressed as

$$u=\frac{\theta \left(t+\mathrm{\Delta }t\right)}{\theta }\left(0\le u\le 1\right)$$E12

The general linear interpolation will cause uneven changes in angular velocity, while spherical linear interpolation can ensure a smooth curve between two quaternions. Therefore, the orientation of the end effector at the time instant $t+\mathrm{\Delta }t$ can be expressed as

$$Q_{P_{t+\mathrm{\Delta }t}}=\frac{sin\left[\left(1-u\right)\theta \right]Q_A+sin\left(\mathit{u\theta }\right)Q_B}{sin\theta }$$E13

When the angle $\theta$ is very small, it will result in a large value in the Eq. (13). Therefore, the linear interpolation formula can be used

$$Q_{P_{t+\mathrm{\Delta }t}}=\left(1-u\right)Q_A+{\mathit{uQ}}_B$$E14

5.1.2.3 A circle position trajectory

According to the requirements of the task, the position of the end effector needs to be moved in a circle between points A, B, and C. Generally, the positions of the start point A, the middle point B and the end point C in the operational space are known, and the positions of a set of intermediate points on the trajectory ABC are calculated, see Figure 8. Based on the position of the start point A, the middle point B, and the end point C, the radius $R$ and center $M$ of the circle can be solved. Then the unit normal vector $\overrightarrow{n}$ of the circle, the unit tangent vector $\overrightarrow{e}$ of the start point, and the unit vector ${\overrightarrow{w}}_A$ from the center to the start point can be solved.

Figure 8

A circle position trajectory in the operational space.

Length function in normalized form can be expressed as

$$u=\frac{s\left(t+\mathrm{\Delta }t\right)}{L_{\stackrel{⌢}{\mathit{ABC}}}}\left(0\le u\le 1\right)$$E15

Therefore, the position of the end effector at the time instant $t+\mathrm{\Delta }t$ can be expressed as

$$\left[\begin{array}{c}{P}_x\\ P_y\\ P_z\end{array}\right]=\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]+R•cos\left(\frac{\mathit{uS}}{R}\right){\overrightarrow{w}}_A+R•sin\left(\frac{\mathit{uS}}{R}\right)\overrightarrow{e}$$E16

5.1.2.4 A circle orientation trajectory

According to the requirements of the task, the orientation of the end effector needs to be moved continuously between points A, B, and C. Generally, the orientations of the start point A, the middle point B and the end point C in the operational space are known, and the orientations of a set of intermediate points on the trajectory ABC are calculated, see Figure 9.

Figure 9.

A circle orientation trajectory in the operational space.

The orientation unit vector of the start point A is ${\overrightarrow{r}}_A$, corresponding to the quaternion $Q_A$. The orientation unit vector of the middle point B is ${\overrightarrow{r}}_B$, corresponding to the quaternion $Q_B$. The orientation unit vector of the end point C is ${\overrightarrow{r}}_C$, corresponding to the quaternion $Q_C$. The angle of the orientation unit vectors between the start and the middle point is ${\alpha }_{\mathit{AB}}$. The angle of the orientation unit vectors between the middle and end point is ${\alpha }_{\mathit{BC}}$. The angle of the orientation unit vectors between the start and end point is $\alpha$. The end effector is located at the point $P_{t+\mathrm{\Delta }t}$ on the trajectory AB, corresponding to the angle $\alpha \left(t+\mathrm{\Delta }t\right)$ at the time instant $t+\mathrm{\Delta }t$. Its orientation unit vector is ${\overrightarrow{r}}_{P_{t+\mathrm{\Delta }t}}$, and the corresponding quaternion is $Q_{P_{t+\mathrm{\Delta }t}}$.

Middle angle function in normalized form can be expressed as

$$u_{\mathit{\text{Middle}}}=\frac{{\alpha }_{\mathit{AB}}}{\alpha }$$E17

Angle function in normalized form can be expressed as

$$u=\frac{{\alpha }_{t+\mathrm{\Delta }t}}{\alpha }$$E18

If $u\le u_{\mathit{\text{Middle}}}$, the orientation of the end effector at the time instant $t+\mathrm{\Delta }t$ can be expressed as

$$Q_{P_{t+\mathrm{\Delta }t}}=\frac{sin\left[\left(1-\frac{u}{u_{\mathit{\text{Middle}}}}\right){\alpha }_{\mathit{AB}}\right]Q_A+sin\left(\frac{u}{u_{\mathit{\text{Middle}}}}{\alpha }_{\mathit{AB}}\right)Q_B}{sin{\alpha }_{\mathit{AB}}}$$E19

If $u_{\mathit{\text{Middle}}}\le u$, the orientation of the end effector at the time instant $t+\mathrm{\Delta }t$ can be expressed as

$$Q_{P_{t+\mathrm{\Delta }t}}=\frac{sin\left[\left(1-\frac{u-u_{\mathit{\text{Middle}}}}{1-u_{\mathit{\text{Middle}}}}\right){\alpha }_{\mathit{BC}}\right]Q_B+sin\left(\frac{u-u_{\mathit{\text{Middle}}}}{1-u_{\mathit{\text{Middle}}}}{\alpha }_{\mathit{BC}}\right)Q_C}{sin{\alpha }_{\mathit{BC}}}$$E20

5.2 Multipoint trajectories

Multipoint trajectories are often used for complex operations such as painting, arc welding, or surface machining. The user teaches a series of points (such as start, end, and intermediate points) on a continuous path, and the robots connect the path points or multi-segment paths sequentially using polynomial functions, Bezier curve functions, or spline functions.

5.2.1 Bezier curve functions

In order to further improve the efficiency of the tasks, the robot end effector is required to achieve continuous operation between multiple trajectories. That is to say, the displacement and speed motion profiles are continuous. Due to the characteristics of the geometric invariance and convex hull, high-order Bezier curves are often used in industrial practice to connect multi-segment paths in Ref. [7]. Taking the connection of two straight-line trajectories as an example, the position of point A, point B and point C of line 1 and line 2 is generally known, and the end point B of line 1 is used as the start point of line 2, see Figure 10.

Figure 10.

A quartic Bezier curve trajectory.

A circle with B as center point and the given transition value R as radius can be obtained, and it intersects the AB and BC trajectories at D and E points, respectively. A quartic Bezier curve is used to connect the two trajectories, and five control points need to be specified on the DB and BE segments. The center point B is the control point $P_2$, and the points D and E are the control points $P_0$ and $P_4$, respectively. $P_1$ is taken halfway through the DB segment and $P_3$ is taken halfway through the BE segment.

The quartic Bezier curve function can be written as

$$P\left(t\right)=\sum _{i=0}^4{P}_i{B}_{i,4}\left(t\right),t\in \left[0,1\right]$$E21

The corresponding expansion in Eq. (21) is

$$\begin{array}{l}P\left(t\right)={\left(1-t\right)}^4{P}_0+4t{\left(1-t\right)}^3{P}_1+6t^2{\left(1-t\right)}^2{P}_2\\ +4t^3\left(1-t\right)P_3+t^4{P}_4,t\in \left[0,1\right]\end{array}$$E22

Recursive equation is

$$P_i^k=\left\{\begin{array}{c}{P}_i\\ \left(1-t\right)P_i^{k-1}+{\mathit{tP}}_{i+1}^{k-1}\end{array}\right.\begin{array}{l}k=0\\ k=1,2,\dots ,4,i=0,1,\dots ,4-k\end{array}$$E23

5.2.2 NURBS curve functions

For the complex trajectories of the end effector in the operation space, the NURBS curve has been widely used because it can accurately and uniformly express standard analytical curves and free curves and has the characteristic of flexible shape control in Ref. [8]. Any k-th degree NURBS curve can be represented as a piecewise rational polynomial function.

$$P\left(t\right)=\frac{{\sum }_{i=0}^n{d}_i{\omega }_i{N}_{i,k}\left(t\right)}{{\sum }_{i=0}^n{\omega }_i{N}_{i,k}\left(t\right)}$$E24

where ${\omega }_i$ are the weight factors, $d_i$ are the curve control points, and $N_{i,k}\left(t\right)$ are B-spline basis functions determined by the node vectors $t_i$ .

Based on De Boor-Cox recursive function, the basis function can be written as

$$\left\{\begin{array}{l}{N}_{i,0}=\left\{\begin{array}{c}1\\ 0\end{array}\begin{array}{c},t\in \left[t_i,t_{i+1}\right)\\ ,\mathit{\text{otherwise}}\end{array}\right.\\ N_{i,k}=\frac{t-t_i}{t_{i+k}-t_i}{N}_{i,k-1}\left(t\right)+\frac{t_{i+k+1}-t}{t_{i+k+1}-t_{i+1}}{N}_{i+1,k-1}\left(t\right)\end{array}\right.$$E25

where $\frac{0}{0}=0$ is specified in Eq. (25).

Due to the computational complexity of the control points solved by the via-points of the NURBS curve, the De Boor-Cox algorithm is often used to avoid repeated iteration. More details on NURBS curve calculations can be found in Ref. [9].

6. Conclusions

This chapter discusses the trajectory planning of robots. First, the basic principle of trajectory planning is described, which is mainly determined by the geometric path and the motion law. Second, the characteristics of the joint space and the operational space are expounded. Then the trapezoidal trajectory and the trajectory with S velocity profile commonly used in industrial practice are introduced. Finally, point-to-point trajectories and multipoint trajectories are described. Point-to-point trajectories include a point-to-point trajectory in the joint space and a straight-line and circle trajectory in the operational space. Multipoint trajectories include Bezier and NURBS curves functions.