1 Introduction
Heavy machinery such as excavators are widely used in construction, mining and many other scenarios. There is a strong need for automating the excavation process in order to save the labor cost and improve the overall working condition, especially for hazardous environment. The fundamental function of an excavator is to remove or reshape materials by excavation operations. During a digging cycle, interaction forces between materials and the bucket might require large torque of the excavator joints to complete an excavation task. Thus, for preventing mechanical damage and saving fuel consumption, it is necessary to generate a trajectory with small torque demands. In addition, high productivity requires the trajectory to be timeefficient. Even though excavation motions appear highly repetitive, automating the excavator to meet these performance criteria still remains as a challenging problem. This motivates us to develop a motion planner that generates optimal excavation trajectories simultaneously considering dynamics and time factors.
To fully fill the bucket with soils, one of the commonest excavation tasks, a trajectory of the bucket is softly constrained to follow a similar pattern of motions. Singh [sing1995synthesis] concluded that the pattern is comprised of four phases: penetration, dragging, rotation and lifting. However, unlike other autonomous manipulation tasks such as welding and grinding in which endeffector paths are restricted to exactly desired ones, there exist many kinematically feasible bucket trajectory candidates satisfying the filling task for a given terrain, as shown in Figure 1. To select the dynamically optimal trajectory from as many trajectory candidates as possible, we need a proper and flexible way to formulate the excavation task instead of just providing an endeffector trajectory to track. Furthermore, since interaction forces are influenced by soil properties such as density and cohesion, trajectories should be adaptive to soil conditions for dynamics feasibility. In this paper, we focus on planning dynamically optimal trajectory for excavator considering the soiltool interaction.
1.1 Related Work
A motion planner for excavation needs to simultaneously consider the endeffector kinematics constraints and dynamics performances such as torque demands. There have been diverse research areas for control and planning of a single excavation operation [singh1992task]. Jud et al. [jud2017planning] proposed an optimizationbased force control method, which is adaptive to different soil conditions and protects the excavator from exceeding force/torque limits. The method optimizes for the next control command, but does not optimize over the entire excavation cycle to generate an optimal trajectory. Singh [sing1995synthesis] formulates the excavation motion planning problem as an optimization over a simplified variables i.e., the bucket’s approaching angle, height and digging distance, by determining other variables aforementioned with some deliberate metrics. However, since the formulation is overly simplified, it loses a lot of space for optimization. In [kim2013dynamically], timeefficient and minimum torque motions are generated with the classical parametric optimization method based on Bsplines. Since they regard the digging phase as a point to point process without considering kinematics constraints, the computed trajectory might violate some excavation task specific constraints. In the motion planning literature, tree samplingbased planners with forward proportion have been successfully developed for planning dynamically feasible motion [karaman11, lavalle01, bekris16]. While some of these approaches can achieve asymptotic optimality in theory, they tend to have slow convergence to high quality trajectories in practice.
1.2 Main Contribution
This paper proposes a minimal torque and time variable trajectory optimization method for a single excavation task. To reduce the number of optimization variables and improve the overall efficiency, we propose to only use few keypoints as variables and compute torque of linearly interpolated waypoints with small fixed time step between them. To enable autonomous time allocation, we also put the time intervals between keypoints under optimization. In this way, it is possible that the optimized trajectory not only costs less torque but also less time compared to the initial trajectory.
Our formulation considers geometric, kinematic and dynamics constraints for generating feasible excavation motion. We define swept volume geometric constraint to estimate the amount of soil to be excavated. The heading and movement directions of the bucket are further constrained over the entire trajectory. The common constraints of a robotic maniputlator,
e.g. the joint angles, velocity and torque limits are also imposed. To efficiently calculate torque, we use the recursive inverse dynamics (ID) algorithm based on Lie group. To effectively take into account the interaction between the soil and the excavator bucket, we introduce a simple soiltool interaction force model, which considers the geometric shape of the bucket and the most fundamental properties of the soil. Finally, we use a kinematicbased method for quick searching of initial trajectory for the optimization using a reachability map based representation.As compared to previous work excavation motion planning and control [jud2017planning, kim2013dynamically, sing1995synthesis], our optimizationbased trajectory generation method has the following advantages:

Our formulation explicitly takes into account complete excavation specific constraints in a straightforward way while most previous methods only consider a subset of these constraints. Using our method, physically feasible excavation trajectories are generated.

Unlike the parametricbased trajectory representation, which tends to be overly constrained for the underlying solution space, the keypointbased optimization formulation is less conservative and can yield more optimal solution by exploring large state space.

