Trajectory Planning of High Precision Collaborative Robots

: In order to satisfy the high efficiency and high precision of collaborative robots, this work presents a novel trajectory planning method. First, in Cartesian space, a novel velocity look-ahead control algorithm and a cubic polynomial are combined to construct the end-effector trajectory of robots. Then, the joint trajectories can be obtained through the inverse kinematics. In order to improve the smoothness and stability in joint space, the joint trajectories are further adjusted based on the velocity look-ahead control algorithm and quintic B-spline. Finally, the proposed trajectory planning method is tested on a 4-DOF serial collaborative robot. The experimental results indicate that the collaborative robot achieves the high efficiency and high precision, which validates the effectiveness of the proposed method.


Velocity calculation of break points
The curve path of end-effector consists of discrete break points, as shown in Fig. 1. When the manipulator passes through these break points, it will generate large acceleration. In order to improve the motion performance of manipulator, we propose the modified velocity look-ahead control algorithm to restrict the velocity and acceleration at break points.
( 1) where, For the high efficiency of robot motion, the velocity of break point takes the maximum permissible value.
The traditional velocity look-ahead control algorithm only adjusts the velocity of break points once and cannot guarantee that all the velocities meet the displacement constraints shown in Eq. (8). For this, a novel velocity look-ahead control method is proposed to adjust the velocities of all break points and the adjustment flow chart is shown in Fig. 2.

Velocity calculation of linear points
The linear points, which are between the adjacent break points, are obtained by dispersing straight lines. To ensure the smoothness and stability of trajectory, we utilize the acceleration/deceleration (ACC/DEC) hybrid algorithm, based on the cubic polynomial, to calculate the velocity of these linear points. The velocity equation of ACC/DEC is described as

VV V t t T T t T T t T T T V t T T TT
where, 0 V , 1 V , 0 T and 1 T are the initial velocity, terminal velocity, initial time and terminal time of ACC/DEC period, respectively.
When the velocities of start and end points are s V and e V , respectively, the velocity of uniform phase is a V , the maximum velocity is m V , there exist seven profiles of velocity curves with the ACC/DEC hybrid algorithm. The velocity curves are shown in Fig. 3.
The boundary conditions corresponding to the seven profiles of velocity curves are shown in Tab. 1.
where, L is the total displacement along a straight line， b L is a displacement boundary condition, when b LL  , there has a uniform phase, when b LL  , the uniform phase does not exist.

Time calculation of discrete points
On the basis of trajectory planning in Cartesian space, we can get the angles and angular velocities by applying inverse kinematic transformation. In joint space, the joint trajectories also need to meet the constraints shown in Eq. (8). Therefore, the angular velocities must be adjusted, and the adjustment process is as follows.
Step 1: detecting the break points in joint space and changing the angular velocities of break points to zero.
Step 2: using the points whose velocities are zero as boundaries to separate the entire trajectory to independent parts.
Step 3: defining m  and m  as the maximum angular velocity and angular acceleration, respectively, and taking all discrete points as break points. Then, the angular velocities of each independent part can be adjusted through the method shown in Fig. 2.
The proposed ACC/DEC hybrid algorithm is applied to fit the adjacent points in joint space. According to the seven profiles of velocity curves shown in Fig. 3, the motion time between two adjacent points can be calculated by the equations described in Tab. 2.
The time of discrete points is where, p is the number of joints, is the time that the j th joint moves from point 1 i − to point i .

Quintic B-spline fitting
Through the above calculation, we have gained the discrete angles and time of every joint. In order to get the smooth trajectory of each joint, the multi-degree B-splines which have good smoothness and local support is adopted to fit these discrete joint points. Based on the Cox-de Boor recursion formula, we can get the basis function of the kth-degree Bspline as follows.
where, i u is the knot and normalized to the interval [0,1] . Using the accumulative chord length method, we normalize the time variable i t to gain the knot variable i u .
where, the knots have multiplicity 1 k + at each end so that the parameters at the endpoints of trajectory can be controlled.
To interpolate 1 n + discrete points i p by the kth B-spline, the 1 n + equations of trajectory can be written as follows.
where, ( 0,1, , 1) j d j n k = + − is the control points that need to be solved, the vector i p is ( ) , , 0,1,2, , where, i  and i t is the angle and time of point i , respectively.
According to Eqs. (18) and (19), we only have 1 n + equations to solve nk + control points, the extra 1 k − equations must be provided by kinematic constraint conditions, including velocity and acceleration constraints at start and end points. The rth order derivatives for kth-degree B-spline can be expressed as here, (1) (1) (1) 0 , 1 1 1 According to Eqs. (18), (19) and (22), we can solve all control points of quintic B-spline ( 5 k = ). Then, substituting the control points to Eq. (18), we can obtain the continuous joint trajectories. Fig. 4 shows the whole process of trajectory planning.  Figure 4: The whole process of trajectory planning

