{'fraction_non_alphanumeric': 0.02213965010063477, 'fraction_numerical': 0.0049156216132528255, 'mean_word_length': 4.975254394079556, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 7, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Time Variable Minimum Torque Trajectory Optimization for Autonomous Excavator

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.


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 [18] 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.

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 [19]. Jud et al. [4] 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 [18] 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 [8], 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 [6,9,11]. While some of these approaches can achieve asymptotic optimality in theory, they tend to have slow convergence to high quality trajectories in practice.

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 [4,8,18], 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 150 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 4 -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.

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.

Trajectory Optimization Statement
The classical optimal control problem is to minimize some cost functionals over trajectory variables which could be formulated as: where q andq 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 L and a penalty term Φ to force the final condition at t f to be satisfied. In general, the trajectory is constrained by the boundary conditions as described in Equation 2. The final time t f could be either fixed or flexible [10]. Torque and trajectory variables must obey the equation of motion: where M denotes the mass inertia of the manipulator, C is the Coriolis matrix and V contains gravitational forces. R 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 q = {q 0 , q 1 , . . . , q N } with N +1 timesteps. As a result, the minimal torque trajectory could be approximately expressed as where g and h 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 ∆t between two adjacent discrete points, the joint velocity and acceleration are obtained witḣ ∆t should be very small so thatq andq are calculated accurately enough to obtain the correct torque value. In other words, the number of variable waypoints N +1 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 ∆t between keypoints. The problem is then reformulated as where k = {k 0 , k 1 , . . . , k M } denotes selected keypoints with M size and Q is the number of interpolated waypoints.
To enable flexible final time t f as in the aforementioned optimal control problem, we add the time intervals between keypoints T = {T 0 , T 1 , . . . , T M −1 } as variables in the optimization problem. Subsequently, Q i = Ti ∆t is no longer fixed. Finally, we adopt the problem formulation:

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 4 revolute joints according to the excavator's geometric dimensions [4,8]. Thus, for the excavation problem, the commonly generalized joint position variable q = [θ 0 , θ 1 , θ 2 , θ 3 ] T is utilized. During an excavation, the resistance forces that surrounding soils exerting on the bucket surface could be integrated into one resultant force R applied at B − xyz. For each waypoint on the trajectory, we calculate the necessary torque to achieve the target acceleration while overcoming R with the parallel recursive inverse dynamics algorithm [14,20].

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.  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. n is the normal vector of the surface at the keypoint a. The heading direction is restricted between a range around n. 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 [Z 1 , Z 2 ] to guarantee that they are beneath the soil surface.
Swept Volume Constraint Although the bucket filling is a complex dynamics process depending on both soil conditions and the bucket trajectory [2], the excavated soils could be estimated with the swept volume -the soil volume above the bucket path [4,8]. As shown in Figure 3, the swept volume could be approximated with four points: the entry point of soil k 0 (a), end point of the penetration phase k 1 (b); the end point of the dragging phase k 2 (c), and the exit point of the soil k 4 (d). we enforce the swept volume to be within a range determining by the bucket's real capacity: where W is the bucket width perpendicular to the excavation plane and S abcd is the area of the quadrilateral abcd.
Direction Constraints Whether a bucket trajectory is successful for an excavation task highly depends on two directions. As Figure 4a depicts, one is the bucket heading direction d h which refers to the direction along the bucket tooth; the other is the bucket tip translation vector d t referring to the bucket position movement between two adjacent time steps. In general, as illustrated  illustrates direction constraints represented using hyperplanes. For example, the direction of v is constrained within the intersection of half-planes defined by vector n 1 and n 2 , i.e. v · n 1 < 0 and v · n 2 < 0.
in Figure 4b, we use hyperplanes to restrict directions. The vector v locating in the half-plane which is in the opposite direction of the normal vector n could be expressed as v · n < 0.
The heading directions at some time steps are restricted within some ranges. For example, d h at the approach point is supposed to align with the normal vector of the terrain surface; d h during bucket lifting should keep approximately upright. With a certain heading direction d B h with respect to the frame B, one could obtain its coordinate in the inertial frame O with rotation R: d h = R(q)d B h . 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. The constraints of monotonic change of directions are formulated as where R 90 is rotation around the axis perpendicular to the excavation plane.
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: Step 1 Step 2 Step 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 [3,12]. Analytical models such as the fundamental earthmoving equation [15] 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 [1,7,16]. 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 f (q,q,q) → R. As shown in Fig. 6, our model simplifies the bucket with two line segments, named the back plane and the separation plane [13]. 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, g denotes the gravitational constant, and h 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 [5]. 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 N 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 (R i = N i + F i ) for discretized small areas of the submerged bucket part and integrate them into the resultant force and moment R = (F, M) at the bucket tip frame B − xyz. We model both N i and F i as functions of the velocity of the ith small area. As the Figure 6 illustrates, velocity of each small area v = v n + v t , where v n is normal to the bucket face while v t is along with this face. N and F are opposite to v n and v t respectively. The force magnitude is controlled by several coefficients that implicitly stand for the soil conditions. In particular, N for each area is given by: where S is the area of the element segment. K p scales the height related pressure while K v adjusts the influence of velocity magnitude. F s is calculated with where K f is the friction coefficient depending on the soil cohesion and density [12]. We obtain the velocity of each point fixed to the bucket with its body velocity V B = (ω B , v B ) which is calculated with the differential forward kinematics where ω B is the angular velocity of the frame B, v B is its linear velocity, J is the kinematics Jacobian matrix, r i is the point position relative to the frame B.
Finally, the resultant force F and moment M is computed by

Experiments
In this section, we present the implementation and experiment results of our optimization-based trajectory generation. Our method uses the open source Tra-jOpt SQP solver [17] to solve the optimization problem. We efficiently calculate torque with the recursive inverse dynamics (ID) algorithm based on Lie group [20]. We use a kinematic-based method for quick searching of initial trajectory for the optimization using a reachability map based representation [21]. 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.  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 0.8 m 3 ≤ V ≤ 1 m 3 . 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 1 m 3 . The swept volume of the initial trajectory, as shown in Figs. 7, is apparently less than 0.8 m 3 . Thus, the value of volume constraint is large when the optimization starts. Time intervals between all keypoints are 1 s 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.  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 penetrationdragging-rotation phases.
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 5.5 s while the time fixed trajectory spends 6.4 s. 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.
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 [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.

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 op-   timizes 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.