Spline-Based Optimal Trajectory Generation for Autonomous Excavator

Abstract

In this paper, we propose a novel trajectory generation method for autonomous excavator teach-and-plan applications. Rather than controlling the excavator to precisely follow the teaching path, the proposed method transforms the arbitrary slow and jerky trajectory of human excavation into a topologically equivalent path that is guaranteed to be fast, smooth and dynamically feasible. This method optimizes trajectories in both time and jerk aspects. A spline is used to connect these waypoints, which are topologically equivalent to the human teaching path. Then the trajectory is reparametrized to obtain the minimum time-jerk trajectory with the kinodynamic constraints. The optimal time-jerk trajectory generation method is both formulated using nonlinear programming and conducted iteratively. The framework proposed in this paper was integrated into a complete autonomous excavation platform and was validated to achieve aggressive excavation in a field environment.

1. Introduction

Excavators are a widely used construction machinery that play a vital role in transportation construction, mining engineering, disaster relief and other scenarios. Nowadays, excavators are controlled by skilled operators, thereby heavily relying on human operators with a low level of automation and low efficiency; in addition, the working environment is high-risk. Therefore, improving the automation of excavators can enable their operation in infeasible sites and reduce the injury to their operators. In addition, the adoption of autonomous construction robot technology helps to achieve autonomous construction [1]. Robotic task decision and motion planning technology can handle repetitive tasks, which can free operators from repetitive and tedious work and save on construction costs.

In general, the operating environment of the autonomous robot system does not have much variation. However, when the operating environment or conditions of the system change, a higher-level ability of decision making and planning is needed to adapt to these changes [2]. For this adaptability, the behavior and decision-making processes of skilled operators for a specific task may have great utility. In some related studies, the motion of the bucket was usually divided into three stages [3,4]: bucket penetration, bucket dragging and bucket lifting [5]. It should be noted that although these studies make some divisions in and modeling of the excavation process, operation skill is not considered. Sakaida et al. [6,7] developed a system to analyze the operation skill (e.g., when the excavation task changes from one stage to the next stage) of skilled operators by measuring the pose (i.e., position and orientation) of the working device and the trajectory of the bucket teeth, according to which the corresponding excavation rules were established. While these studies did not consider the subsequent optimal trajectory generation, we often expect excavation trajectory to be fast, smooth and efficient. Thus, excavation motion that takes full advantage of the dynamic feasibility of the excavator is required; however, it is difficult to achieve for a skilled operator. Some studies [8] generate the optimal excavation trajectory using waypoints that are given in advance, as well as some physical constraints. However, this method does not consider how to select waypoints and the dynamic feasibility constraints (e.g., velocity limit and acceleration limit) [9,10]. Therefore, the aggressiveness of the excavator cannot be fully utilized, and efficient motion cannot be generated.

To bridge this gap, in this paper we investigate the best way to incorporate the excavation skill of a skilled operator and a flexible, robust and complete trajectory generation system. First, trajectories demonstrated by a skilled operator for specific excavation tasks were recorded. Then, a set of waypoints that preserves the topological information of the manual excavation trajectory was found [11]. Finally, the optimal time-jerk trajectory was iteratively generated to meet the dynamic feasibility. Under the planning framework of this paper, the time-jerk of the generated trajectories is easily adjusted. For precision excavation applications, a smooth and slow trajectory can be generated by setting the preference on motion smoothness (i.e., reduce jerk). In extreme cases such as fast loading operations, an unskilled operator can use their simple operation to teach the excavator the rough path of the expected motion; fully aggressive excavation may even beat a skilled operator.

The trajectory generation method proposed in this paper is based on the modeling of manual excavation and fast, smooth and efficient trajectory optimization technology. The proposed method finds waypoints through manual teaching and then obtains the optimal motion through the trajectory generation framework. Furthermore, this method was tested on our autonomous excavator platform (as shown in Figure 1), and compared with the manual operation to validate its feasibility. The main contributions of this paper are summarized as follows:

• A method of finding topologically equivalent waypoints around the teaching excavation motion of a skilled operator;
• A spline-based trajectory optimization framework for generating optimal time-jerk optimality and satisfying kinodynamic feasibility;
• The sensing system, trajectory generation framework and control model are integrated into the autonomous excavator platform, and the aggressive excavation motion is presented in a field test.

The rest of the paper is organized as follows: Section 2 reviews related literature works. The modeling of the system and the trajectory optimization approach are detailed in Section 3 and Section 4, respectively. Section 5 presents the experimental results of the optimal motion generated using our method with the manual excavation motion performed by a skilled operator. Our conclusion and future work are summarized in Section 6.

2. Related Work

There are currently two directions for realizing the motion/trajectory planning of autonomous excavators. One is to model the excavation process based on the skills of skilled operators, then generate some rules that can be used as commands to control autonomous excavation. The other is to generate an optimal motion under dynamic constraints or physical constraints. Here, this paper overviews representative works that are especially relevant to the motion planning of autonomous excavators.

