Time-jerk optimal trajectory planning of hydraulic robotic excavator

Due to the fact that intelligent algorithms such as Particle Swarm Optimization (PSO) and Differential Evolution (DE) are susceptible to local optima and the efficiency of solving an optimal solution is low when solving the optimal trajectory, this paper uses the Sequential Quadratic Programming (SQP) algorithm for the optimal trajectory planning of a hydraulic robotic excavator. To achieve high efficiency and stationarity during the operation of the hydraulic robotic excavator, the trade-off between the time and jerk is considered. Cubic splines were used to interpolate in joint space, and the optimal time-jerk trajectory was obtained using the SQP with joint angular velocity, angular acceleration, and jerk as constraints. The optimal angle curves of each joint were obtained, and the optimal time-jerk trajectory planning of the excavator was realized. Experimental results show that the SQP method under the same weight is more efficient in solving the optimal solution and the optimal excavating trajectory is smoother, and each joint can reach the target point with smaller angular velocity, and acceleration change, which avoids the impact of each joint during operation and conserves working time. Finally, the excavator autonomous operation becomes more stable and efficient.


Introduction
Hydraulic excavators are widely used in extremely harsh environments for mining, transportation, and civil engineering. 1,2 However, the following problems persist regarding the process of work: (1) the high intensity of labor required from the operators, (2) a dangerous working environment, and (3) the large amounts of money, material resources, and time required for an operator to develop the requisite skill. Thus, unmanned excavators have gradually replaced manually operated ones. 3,4 Intelligent excavators can be used for submarine operations and earthquake relief work as well as at nuclear power plants. Improving the operating efficiency and reliability of the excavator is an important research subject. Trajectory planning is the premise and foundation for realizing control of the excavation trajectory. An appropriately planned trajectory enables quick and smooth completion of the excavation task. In particular, a large jerk value for each joint during operation results in unsmooth motion, which has a significant negative impact on the hydraulic cylinder. This also reduces the service life of the machine and damages the equipment. [5][6][7][8] To ensure the speed and stability of excavator in independent operation, the planning of time-jerk optimal excavation trajectory has become a research hot-spot. In the trajectory planning method with optimal time or smoothing as the goal, Liu et al. 9 applied interpolation in joint space using B-splines and also employed SQP for optimizing the minimum time trajectory. However, they did not consider the adverse effects of larger jerk on the work equipment. Boryga and Grabos´1 0 used a high-order polynomial with only one unknown parameter to achieve time-optimal trajectory planning but presupposed that the maximum acceleration value of the terminal endpoint moving along the path must be given. Elnagar and Hussein 11 used the numerical iterative method to obtain the optimal trajectory of a robot in energy consumption in a 3D environment with certain boundary conditions. Lin et al. 12 projected the minimum time trajectory of an industrial robot by using a polyhedron search. By imposing constraints on velocity, acceleration, and jerk, cubic splines have been used to interpolate optimal trajectory in the joint space. Piazzi and Visioli 13 proposed interval analysis, which requires presetting the total time of the trajectory in order to obtain the optimal trajectory. Gasparetto and Zanotto, 14 Gasparetto et al., 15 and Zanotto et al. 16 used cubic splines to plan a multiobjective optimal trajectory and verified it for a robot through experimentation. Wang et al. 17 uses the Beetle Swarm algorithm to optimize the minimum time and joint rotation angle optimal trajectory. Zhang et al. 18 solved the problem of non-convex global optimal trajectory planning and produced an efficient and continuous trajectory. For the point-to-point optimal trajectory planning problem, Wang et al. 19 obtained the point-to-point time optimal trajectory based on the expression of interpolation in the form of multiple roots. However, each joint obtained by the solution does not synchronously reach the limits of their allowable angular velocity, acceleration, and jerk. Qian et al. 20 improved the B-spline interpolation method and optimal the time-jerk trajectory. Fang et al. 21,22 use the sigmoid piecewise function and the improved sinusoidal function to achieve interpolation. According to the given angular velocity, acceleration, and jerk constraints of each joint, they acquired the synchronized motion curve. Zhao et al. 23 solve the optimal trajectory planning with time-jerk as the optimization objective based on the optimal control method. In addition, an optimal trajectory was planned by considering the hybrid objective of optimizing time, energy, and jerk. 24,25 During this process, the robots efficiency, stability, and energy consumption of the robot during the operation were comprehensively considered. Kim et al. 26 regarded velocity and acceleration as given values to optimize time-torque trajectory planning of the hydraulic excavator. Xiao et al. 27 used cubic splines to fit joint angles in joint space to conduct online form time-optimal trajectory planning for industrial robots.
In this paper, the Lipai PC1012 hydraulic robotic excavator is taken as the research object. To ensure the efficiency and stability of the excavator in the working process, this paper takes the angular velocity, angular acceleration, and jerks as the constraints, and uses the SQP, PSO, 28 and DE 29 algorithms to optimize the timejerk optimal cubic spline interpolation trajectory. The experimental results show that the mining trajectory obtained by the SQP algorithm is more efficient and smooth, and the SQP algorithm more efficient to obtain the optimal solution.
The remainder of this paper is organized as follows. Section 2 briefly introduces joint trajectory using cubic spline parameterization. Section 3 establishes the optimization model of the multi-objective function and the process of using SQP algorithm optimization is introduced. Section 4 simulates the trajectory planning problem using different weight coefficients and also analyzed the optimal trajectory obtained through SQP, DE, and PSO optimization. Finally, Section 5 summarizes the conclusions.