Experimental results
To verify the effectiveness of the proposed trajectory planning method, the experiments are carried out on a 4-DOF serial collaborative robot, as shown in Fig. 5. Fig. 5(a) shows the simulated model of robot including four joints with its standard D-H link coordinate shown in Fig. 5(b). The D-H parameters of the robot are described in Tabs. 3, and the initial value of ( 1, 2,3, 4) i i  = is zero. The kinematic constraints in Cartesian space (the maximum velocity and acceleration of end-effector) are given as in Tab. 4. Tab. 5 shows the working range and kinematic constraints of each joint (the maximum angular velocity and angular acceleration of each joint). The task of experiments is to track a closed path in the XY plane (Z=0), as shown in Fig.  6. The closed path is made up of 686 discrete points. The end effector starts from point S and moves along the closed path anticlockwise. The velocity and acceleration at the initial and the ending moments are configured as zero. We adopt the proposed velocity look-ahead control algorithm and ACC/DEC algorithm to gain the velocities of all discrete points in Cartesian space. The displacement and velocity of discrete points are described in Fig. 7. From Fig. 7, we can find that the velocity of end effector increases from zero quickly at the beginning because the curvature of line b SP shown in Fig. 6 is zeros. But after the point b P is an arc and the curvature rise suddenly. In order to guarantee the motion of end-effector meets the kinematic constraints when passing the arc with large curvature, the velocity drops steadily in advance when approaching the point b P . Moreover, the acceleration period reduces with the linear distance decreases between adjacent break points, like the line cd PP shown in Fig. 6.      To display the experimental result that the manipulator tracks the closed path shown in Fig. 6, we use a black pen as the end effector and the pen length is 60 mm. The experimental result is shown in Fig. 9, which indicates that the 4-DOF collaborative robot achieves the expected requirements successfully. The trajectory of end effector is shown in Fig. 10, and the equations of velocity and acceleration are as follows.
where, x V , y V , x A and y A are the projections of velocity and acceleration on the X-axis and Y-axis, respectively. From Fig. 10, we can find that the manipulator trajectory meets the kinematic constraints shown in Tab. 4. Because of the nonlinear relationship between the end-effector and joint and the kinematic singularity of joints, the final trajectory of end-effector is properly adjusted at some of break points, like the stage R. Fig. 11 indicates the period that the acceleration is greater than specified value of boundary. From Fig. 11 we can see that the period that the acceleration is greater than 0.8 m a is more than 15 s, the period that the acceleration is greater than 0.5 m a is more than 20 s in the whole movement. The average acceleration is 2 66.8 mm / s which is greater than 60% of the maximum acceleration of end-effector obviously. Therefore, the robot achieves the high efficiency and high precision. The period that the acceleration is greater than its boundary

Conclusions
This paper proposed a novel trajectory planning method for collaborative robots. Compared with the traditional methods based on the complex optimization algorithm, the proposed method has higher computational efficiency. Furthermore, through the cubic polynomial, the trajectory in Cartesian space are twice continuous differentiable. When using the quintic B-spline in joint space, the angular acceleration and jerk of joints are continuous. Because the kinematic constraints and minimum-time problem are all taken into account in both Cartesian space and joint space, the precision and efficiency of robot are improved effectively. The experimental results on a 4-DOF serial collaborative robot show that the robot maintains the high efficiency for about 75% of whole movement under kinematic constraints. Therefore, the effectiveness and practicability of the proposed method is verified.