Previous studies have compared some differences in skill between skilled operators and unskilled operators when excavating with regard to specific tasks [6,7]. Some rules for autonomous excavation were extracted via analysis of angle changes in the working device (i.e., swing, boom, stick and bucket). Further, Yamaguchi and Yamamoto [12] transformed these rules into commands to precisely track the excavation motion of skilled operators [13]. Koiwai et al. [14] proposed a method for evaluating and quantifying skills in manual excavation by correlating the performance of different operators and the variations in PID controller parameters. Sekizuka et al. [15] analyzed some geometric indicators (e.g., length of bucket trajectory) of the excavation trajectory through statistical analysis of specific tasks to assess operation skills. Du et al. [2,16] investigated the excavation strategies of expert human operators and their decision-making processes. Then they summarized some rules for when the excavator changed from one stage to the next stage (i.e., bucket fill, bucket lift, swing to dumping, dumping and swing to trench) based on the variation of the joint angle (i.e., swing, boom, stick and bucket) and the trajectory of the bucket teeth [2]. Finally, these were integrated into virtual operator models to control the excavator to complete the trench task.

The above methods provide a way to convert the skills of skilled operators into some rules that can be used to find an excavation path for a specific task. However, the generated trajectory via these methods is generally not the optimal motion; thus, the machine is unable to carry out excavation work quickly and efficiently. Yoo et al. [17] proposed a trajectory optimization method of minimum torque by parametrically representing the trajectory using a B-spline and solving for the optimal trajectory under the maximum flow constraint of the cylinder. Based on previous work [17], Kim et al. [9] obtained the minimum torque trajectory with the constraints of the soil-tool interaction dynamics. Zou et al. [18] formulated a trajectory optimization problem of minimum time, minimum energy consumption and minimum machine damage, which obtained the excavation trajectory under constraints such as kinematics and cylinder maximum pressure. Guan et al. [19] proposed an optimal time trajectory generation method that used NURBS to parameterize the trajectory to generate optimal trajectories that satisfied dynamic constraints. A method using the 4-3-3-3-4 polynomial, which can generate a time-optimal trajectory under the velocity and acceleration constraints, was proposed [20]; however, this method reduced numerical computation efficiency and numerical stability. After that, Zhang et al. [8] adopted cubic splines to represent the excavation trajectory and obtained a time-jerk optimal trajectory. Yang et al. [10] proposed a trajectory optimization framework for a specific excavation task, using a mixed-order spline (5-3-5 polynomial) as an interpolating spline to generate a trajectory under a swept volume and bucket instantaneous direction; they further developed a minimal torque trajectory considering the soil-tool interaction [21]. In addition, MPC (model predictive control) [22] was also used to track the optimal trajectory. Lee et al. [23] used a global planner to generate a reference trajectory that satisfied the maximum bucket volume and minimum force cost, then applied a local planner based on model predictive control to track the reference trajectory. A planning method based on DMP (dynamic movement primitives) that effectively emulated the strategies of experts was developed by Son et al. [24]. However, since these methods cannot explicitly consider constraints (e.g., kinodynamic constraints), the violation of machine physics constraints can occur. At present, there are some new platforms of the trajectory planning of autonomous excavation, such as walking excavators [25,26,27].

Although there are various trajectory planning methods for autonomous excavators in the previous literature, a complete framework has not yet emerged to complete the manual excavation modeling of a skilled operator while incorporating optimal time and smooth trajectory. The strong need for such a framework stems from the fact that high-quality motion requires a combination of skilled operators’ decision making on excavation tasks under changing working conditions. To address this gap, this paper draws on the ideas from [8,10,13] to propose a framework specialized for excavators, exploring and exploiting the excavation skills of skilled operators and trajectory generation.

3. Manual Excavation Model

3.1. System Architecture

The hardware setting of the system is shown in Figure 1. The autonomous excavator is equipped with four inclination sensors and an absolute encoder to measure the angle of the working device (boom, bucket, and stick) and the swing. Three pairs of pressure sensors are used to monitor the pressure in the hydraulic cylinders (boom, stick and bucket) of the work equipment. Computers are used for processing feedback signals of the controller, monitoring pressure changes of cylinders and generating the optimal excavation trajectory. The controller is used to control the electro-hydraulic proportional valve and transmit the sensor signal to the computer.

The overall software architecture of our system is shown in Figure 2. The trajectory execution module was run on a controller, while others were run on the computer. Before the excavation, waypoints were generated according to the topological information of the manual excavation trajectory, and then a reference optimal trajectory was generated within machine kinodynamic constraints. During the excavation, the sensor measured the angle of the working device in real time. Then, the state measurement module preprocessed the data in real time and monitored the pressure changes of the working device. Finally, we used a feedback controller to track the optimal trajectory in real time.