Parameterized trajectory using cubic splines
Cubic spline interpolation is a universal method that can ensure the succession of acceleration in trajectory planning, and the obtained trajectory is smoother and continuous. Compared with the NURBS interpolation method, 30 the calculation amount and complexity of the cubic spline are small. And it can also help avoid Runge's phenomenon that results in excessive oscillations and collisions in the case of high-order polynomial interpolation. In this paper, cubic splines are used for optimal trajectory planning in joint space.
Taking the j À th joint as an example, s m indicates a series of the given joint angles and every two adjacent joint angle values are connected by cubic splines. t 1 , t 2 , :::::, t n are the time required for passing through each interpolation point (n = s m + 2). The time interval between continuous interpolation points is h i = t i + 1 À t i . The expressions of joint acceleration on the interval are linear functions of t.
By integrating equation (1) twice with respect to the given boundary conditions Q j, i (t i ) = q j, i and Q j, i (t i + 1 ) = q j, i + 1 . The expression for joint displacement Q j, i (t) can be obtained as follows.
where A is nonsingular and diagonal about the interpolation time interval, a j is the acceleration at each interpolation point, and O j is a vector of the joint displacement and interpolation time interval.

Modeling and trajectory optimization
To improve the efficiency of completing a given task, Wang et al. 29 took time as the optimization goal and conducts time-optimal trajectory planning and to improve the smoothness of the trajectory, Lu et al. 31 took jerk as the performance index to obtain a smooth optimal trajectory. While Huang et al. 24 comprehensively considers multiple factors and establishes a multi-objective trajectory planning. So in this paper, to ensure the efficient and smooth motion of each joint, the problems of time and jerk were considered simultaneously for determining the optimal trajectory. The optimal objective function is as follows.
In equation (7), the first item is the total time needed to complete the task, which is related to efficiency. The second item is the total jerk at each joint, which is related to the problem of smoothness. The w 1 is the weight coefficient. When w 1 is small, the optimal trajectory focuses on the influence of jerk on the equipment, where VC j , WC j , JC j are related to the constraints on the velocity, acceleration, and jerk, respectively, of the revolute, boom, arm, and bucket joints.
It can be seen from equations (3)-(7) that the parameters to be solved in the expression are closely related to the interpolation time of each segment of the excavation path when calculating the cubic spline expression. The SQP is used to find the optimal value of each interpolation time. When each optimal interpolation time can be optimized, the expression of the joint angle curve can be obtained. Through time-jerk optimal trajectory planning, the excavator can reach the target point with a small range of angle changes in the actual moving process, so as to realize the completion of the excavation task in a short time and ensure the smooth operation of each joint in the working process to the servo control of the control system. By limiting the angular velocity, acceleration, and jerk of each joint in the process of moving, the negative effect on the hydraulic cylinder can be avoided in the process of operation.
The time intervals h i used as optimization variables, which are solved by SQP, must have appropriate initial values. Further, each value of h i has a lower bound that should be satisfied by equation (8).
Based on the values of k 1 , k 2 , k 3 , the suitable initial values h 0 can be obtained as follows.
The trajectory planning problem is a nonlinear optimization problem. This problem is transformed into the Quadratic Programming (QP) problem to solve it using SQP. Through Taylor expansion, the objective function can be reduced to a quadratic function at the iteration point h k and the constraint functions can be reduced to linear functions. Further, the QP problem becomes a problem with respect to variable H as follows. where The specific steps for solving the optimization problems are as follows and the corresponding flowchart is shown in Figure 1.
Step 2: Simplify point h 0 to a QP problem.
Step 3: Solve the QP problem and consider H k = H * .
Step 4: Execute the constrained one dimensional search of the objective function along the direction H k and output point h k + 1 . Step 5: If h k + 1 satisfies the given precision, that is, H j jł e, terminate the iteration. Otherwise, go to Step 6.
Step 6: Modify the Hessian matrix X k + 1 using the method as shown in equation (12). where , order k = k + 1; then, return to Step 2.