By using time variable keypoints, we can reduce the total number of optimization variables and thus improve the overall performance, which is achieving minimum torque while spending less time. The experiment shows that the optimization problem can quickly converge with few iterations and dynamically feasible trajectory can be computed within seconds on average.
We highlight the experimental result by comparing the trajectories generated under different swept volume, torque limit and other constraints, and demonstrate our method can generate dynamically feasible trajectories. We further validate the generated trajectories using AGX ^{1}^{1}1https://www.algoryx.se/products/agxdynamics/  a high fidelity dynamic simulator with realistic terrain modeling. The experimental results further show that the trajectories can be successfully tracked with simple trajectory controller. Despite the soiltool modeling difference between our method and the simulator, the torques along the entire excavation cycle are consistent between each other.
The rest of the paper is organized as follows. Section 2 presents the excavation trajectory optimization problem and constraint formulation. Section 3 presents the bucketsoil interaction model. Section 4 presents experiments of our optimization approach. Conclusions and future work are in Section 5.
2 Excavation Trajectory Optimization
In this section, we present our time variable optimizationbased excavation trajectory generation method. We will first introduce the classical optimal control problem from which the dynamical trajectory optimization formulation is derived. We then illustrate excavation task specific constraints which guarantee the trajectory is successful to excavate soils while leaving a plentiful room for optimization.
2.1 Trajectory Optimization Statement
The classical optimal control problem is to minimize some cost functionals over trajectory variables which could be formulated as:
(1)  
(2) 
where and represent trajectory position and velocity in the joint space while is the necessary torque to execute the trajectory. The objective combines a main cost function and a penalty term to force the final condition at to be satisfied. In general, the trajectory is constrained by the boundary conditions as described in Equation 2. The final time could be either fixed or flexible [lee2005newton].
Torque and trajectory variables must obey the equation of motion:
(3) 
where denotes the mass inertia of the manipulator, is the Coriolis matrix and contains gravitational forces. is the external force usually applied at the endeffector of the manipulator.
For numerical computation, the trajectory is discretized into a sequence of joint positions with timesteps. As a result, the minimal torque trajectory could be approximately expressed as
(4) 
where and represent a tuple of inequality and equality constraints for a specified task. Since many tasks in reality are actually not a pointtopoint problem, we relax the boundary constraints commonly required in the optimal control formulation. With a given small time step between two adjacent discrete points, the joint velocity and acceleration are obtained with
(5) 
should be very small so that and are calculated accurately enough to obtain the correct torque value. In other words, the number of variable waypoints should be large, which in turn increases the complexity of the optimization problem.
To deal with this issue, we propose a concept of keypoints of the trajectory, based on the observation that, for tasks like excavation (as will be shown in the following section), constraints on the entire trajectory could be reduced to constraints on some keypoints. We only use keypoints as variables of the optimization problem and compute torque of linearly interpolated waypoints with fixed time step between keypoints. The problem is then reformulated as
(6) 
where denotes selected keypoints with size and is the number of interpolated waypoints.
To enable flexible final time as in the aforementioned optimal control problem, we add the time intervals between keypoints as variables in the optimization problem. Subsequently, is no longer fixed. Finally, we adopt the problem formulation:
(7) 
2.2 Excavator Model
As Figure 2 describes, an excavator is a hybrid serialparallel mechanism. It is composed of serially connected links: cabin, boom, stick and bucket. Hydraulic cylinders are assembled on these links which result in several closedloop structures. The prismatic hydraulic cylinder forces could be transformed into torque of revolute joints according to the excavator’s geometric dimensions [jud2017planning, kim2013dynamically]. Thus, for the excavation problem, the commonly generalized joint position variable is utilized.
The body fixed coordinate frame located at the tip of bucket represents bucket pose relative to the track base fixed coordinate frame , where denotes orientation and is the position of the tip. Given a joint position, the bucket pose is calculated with the forward kinematics function: .
During an excavation, the resistance forces that surrounding soils exerting on the bucket surface could be integrated into one resultant force applied at . For each waypoint on the trajectory, we calculate the necessary torque to achieve the target acceleration while overcoming with the parallel recursive inverse dynamics algorithm [park1995lie, yang2017parallel].
2.3 Excavation Constraints
In this section, we will illustrate excavation constraints and how to select keypoints to reduce the computation complexity. Constraints play two roles in an optimization problem for a given task:

They specify the requirement of the task, while the optimization objective function represents the criteria of how well the task is performed.