3.2. Excavator Kinematics

The generation of the optimal excavation trajectory was generally conducted in the joint space and $({\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4)$ was utilized to represent the joint angle of the swing, boom, arm and bucket in the joint space. However, the path planning was conducted in the pose space (i.e., task space) according to the excavation task, that is, planned according to the pose $O_4(x,y,z,\zeta )$ of the bucket tooth tip; it was therefore necessary to establish a kinematic model from the joint space to the pose space. Figure 3 shows the Denavit–Hartenberg coordinate system (D–H coordinate system) [28] of the autonomous excavator. The bucket teeth pose is calculated as:

$$\{\begin{array}{l}x=a_4\cos \left({\theta }_2+{\theta }_3+{\theta }_4\right)\cos {\theta }_1+a_3\cos \left({\theta }_2+{\theta }_3\right)\cos {\theta }_1+a_2\cos {\theta }_2\cos {\theta }_1+a_1\cos {\theta }_1,\hfill \\ y=a_4\cos \left({\theta }_2+{\theta }_3+{\theta }_4\right)\sin {\theta }_1+a_3\cos \left({\theta }_2+{\theta }_3\right)\sin {\theta }_1+a_2\cos {\theta }_2\sin {\theta }_1+a_1\sin {\theta }_1,\hfill \\ z=a_4\sin \left({\theta }_2+{\theta }_3+{\theta }_4\right)+a_3\sin \left({\theta }_2+{\theta }_3\right)+a_2\sin {\theta }_2+d_1,\hfill \\ \zeta ={\theta }_2+{\theta }_3+{\theta }_4,\hfill \end{array}$$

(1)

3.3. Manual Excavation Modeling and Waypoint Finding

3.3.1. Manual Excavation Modeling

To obtain topological information on the excavation path of the skilled operator and find the waypoints, we established the excavation rules of the skilled operator for the trench task (trapezoidal trench). Figure 4 shows a diagram of the bucket teeth path for the five stages for trenching. For trench excavation, the complete operation process can be divided into five stages: bucket penetration, bucket dragging, bucket lifting, swinging to the dumping location and swinging to the trenching location [2].

The excavation rules were extracted from the trajectory generated by a skilled operator with more than 10 years of excavation experience. In the stage of penetration, the initial position of the bucket was selected as the position where the bucket was easy to move to the penetration point, and the initial orientation of the bucket was a direction with less soil cutting resistance (the orientation between bucket teeth and ground is about 30° to 60°). In the stage of bucket dragging, the bucket was kept full of soil and horizontal dragging was quickly completed. Third, in the bucket lifting stage, after the second step was completed, the bucket was lifted and rotated to keep the excavated material from falling out of the bucket. During the swing to the dumping location, the bucket pose was kept unchanged and quickly swung to the dumping location (the rotation was set to 90° counterclockwise), slowing down as needed when approaching the dumping location. During the swing to the trenching location, the working device maintained the pose of the previous stage; it adjusted to the pose when approaching the initial penetration location.

3.3.2. Waypoint Finding

After the trajectories of the skilled operators were generated based on the excavation rules, we analyzed the topological information of the trajectory in the joint space. As shown in Figure 5, firstly, the data was processed with the signal alignment method [29] to keep the data length consistent, because operators could not guarantee that the operation state of all tests was completely consistent during the operation. Then, a moving average filter was used to reduce the noise generated by the jerk of the cylinders during excavation. The use of a moving average filter can reduce the interference of noise on the excavator trajectory. Finally, the paths of skilled operators were averaged to obtain a statistically average excavation path [15].

Since waypoint finding needed to be conducted in the pose space, we needed to transform the obtained joint space trajectory into the pose space. In addition, the excavation path obtained by the skilled operator was extremely poor, as its geometric shape and time distributions were far from optimal. Thus, they were useless or even harmful for trajectory optimization. However, the topological information of the manual excavation paths was essential as it reflected the skilled operator’s intention (e.g., fast and stable excavation). To preserve the topological information of the path and improve the efficiency and quality of the trajectory optimization, this paper proposes an intelligent waypoint selection strategy to obtain a sparse excavation path, these waypoints providing a large degree of freedom for trajectory optimization. The waypoint selection policy borrows an idea from the Douglas–Peucker algorithm [30]. Once all the waypoints are found, these can be used to generate an optimal trajectory that satisfies the dynamic feasibility.

4. Spline-Based Trajectory Optimization

4.1. Problem Formulation

Through the method in Section 3, the topological structure (i.e., waypoints) of the excavation path was obtained. However, optimal trajectories could not be tracked by the controller without trajectory smoothing and time optimization. Therefore, we used a piecewise polynomial to connect these waypoints and formulate a trajectory optimization problem that satisfied kinodynamic feasibility.

The trajectory consisting of piecewise polynomials is parameterized by the time variable $t$ in each joint space. The $n$-segment $k$ degree polynomial trajectory of the $j$th joint can generally be expressed in the following form:

$$Q_j(t)=\{\begin{array}{ll}{c}_1^{\mathrm{T}}\beta (t),\hfill & t\in [t_0,t_1],\hfill \\ c_2^{\mathrm{T}}\beta (t),\hfill & t\in [t_1,t_2],\hfill \\ \hfill ⋮\hfill & \hfill ⋮\hfill \\ c_n^{\mathrm{T}}\beta (t),\hfill & t\in [t_{n-1},t_n],\hfill \end{array}$$

(2)

where $c_i^{\mathrm{T}}=(c_{i,0},c_{i,1},\cdots ,c_{i,k})$ is the $i$th segment polynomial coefficient vector and $\beta (t)={(1,t,\cdots ,t^k)}^{\mathrm{T}}$ is the time vector.

Excavation is always expected to be as fast as possible, however, this leads to a final state of excavation that is kinodynamically infeasible (i.e., exceeding velocity or acceleration limits), such that the acceleration at the final point is not zero, resulting in a jerk of the mechanical devices. Therefore, we chose the objective function as a trade-off between minimizing the third-order derivative (i.e., jerk) and minimizing time. The general trajectory optimization problem was formulated as:

$$\begin{array}{cc}\min \hfill & J={\displaystyle {\int }_0^{t_n}{\left({\stackrel{⃛}{Q}}_j(t)\right)}^2\mathrm{d}t}+\rho t_n,\hfill \\ \mathrm{s}.\mathrm{t}.\hfill & Q_j\left(t_0\right)=q_{j,0}, Q_j^{\left(1\right)}\left(t_0\right)=v_{j,0}, Q_j^{\left(2\right)}\left(t_0\right)=a_{j,0},\hfill \\ \hfill & Q_j\left(t_n\right)=q_{j,n}, Q_j^{\left(1\right)}\left(t_n\right)=v_{j,n}, Q_j^{\left(2\right)}\left(t_n\right)=a_{j,n},\hfill \\ \hfill & \left|{\dot{Q}}_j(t)\right|\le V_{j\max }, \left|{\ddot{Q}}_j(t)\right|\le A_{j\max }, \forall t\in \left[t_0,t_n\right],\hfill \\ \hfill & Q_{j,i}^{\left(s\right)}\left(t_i\right)=Q_{j,i+1}^{\left(s\right)}\left(t_i\right), s\in \mathbb{N}, i=1,\cdots ,n-1,\hfill \end{array}$$

(3)

where $\rho$ is the weight of time; $q_{j,0}$, $v_{j,0}$ and $a_{j,0}$ are the initial position, velocity and acceleration, respectively; $q_{j,n}$, $v_{j,n}$ and $a_{j,n}$ are the final position, velocity and acceleration, respectively; and $V_{j\max }$ and $A_{j\max }$ are the kinodynamic limits.

4.2. Spline Parameterization of Excavation Trajectory

Trajectory optimization problems are usually nontrivial due to the nonlinear constraints in Equation (3), so we needed to make some reasonable choices to make the problem solvable [31]. In this research, the joint trajectories of the excavator manipulator were obtained by using a cubic spline to sequentially pass through the waypoints [32].

For the $j$th joint, $Q_{j,i}(t)$ is a cubic spline in the interval $[t_i,t_{i+1}]$; thus, if ${\ddot{Q}}_{j,i}(t)$ is a linear function in $[t_i,t_{i+1}]$ and $h_i=t_{i+1}-t_i$ is the time interval, then ${\ddot{Q}}_{j,i}(t)$ can be written as:

$$\begin{array}{c}{\ddot{Q}}_{j,i}(t)= \frac{t_{i+1}-t}{h_i}{\ddot{Q}}_{j,i}(t_i)+\frac{t-t_i}{h_i}{\ddot{Q}}_{j,i}(t_{i+1})\end{array}.$$

(4)

By integrating Equation (4) twice, and by setting $Q_{j,i}(t_i) = q_{j,i}$ and $Q_{j,i}(t_{i+1}) = q_{j,i+1}$, $Q_{j,i}(t)$ can be obtained:

$$\begin{array}{cc}\hfill Q_{j,i}(t)=& \frac{{\ddot{Q}}_{j,i}(t_i)}{6h_i}{\left(t_{i+1}-t\right)}^3+\frac{{\ddot{Q}}_{j,i}(t_{i+1})}{6h_i}{\left(t-t_i\right)}^3\hfill \\ \hfill & +\left(\frac{q_{i+1}}{h_i}-\frac{h_i{\ddot{Q}}_{j,i}(t_{i+1})}{6}\right)\left(t-t_i\right)\hfill \\ \hfill & +\left(\frac{q_i}{h_i}-\frac{h_i{\ddot{Q}}_{j,i}(t_i)}{6}\right)\left(t_{i+1}-t\right).\hfill \end{array}$$

(5)

Differentiating Equation (5), the ${\dot{Q}}_{j,i}(t)$ and ${\stackrel{⃛}{Q}}_{j,i}(t)$ can be expressed as:

$$\begin{array}{cc}\hfill {\dot{Q}}_{j,i}(t) =& -\frac{{\ddot{Q}}_{j,i}(t_i)}{2h_i}{\left(t_{i+1}-t\right)}^2+\frac{{\ddot{Q}}_{j,i}(t_{i+1})}{2h_i}{\left(t-t_i\right)}^2\hfill \\ \hfill & +\left(\frac{q_{i+1}}{h_i}-\frac{h_i{\ddot{Q}}_{j,i}(t_{i+1})}{6}\right)\hfill \\ \hfill & +\left(\frac{q_i}{h_i}-\frac{h_i{\ddot{Q}}_{j,i}(t_i)}{6}\right),\hfill \end{array}$$

(6)

$$\begin{array}{c}{\stackrel{⃛}{Q}}_{j,i}(t)= \frac{1}{h_i}\left({\ddot{Q}}_{j,i}(t_{i+1})-{\ddot{Q}}_{j,i}(t_i)\right)\end{array}.$$

(7)

According to Equation (6) and the continuity of velocity ${\dot{Q}}_{j,i}(t_i) = {\dot{Q}}_{j,i-1}(t_i)$, we obtained:

$$\begin{array}{l}{h}_{i-1}{\ddot{Q}}_{j,i-1}(t_{i-1})+2\left(h_{i-1}+h_i\right){\ddot{Q}}_{j,i}(t_i)+h_i{\ddot{Q}}_{j,i}(t_{i+1})\\ = \frac{6}{h_i}\left(\frac{q_{i+1}-q_i}{h_i}-\frac{q_i-q_i}{6}\right), i=2,\cdots ,n-1.\end{array}$$

(8)

The initial velocity and acceleration are specified, and the final velocity and acceleration are also specified values. In addition, two virtual points are set at the second and penultimate positions, respectively [28], giving enough freedom for solving the optimization problem while considering the continuity of velocity and acceleration. Consequently:

$$q_{j,2}= q_{j,1}+h_1{v}_{j,1}+\frac{h_1^2}{3}{a}_{j,1}+\frac{h_1^2}{6}{\ddot{Q}}_{j,1}\left(t_2\right),$$

(9)

$$q_{n-1}= q_n-h_{n-1}{v}_n+\frac{h_{n-1}^2}{3}{a}_n+\frac{h_{n-1}^2}{6}{\ddot{Q}}_{n-2}\left(t_{n-1}\right).$$

(10)

Now, the following equation can be concluded from Equations (6)–(8):

$$\begin{array}{cc}K{a}_j= b_j,& j=1,\cdots ,N,\end{array}$$

(11)

where $a_j$, $K$, $b_j$ are detailed in Appendix A.

To obtain the optimal time and smooth trajectory, the objective function was parameterized using a cubic spline.

A. Object function

$$\begin{array}{cc}\min & K_{T}N{\displaystyle \sum _{i=1}^{n-1}{h}_i}+\alpha K_J{\displaystyle \sum _{j=1}^N{\displaystyle {\int }_0^{t_f}{\left({\stackrel{⃛}{Q}}_j\left(t\right)\right)}^2\mathrm{d}t}}\end{array},$$

(12)

where $N$ is the number of joints, $t_f$ is the total excavation time, $K_T$ is the weight of time, $K_J$ is the weight of jerk, increasing $K_T$ will cause an aggressive trajectory and $\alpha$ is the elastic coefficient, which was introduced to balance this difference between time and jerk, as jerk and time usually have a huge difference in the order of magnitude that results in the role of the time being weakened.

Since the jerk ${\stackrel{⃛}{Q}}_{j,i}$ is a constant value in each time interval, substituting Equation (7) into Equation (12) yields the following equation:

$$\begin{array}{cc}\min & K_{T}N{\displaystyle \sum _{i=1}^{n-1}{h}_i}+\alpha K_J{\displaystyle \sum _{j=1}^N{\displaystyle \sum _{i=1}^{n-1}\left[\frac{{\left({\ddot{Q}}_{j,i+1}-{\ddot{Q}}_{j,i}\right)}^2}{h_i}\right]}}\end{array}.$$

(13)

B. Velocity constraints: $\left|{\dot{Q}}_{j,i}\left(t\right)\right|\le V_{j\max }, j=1,\cdots ,N; i=1,\cdots ,n-1$

The velocity is a quadratic function in the time interval $[t_i,t_{i+1}]$; the maximum absolute velocity exits at $t_i$, $t_{i+1}$ or ${\ddot{Q}}_{j,i}\left(t_i^{*}\right)=0$. Thus, the velocity constraint can be written as:

$$\max \left|{\dot{Q}}_{j,i}\left(t\right)\right|=\max \left\{\left|{\dot{Q}}_{j,i}\left(t_i\right)\right|, \left|{\dot{Q}}_{j,i}\left(t_i^{*}\right)\right|, \left|{\dot{Q}}_{j,i}\left(t_{i+1}\right)\right|\right\}\le V_{j\max },$$

(14)

where

$${\dot{Q}}_{j,i}\left(t_i\right)=-\frac{h_i}{2}{\ddot{Q}}_{j,i}\left(t_i\right)+\frac{q_{j,i+1}-q_{j,i}}{h_i}+\frac{h_i}{6}\left({\ddot{Q}}_{j,i}\left(t_i\right)-{\ddot{Q}}_{j,i}\left(t_{i+1}\right)\right),$$

(15)

$$\begin{array}{cc}\hfill {\dot{Q}}_{j,i}\left(t_i^{*}\right)=& -\frac{h_i}{2\left({\ddot{Q}}_{j,i}\left(t_{i+1}\right)-{\ddot{Q}}_{j,i}\left(t_i\right)\right)}{\ddot{Q}}_{j,i}\left(t_i\right){\ddot{Q}}_{j,i}\left(t_{i+1}\right)\hfill \\ \hfill & +\frac{q_{j,i+1}-q_{j,i}}{h_i}+\frac{h_i}{6}\left({\ddot{Q}}_{j,i}\left(t_i\right)-{\ddot{Q}}_{j,i}\left(t_{i+1}\right)\right),\hfill \end{array}$$

(16)

$${\dot{Q}}_{j,i}\left(t_{i+1}\right)=\frac{h_i}{2}{\ddot{Q}}_{j,i}\left(t_{i+1}\right)+\frac{q_{j,i+1}-q_{j,i}}{h_i}+\frac{h_i}{6}\left({\ddot{Q}}_{j,i}\left(t_i\right)-{\ddot{Q}}_{j,i}\left(t_{i+1}\right)\right).$$

(17)

C. Acceleration constraints: $\left|{\ddot{Q}}_{j,i}\left(t\right)\right|\le A_{j\max }, j=1,\cdots ,N; i=1,\cdots ,n-1$

The acceleration is a linear function in the time interval $[t_i,t_{i+1}]$; the maximum absolute acceleration exits at $t_i$ or $t_{i+1}$. Thus, the acceleration constraint can be written as:

$$\max \left|{\ddot{Q}}_{j,i}\left(t\right)\right|=\max \left\{\left|{\ddot{Q}}_{j,i}\left(t_i\right)\right|, \cdots , \left|{\dot{Q}}_{j,i}\left(t_{i+1}\right)\right|\right\}\le A_{j\max }.$$

(18)

D. Maximum time constraints

$$t_f={\displaystyle \sum _{i=1}^{n-1}{h}_{ i}}\le T_{\max },$$

(19)

where $T_{\max }$ is the maximum time.

Consequently, the trajectory optimization problem is formulated as:

$$\begin{array}{cc}\min \hspace{1em}\hfill & K_{T}N{\displaystyle \sum _{i=1}^{n-1}{h}_{ i}}+\alpha K_J{\displaystyle \sum _{j=1}^N{\displaystyle \sum _{i=1}^{n-1}\left[\frac{{\left({\ddot{Q}}_{j,i+1}-{\ddot{Q}}_{j,i}\right)}^2}{h_{ i}}\right]}},\hfill \\ \mathrm{s}.\mathrm{t}.\hfill & \max \left\{\left|{\dot{Q}}_{j,i}\left(t_i\right)\right|,\left|{\dot{Q}}_{j,i}\left(t_i^{*}\right)\right|,\left|{\dot{Q}}_{j,i}\left(t_{i+1}\right)\right|\right\}\le V_{j\max },\hfill \\ \hfill & \max \left\{\left|{\ddot{Q}}_{j,i}\left(t_i\right)\right|, \cdots , \left|{\dot{Q}}_{j,i}\left(t_{i+1}\right)\right|\right\}\le A_{j\max },\hfill \\ \hfill & {\displaystyle \sum _{i=1}^{n-1}{h}_i}\le T_{\max },\hfill \end{array}$$

(20)

where $j=1,\cdots ,N; i=1,\cdots ,n-1$.

4.3. Execution of the Algorithm

A. Choice of initial solution

The choice of the initial solution of the iterative solution has a significant impact on the optimization result. An inappropriate choice can affect the final optimization result. Therefore, a reasonable determination of the initial solution is the key to solving nonlinear constrained optimization problems.

For the $j$th joint, the time interval $h_i(i=1,\cdots ,n-1)$ must satisfy:

$$h_i \ge \left|\frac{q_{j,i+1}-q_{j,i}}{V_{j\max }}\right|.$$

(21)

Therefore, the time interval $h_i$ has a lower bound $\left|(q_{j,i+1}-q_{j,i})/V_{j\max }\right|$.

Since the above-mentioned lower bound value does not necessarily satisfy the kinodynamic constraints, then the need $h_i$ is expanded by a factor of $\lambda$ (i.e., $h_i^0=\lambda h_i$) times to satisfy kinodynamic constraints, correspondingly, $\tau =\lambda h$; the time interval $[t_i,t_{i+1}]$ becomes $[{\tau }_i,{\tau }_i+h_i^0]$. If ${\tau }_i$, ${\tau }_{i+1}$ and $h_i^0$ are used instead of $t_i$, $t_{i+1}$ and $h_i$, then the following equations can be obtained:

$$\begin{array}{c}{Q}_{j,i}\left(\tau \right) = Q_{j,i}\left(t\right),\end{array}$$

(22)

$$\begin{array}{c}{\dot{Q}}_{j,i}\left(\tau \right) \frac{{\dot{Q}}_{j,i}\left(t\right)}{\lambda },\end{array}$$

(23)

$$\begin{array}{c}{\ddot{Q}}_{j,i}\left(\tau \right) = \frac{{\ddot{Q}}_{j,i}\left(t\right)}{{\lambda }^2}.\end{array}$$

(24)

It can be seen from the above equations that as $h_i$ increases by a factor of $\lambda$ times, the joint position remains unchanged, while velocity and acceleration are reduced by factors of $1/\lambda$ and $1/{\lambda }^2$, respectively. Now let:

$$\begin{array}{c}{\lambda }_1 = \underset{j=1,\cdots ,N}{\max }\left\{\underset{t\in [t_i,t_{i+1}], i=1,\cdots ,n-1}{\max }\frac{\left|{\dot{Q}}_{j,i}\left(t\right)\right|}{V_{j\max }}\right\},\end{array}$$

(25)

$$\begin{array}{l}{\lambda }_2 = \underset{j=1,\cdots ,N}{\max }\left\{\underset{t\in [t_i,t_{i+1}], i=1,\cdots ,n-1}{\max }\frac{\left|{\ddot{Q}}_{j,i}\left(t\right)\right|}{A_{j\max }}\right\}.\hfill \end{array}$$

(26)

Then the feasibility adjustment factor $\lambda$ is obtained by setting:

$$\begin{array}{c}\lambda = \max \left\{\begin{array}{ccc}1 ,& {\lambda }_1,& {\lambda }_2^{1/2}\end{array}\right\}\end{array}.$$

(27)

Finally, the initial solution of the time interval vector is:

$$h^0=\lambda h,$$

(28)

where $h = {[h_1,\cdots ,h_{n-1}]}^{\mathrm{T}}$.

B. Execution of the algorithm

Once the initial solution was determined, Equation (20) was solved by running a nonlinear optimization algorithm through a solver. The procedures for solving the trajectory optimization problem are summarized below:

• Start from the given waypoints, the kinodynamic limits and maximum time of excavation;
• Find a suitable initial solution using Equations (21)–(28);
• Choose the appropriate solver and the corresponding optimization algorithm. In this research, SQP (sequential quadratic programming) [33], which tackles nonlinear programming problems by iteratively and locally approximating the original problem with a sequence of quadratic optimization subproblems, was utilized;
• Put the objective function, Equation (12), and constraints, Equations (14), (18) and (19), into the solver;
• Finally, obtain the solution to the optimization problem and trajectory sequence.

5. Results

In this section, the feasibility of our proposed method was validated by comparing autonomous excavation with manual excavation. First, in this paper we conducted several groups of teaching experiments to obtain the waypoints of manual excavation. Then an optimal trajectory was generated through the proposed method in the previous section. Finally, field tests were performed to test our method.

5.1. Implementation Details

Our trajectory generation experiments were completed on the self-developed SWE50E autonomous working excavator platform (Figure 6). Its D–H parameters are shown in

Table 1. D–H parameters of SWE50 excavator.

i	di (mm)	ai (mm)	αi (°)	θi (°)
1	1125	860	90	[−180, 180]
2	0	2845	0	[−60, 65]
3	0	1495	0	[−15, −25]
4	0	835	0	[−135, 40]

. Computing resources included a laptop computer, which had a dual-core Intel i5-3210 processor running at 2.50 GHz with 12 GB RAM and a BODAS controller. We set the sampling frequency of the controller to 10 Hz to guarantee the controller worked at a high frequency. In the autonomous test, sensors were used to measure online during excavation. The number of waypoints used for trajectory generation in the experiment was set to 169. The kinodynamic feasibility constraints of the autonomous excavator are shown in

Table 2. Kinodynamic feasibility constraints of the excavator.

Constraints	Swing	Boom	Stick	Bucket
Velocity (°/s)	70	45	50	90
Acceleration (°/s2)	160	140	135	135

. In the trajectory optimization, the maximum number of iterations was set as 50; the time weight and jerk weight were 0.95 and 0.05, which meant that we expected to complete the excavation work as fast as possible; and the elastic coefficient was 0.001. In addition, the maximum time in the excavation process was 90 s.

5.2. Field Test

In this research, the proposed method was tested on a 5-ton autonomous excavator platform. For a fair comparison with a skilled operator (Figure 6), the size of the trench was set as 2 × 0.8 × 1 m and the engine speed of excavator was set as 1800 rpm for the test. The distance between the farthest edge of the excavation area and the crawler was 3.5 m. During the dumping stage, the body rotated 90° counterclockwise. As shown in Figure 7a, for an operation cycle that included seven shovels of excavation, the operation time was as consistent as possible. First, the excavator performed 15 operation cycles (note that it required fast and smooth excavation trajectories) and recorded the excavation trajectories of a skilled operator. Then, the waypoints were obtained through the method in Section 3. Finally, the excavation trajectory was generated using the method in Section 4. It should be noted that this paper focuses on the specified dimension of the trench. Afterwards, the autonomous excavation was completed by controlling the excavator arm (i.e., boom, stick and bucket) to track the generated trajectory. To compare angle and pressure, this paper only compares the joint angle of the excavator working devices (i.e., this trajectory is YZ projection as shown in Figure 7b) and the corresponding cylinder pressure (boom, stick, bucket) (Figure 8 and Figure 9).

By comparing the autonomous excavation trajectory using the proposed method with the excavation trajectory of a skilled operator, as shown in Figure 8, we see that the method of this paper presents a shorter time while satisfying the dynamic feasibility constraints. In the experiment, the time of autonomous excavation was 80.4 s, while that of the operator was 83.2 s, as listed in

Table 3. Comparison of autonomous and manual excavation joint motion.

Operation Mode	Standard Deviation			Time
	Boom	Stick	Bucket
Manual	9.1591	9.1110	30.0052	83.2
Autonomous	8.8264	10.3288	33.9250	80.4

. By using this method proposed, the speed and acceleration during the excavation motion are closer to the dynamic feasibility limit; hence, autonomous excavation tends to move faster. However, it can be seen that the angle fluctuation of each joint in autonomous excavation (comparison of standard deviation in Table 3) is larger when compared with that of manual excavation.

On the other hand, as shown in Figure 9, the maximum absolute pressure of the working device was larger than that of manual excavation, due to the aggressiveness of the excavator being fully used. However, this led to a larger standard deviation of the pressure of the working device for autonomous excavation motion (as shown in

Table 4. Comparison of manual and autonomous excavation cylinder pressure.

Operation Mode	Standard Deviation			Maximum Absolute Pressure
	Boom	Stick	Bucket	Boom	Stick	Bucket
Manual	4.6216	5.5551	5.0018	17	26.4	17.4
Autonomous	7.2873	8.1524	6.6024	18.6	27.2	18.8

); this is because a large jerk of the cylinder results in unstable motion during the operation, and the stability of the movement is therefore worse than manual operation.

For a fair comparison, we compared the proposed method against the previous excavation trajectory generation research based on optimization and bucket capacity [8,19]. In the case of an average excavation cycle, the time used in this paper was 11.5 s with a bucket capacity of 0.18 m³, while the previous research [19] presented 14.59 s with a bucket capacity of 0.02 m³. Additionally, the difference in excavation efficiency is that this paper considered the dynamic feasibility limit of the actuators, thereby making full use of the physical properties of the excavator itself, while the previous research only considered a single time constraint, this leading to the difference in excavation time between the two. In turn, smoother excavation trajectories also promoted better time optimization.

6. Conclusions

In this paper, a novel trajectory generation method is proposed for autonomous excavator teach-and-plan. Detailed conclusions are the following:

• In this paper, the waypoints used for optimal trajectory generation were obtained by combining a manual excavation model with trajectory topology information using the excavation trajectories of skilled operators;
• These waypoints were iteratively optimized in a nonlinear optimization framework to generate fast, smooth and efficient excavation motions while satisfying kinodynamic feasibility constraints;
• A comparison experiment between autonomous excavation and manual excavation highlights the superiority of this method in aggressiveness.

In this work, we assumed that the excavation environment was relatively static (e.g., no earthwork collapses) and that the autonomous excavator was instructed to track the reference trajectory once it was generated. In future research, we will focus on the following aspects:

• To add reactive local replanning to the current framework to handle a dynamic excavation environment. In this way, the generated repetitive trajectory will be used as a reference trajectory for the autonomous excavator. Then, the autonomous excavator will track the reference trajectory and avoid the problem of changes in trench size through local trajectory planning;
• Combine the excavation volume constraint, and transform it into a constraint of the trajectory optimization problem. Then, a full excavation bucket will be achieved.