Log In Sign Up

Time Variable Minimum Torque Trajectory Optimization for Autonomous Excavator

by   Yajue Yang, et al.

In this paper, we present a minimal torque and time variable trajectory optimization method for autonomous excavator considering the soil-tool interaction. The method formulates the excavation motion generation as a trajectory optimization problem and takes into account geometric, kinematic and dynamics constraints. To generate time-efficient trajectory and improve the overall optimization efficiency, we propose a time variable trajectory optimization mechanism so that the time intervals between the keypoints along the trajectory subject to the optimization. As a result, the method uses few keypoints and reduces the total number of optimization variables. We further introduce a soil-tool interaction force model, which considers the geometric shape of the bucket and the physical properties of the soil. The experimental result on a high fidelity dynamic simulator shows our method can generate feasible trajectories, which satisfy excavation task constraints and are adaptive to different soil conditions.


Optimization-Based Framework for Excavation Trajectory Generation

In this paper, we present a novel optimization-based framework for auton...

Comparing Metrics for Robustness Against External Perturbations in Dynamic Trajectory Optimization

Dynamic trajectory optimization is a popular approach for generating opt...

Efficient Trajectory Planning and Control for USV with Vessel Dynamics and Differential Flatness

Unmanned surface vessels (USVs) are widely used in ocean exploration and...

Planning Jerk-Optimized Trajectory with Discrete-Time Constraints for Redundant Robots

We present a method for effectively planning the motion trajectory of ro...

Featured Trajectory Generation for TrackPuzzle

Indoor route graph learning is critically important for autonomous indoo...

Underactuated Waypoint Trajectory Optimization for Light Painting Photography

Despite their abundance in robotics and nature, underactuated systems re...

Trajectory Optimization with Optimization-Based Dynamics

We present a framework for bi-level trajectory optimization in which a s...

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 time-efficient. 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 end-effector 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 end-effector 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 soil-tool interaction.

Figure 1: Two different bucket trajectories for excavation. (a) The bucket approaches the terrain surface. (b) The bucket penetrates into the soil. (c) The bucket drags horizontally to make the soil fail. (d) The bucket rotates and lift soils out from the terrain.

1.1 Related Work

A motion planner for excavation needs to simultaneously consider the end-effector 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 optimization-based 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], time-efficient and minimum torque motions are generated with the classical parametric optimization method based on B-splines. 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 sampling-based 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 soil-tool interaction force model, which considers the geometric shape of the bucket and the most fundamental properties of the soil. Finally, we use a kinematic-based 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 optimization-based trajectory generation method has the following advantages:

  1. 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.

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

  3. 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 111 - 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 soil-tool 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 bucket-soil 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 optimization-based 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:


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:


where denotes the mass inertia of the manipulator, is the Coriolis matrix and contains gravitational forces. is the external force usually applied at the end-effector 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


where and represent a tuple of inequality and equality constraints for a specified task. Since many tasks in reality are actually not a point-to-point 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


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


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:


2.2 Excavator Model

As Figure 2 describes, an excavator is a hybrid serial-parallel mechanism. It is composed of serially connected links: cabin, boom, stick and bucket. Hydraulic cylinders are assembled on these links which result in several closed-loop 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.

Figure 2: Excavator Model

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:

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

  2. 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.

Figure 3: Excavation constraints. Large orange circles denote bucket tip keypoints. Between each pair of adjacent keypoints, waypoints, denoted by tiny green circles, are linearly interpolated. Translation and heading directions are drawn with blue and red directions. The brown curve represents the terrain surface.

is the normal vector of the surface at the keypoint a. The heading direction is restricted between a range around

. The blue shadow area denotes the side view of the excavated swept volume. Depths of keypoints b and c are forced to be within the range to guarantee that they are beneath the soil surface.

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:


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.

(a) Directions
(b) Direction Constraints
Figure 4: (a) illustrates the heading direction and bucket tip translation vector

, 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 half-planes 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 half-plane 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


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.

Figure 5: Monotonic change of the directions. The bucket moves from step to step . Red arrows denote heading directions and blue arrows are translation directions. Double arrows represent the normal vectors of hyperplanes for corresponding directions. At step , pose c is not valid since its heading direction locates at the forbidden halfplane. At step , pose e violates the constraint of monotonic change of translation directions. As a result, the path is a feasible path.

The constraints of monotonic change of directions are formulated as


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:


3 Bucket-Soil 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 visco-plastic 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 soil-tool force model based on the fluid dynamics, which maps from a joint state and action into the resistance force exerted by the surrounding soils .

Figure 6: Bucket simplification and soil-tool interaction force model. The brown curve and shadow area beneath it represent the terrain soil. Any planar rigid body motion could be regarded as a rotation around some point . Based on the rotation motion, velocity of each point is calculated and denoted with a black arrow , which is decomposed into and shown in the enlarged right part of figure. and

denote the resultant force and moment at the bucket tip frame.

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:


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:


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:


where is the area of the element segment. scales the height related pressure while adjusts the influence of velocity magnitude. is calculated with


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


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


4 Experiments

In this section, we present the implementation and experiment results of our optimization-based 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 kinematic-based 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
Table 1: Excavator Specifications

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 .

(a) Bucket Trajectories
(b) Torque Cost and Constraint Values
Figure 7: Experiment i@. The sub-figure (a) shows the side view of the bucket trajectory which is parallel to the excavation plane. Each dot point denotes a keypoint at which the heading direction is represented by the red arrow. The sub-figure (b) draws values of torque cost and some constraints during optimization. LHDirCstr is the heading direction constraint of lifting phase; SweptVolumeCstr is the constraint of swept volume for the whole trajectory; TDirMonoCstrPDR is the monotonic change constraint of the translation direction for penetration-dragging-rotation phases.

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.

(a) Bucket Trajectories
(b) Torque Cost
Figure 8: Comparison of optimization results between fixed and variable time experiments. In sub-figure (a), time intervals between keypoints are annotated for the time variable trajectory.

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 end-effecor 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].

Soft Soil Hard Soil
Table 2: Soil Properties. We simultaneously change density and coefficients which are influenced by other attributes to simulate different soil conditions.
Figure 9: Bucket trajectory comparison

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 soil-tool model between our method and the simulator, the torques along the entire excavation cycle are consistent between each other as shown in Fig. 10.

(a) Torque
(b) Joint Angle
Figure 10: Comparison of optimization results on dynamic simulator. Blue curves are for the initial trajectory; Orange curves are for the optimal trajectory; Green curves are for the optimal simulation trajectory.
Figure 11: Sequence of excavation motion on dynamic simulator.

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 B-spline 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 soil-tool 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 soil-force model with the real-world excavation data on different soil environment. Finally, we are interested in testing our method on real excavators.