They describe other limitations and rules for the robot to obey.
For the excavation task, an excavator needs to fill the bucket with a desired mass of soils, while not violating other constraints such as torque limits the machine could exert and the range of bucket path due to target terrain shape. Fig. 3 illustrates keypoints selection and some important constraints as an example for the excavation task. The bucekt penetrates into the soil from a to b; drags from b to c; rotates from c to d. Compared to other movements, more constraints are imposed to the rotation. For example, the heading directions and translation directions should coordinate well to swept soils in the bucket. Thus, more keypoints are used for the rotation phase. Please refer to notes of the figure for detailed explanation.
2.3.1 Swept Volume Constraint
Although the bucket filling is a complex dynamics process depending on both soil conditions and the bucket trajectory [coetzee2007discrete], the excavated soils could be estimated with the swept volume — the soil volume above the bucket path [jud2017planning, kim2013dynamically]. As shown in Figure 3, the swept volume could be approximated with four points: the entry point of soil (a), end point of the penetration phase (b); the end point of the dragging phase (c), and the exit point of the soil (d). we enforce the swept volume to be within a range determining by the bucket’s real capacity:
(8) 
where is the bucket width perpendicular to the excavation plane and is the area of the quadrilateral .
2.3.2 Direction Constraints
Whether a bucket trajectory is successful for an excavation task highly depends on two directions. As Figure 3(a) depicts, one is the bucket heading direction which refers to the direction along the bucket tooth; the other is the bucket tip translation vector referring to the bucket position movement between two adjacent time steps.
, which are two important factors for the success of an excavation task. (b) illustrates direction constraints represented using hyperplanes. For example, the direction of
is constrained within the intersection of halfplanes defined by vector and , i.e. and .In general, as illustrated in Figure 3(b), we use hyperplanes to restrict directions. The vector locating in the halfplane which is in the opposite direction of the normal vector could be expressed as .
The heading directions at some time steps are restricted within some ranges. For example, at the approach point is supposed to align with the normal vector of the terrain surface; during bucket lifting should keep approximately upright. With a certain heading direction with respect to the frame , one could obtain its coordinate in the inertial frame with rotation : . Thus the constraints on heading direction and translation direction could be formulated as
(9) 
For a successful excavation process, both heading directions and translation directions should not swing under the soil surface. Otherwise, the bucket will push the soil. In other words, these directions along the entire trajectory should monotonically change. We restrict directions by the previous one using the aforementioned hyperplane method, as illustrated in Figure 5.
The constraints of monotonic change of directions are formulated as
(10)  
(11) 
where is rotation around the axis perpendicular to the excavation plane.
2.3.3 Velocity and Torque Constraints
In addition to the task specific constraints presented above, there are some constraints induced by the mechanical properties. We list two main constraints common in excavators: for all waypoints, motion should satisfy joint velocity constraints due to oil flow limits and joint torque constraints:
(12) 
3 BucketSoil Interaction Force
This section considers the soil and the bucket tool interaction, i.e. the relationship between soil resistance force and the tool action. The interaction between the soil and tool is rather complicated as it is affected by the soil material property, the terrain deformation and other unknown parameters. Researches have put a lot of effort into developing mechanics quantitatively describing the reaction of soil to tools [gill1967soil, mckyes1985soil]. Analytical models such as the fundamental earthmoving equation [reece1964paper] has been developed with soil failure mechanics specifically for tillage or traction movements. However, they are not suitable for other types of movements such as penetration and pushing that might happen during the optimization process. From the viscoplastic nature and mass deformation of the soil, another genre of researchers utilize the fluid dynamics to model the tool soil interaction process [andreotti2013granular, karmakar2006dynamic, sauret2014bulldozing]. Inspired by them, we propose a simplified soiltool force model based on the fluid dynamics, which maps from a joint state and action into the resistance force exerted by the surrounding soils .
As shown in Fig. 6, our model simplifies the bucket with two line segments, named the back plane and the separation plane [park2002development]. We assume that the soil pressure exerting on the parts of a static bucket surrounded by soils could be calculated with hydrostatic pressure formula:
(13) 
where represents the mass density of the soil which is one of the most fundamental soil properties, denotes the gravitational constant, and represents the depth of the part from the soil surface. This formula indicates that resistance force from the soil will grow as the bucket penetrates more deeply, which is verified by experiments in [kang2018archimedes]. To make any movement, the bucket needs to overcome friction forces whose directions are opposite to the its velocity. As in general cases, the friction magnitude is given by:
(14) 
where denotes the friction coefficient and is the normal force applied at the bucket face. In this paper, a resistance force at a small area is comprised with the normal force and friction force.
We first compute resistance forces () for discretized small areas of the submerged bucket part and integrate them into the resultant force and moment ) at the bucket tip frame . We model both and as functions of the velocity of the th small area. As the Figure 6 illustrates, velocity of each small area , where is normal to the bucket face while is along with this face. and are opposite to and respectively. The force magnitude is controlled by several coefficients that implicitly stand for the soil conditions. In particular, for each area is given by:
(15) 
where is the area of the element segment. scales the height related pressure while adjusts the influence of velocity magnitude. is calculated with
(16) 
where is the friction coefficient depending on the soil cohesion and density [mckyes1985soil].
We obtain the velocity of each point fixed to the bucket with its body velocity which is calculated with the differential forward kinematics
(17) 
where is the angular velocity of the frame , is its linear velocity, is the kinematics Jacobian matrix, is the point position relative to the frame . Finally, the resultant force and moment is computed by
(18) 
4 Experiments
In this section, we present the implementation and experiment results of our optimizationbased trajectory generation. Our method uses the open source TrajOpt SQP solver
[schulman2014motion] to solve the optimization problem. We efficiently calculate torque with the recursive inverse dynamics (ID) algorithm based on Lie group [yang2017parallel]. We use a kinematicbased method for quick searching of initial trajectory for the optimization using a reachability map based representation [yang2019compact]. In our experiments, we show that our approach can generate trajectories satisfying various constraints such as swept volume constraint, and adaptive to different soil property. We further demosntrate that, in some cases, with variable time intervals between keypoints, our optimization method converges faster and also reduces the cost. In order to further validate our approach, we use a commercial dynamic simulator AGX to simulate and validate the generated trajectories. Important parameters and mechanical constraints of the excavator platform are listed in Table 1.Mass ()  Torque Limit ()  Velocity Limit ()  Bucket Size ()  