Simulation and analysis of results
The PC1012 hydraulic robotics excavator is used as a research object, shown in Figure 2.
According to the D-H coordinate system method, 32 the excavator working device model is established, as shown in Figure 3. The relevant D-H parameters are listed in Table 1. This method expresses the transformation relationship between the coordinate systems by deriving the homogeneous transformation matrix between the coordinate systems. In Figure 3, the transformation matrix between the link joints is where , and a 1 , a 2 , a 3 , a 4 are the joint lengths of the revolute, boom, arm, and bucket. And the coordinates of the end position relative to the base coordinate system are where 0 A 4 = 0 A 1 1 A 2 2 A 3 3 A 4 , n P = ½0, 0, 0, 1 T : Set the position and posture coordinate point of the tip of the bucket as ½x, y, z, z T . According to equation (14), the expression of the bucket tooth tip relative to the basal coordinate is shown below.
where j is the attitude angle of bucket tooth tip. In Table 1, joint 1 represents the rotary platform, joint 2 is the boom, joint 3 is the arm, and joint 4 is the bucket.
Due to the limited range of the driving mechanism, according to the limited range of the angles of each joint in Table 1, without considering rotation (y = 0), each joint of the excavator moves at the same time. The allowable working space at the end of the bucket tooth tip is shown in Figure 4. This paper takes digging as an example and selects the excavation path point in the working range allowed by the working device of the excavator. The mining path points are directly given by the coordinate points at the end of the bucket tooth tip, while the trajectory is planned according to the corresponding angle values of each joint. Therefore, the coordinate value of the path point must be converted into the angle value of each joint corresponding to the coordinate point. This process realizes the conversion from the coordinate space to the   joint space through the inverse kinematics solution. According to equation (16), the analytical method is used to obtain the joint angle values of the boom, arm, and bucket corresponding to the excavation path point. And the corresponding results are shown in Table 2.
There are two extra points in Table 2. They are not given and merely applied to satisfy the continuous condition of velocity and acceleration. Its expressions are as follows.
In the process of obtaining the optimal solution, the constraint conditions of joint angular velocity, acceleration, and jerk are shown in Table 3.
Under the same constraint conditions, SQP, PSO, and DE algorithms are used successively to optimize the best trajectory of time-jerk under different weight coefficients. The optimization results are shown in Table 4. In the same configuration conditions, Table 5 shows the time taken to find the best trajectory of the time-jerk of the cubic spline curve using these three algorithms.
When w 1 = 1, the optimal joint angle, velocity, acceleration, and jerk curves of the boom, arm, and bucket solved by the SQP are shown in Figure 5.
Substituting the optimal solution corresponding to w 1 = 0:6 into the equations (3)-(6), the expression of the joint angle is obtained. The joint angle expressions of the boom, arm, and bucket obtained by SQP, PSO, and DE algorithms as follow.
It can be seen from Figures 6 to 8 that the cubic spline interpolation function uses SQP, PSO, and DE to find the optimal interpolation time that satisfies the constraints and obtains the smooth and continuous curves of the joint angle, velocity, and acceleration which velocity, acceleration, and jerk are within the constrained range. This shows that these three algorithms can be used to optimize the trajectory of the excavator under certain constraints. Secondly, it can be seen from Figure 5 that when only the efficiency problem is considered, although the time to complete the task is short, the jerk value of each joint is large compared to Figures 6 to 8.
For the time-jerk optimal joint angle curve obtained by the optimal solution, the corresponding joint angle value can be obtained from the corresponding time. Therefore, in the optimal joint angle curve obtained by the above optimization solution, under the premise that the time is known, substitute the optimal joint angle value of w 1 = 0:6 into the simulation model of the excavator working device, and the digging trajectory of the tip of the bucket tooth is shown in the Figure 9's mining path. It can be seen from Figure 9 that the second, third, and fifth parts of the excavation trajectory Figure 7. The w 1 = 0:6 optimal joint angle, velocity, acceleration, and jerk curves of the boom, arm, and bucket solved by the PSO: (a) the optimal joint angle of boom, arm, and bucket, (b) the optimal joint velocity of boom, arm, and bucket, (c) the optimal joint acceleration of boom, arm, and bucket, and (d) the optimal joint jerk of boom, arm, and bucket.
generally overlap, but from the trajectory of the first and fourth segments shown in the Figure 10, it can be clearly seen that the best trajectory of the time-jerk obtained by the SQP method is relatively smooth and each joint has completed the given excavation task with the smallest joint angle change, which has helped reduce the wear of hydraulic cylinders and protects the equipment.