Boom  Width  
Stick  Length  
Bucket  Depth 
4.0.1 Constraint Satisfaction
This experiment demonstrates the basic function of our algorithm: given an initial trajectory that might violate some constraints, the motion planner could generate a feasible trajectory that meets the requirement of swept volume while minimizing the torque cost. In Experiment i@, the excavator is required to fill the bucket with soils of volume . Note that since the swept volume is an approximate value and it tends to be smaller than the actual excavated volume, we set this constraint less than .
The swept volume of the initial trajectory, as shown in Figs. 7, is apparently less than . Thus, the value of volume constraint is large when the optimization starts. Time intervals between all keypoints are and we fix them during optimization in this experiment. It is expectable that the torque cost increases since the excavator is required to dig more soils. After all constraints are satisfied, the cost continues to decrease until convergence.
4.0.2 Variable Time
We further conduct an experiment with variable time intervals between keypoints and find that time scaling could help the optimization converge faster and also reduce more cost. As annotated in Fig. 8, the time variable trajectory totally spends while the time fixed trajectory spends . To find a minimal sum of torque over a trajectory is to find a balance between short time and small acceleration. Short time means few number of torques to be added, while small acceleration requires less torque for a single time step according to the equation of motion. This balance basically depends on several factors, such as the mass properties of the excavator and the resistance force exerted by soils. Therefore, allowing time intervals variable actually provides extra dimension to diminishing sum of torque.
4.0.3 Different Soil
To demonstrate this method could adapt to different soil conditions, we compare trajectories for two types of soils, whose properties are given in Table 2 with the same initial trajectory. Since this experiment aims to emphasize the capability of adpation to different endeffecor force, we use an initial trajectory without violating constraints. That means, the trajectory adaptation is purely determined by the process of minimizing torque costs, which shows that the inverse dynamics objective function is well conditioned for optimization. As shown in Fig. 9, the trajectory for the hard soil is shallower than the one for the soft soil. This result matches the trajectory executed by the human operators [jud2017planning].
4.0.4 Validation on Simulator
We validate the generated trajectories using AGX  a high fidelity dynamic simulator with realistic terrain modeling. As shown in Fig. 11, the trajectories can be successfully tracked with simple trajectory controller. Though there is difference of soiltool model between our method and the simulator, the torques along the entire excavation cycle are consistent between each other as shown in Fig. 10.
5 Conclusion
This paper presents a minimal torque and time variable trajectory optimization method for a single excavation task. Our formulation considers geometric, kinematic and dynamics constraints for generating feasible excavation motion. Different from Bspline based parameteric optimization, our method directly optimizes the keypoints along trajectory and we propose a time variable mechanism where the time intervals between the keypoints are also under the optimization. We further introduce a soiltool interaction force model to effectively take into account the interaction between the soil and the excavator bucket. We highlight the experimental result by comparing the trajectories generated under different swept volume and soil properties, and further validate the generated trajectories using a dynamic simulator.
There are many avenues for future work. We are interested in integrating our excavator trajectory planner with task planner for high level excavation tasks. We are also interested in applying system identification to correlate and improve our soilforce model with the realworld excavation data on different soil environment. Finally, we are interested in testing our method on real excavators.