Conclusions and Summary
In this paper, the joint trajectory planning problem of hydraulic robotics excavators is studied. Firstly, the performance indexes considering both jerk and time are established, and the optimal cubic spline interpolation trajectory concerning the problems of time and jerk are obtained by using the three algorithms of the SQP, PSO, and DE. The results of the time-jerk optimal trajectory solution and the efficiency of obtaining the optimal solution show that the optimal trajectory of time obtained by the SQP under the same weight coefficient is the shortest, and the jerk value is relatively small. The results of this experiment show that it can Figure 8. The w 1 = 0:6 optimal joint angle, velocity, acceleration, and jerk curves of the boom, arm, and bucket solved by the DE: (a) the optimal joint angle of boom, arm, and bucket, (b) the optimal joint velocity of boom, arm, and bucket, (c) the optimal joint acceleration of boom, arm, and bucket, and (d) the optimal joint jerk of boom, arm, and bucket. beneficial to reduce the impact and vibration in the process of motion, ensure the smooth continuity of the trajectory and the stability of the movement, improve the tracking accuracy of the trajectory, but also can protect the mechanical structure, reduce mechanical wear, prolong the service life of the equipment.
Secondly, the experimental results show that the greater the weight of time is, the greater the jerk value of each joint will be in the optimization process. Conversely, the greater the weight of the jerk, the longer the time it takes to complete a given task. Therefore, in engineering applications, the problems of time and jerk should both be considered, according to the given circumstances.
In further research, the existence of obstacles must be considered in the process of trajectory planning, perfect the actual working environment of hydraulic excavators, and improve the equipment's ability to work autonomously in complex environments.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) received no financial support for the research, authorship, and/or publication of this article.