- Email: [email protected]
Dynamically optimal trajectories for earthmoving excavators
Automation in Construction 35 (2013) 568–578
Contents lists available at ScienceDirect
Automation in Construction journal homepage: www.elsevier.com/locate/autcon
Dynamically optimal trajectories for earthmoving excavators Young Bum Kim, Junhyoung Ha ⁎, Hyuk Kang, Pan Young Kim, Jinsoo Park, F.C. Park a r t i c l e
i n f o
Article history: Accepted 10 December 2012 Available online 7 March 2013 Keywords: Excavator Earthmoving Motion optimization Minimum torque
a b s t r a c t We consider the problem of generating time-efficient, minimum torque motions for earthmoving excavators. Limits on actuator forces, as well as on joint velocities and accelerations, are assumed given, and the objective is to produce the fastest possible minimum torque motions while avoiding torque saturation limits. Relying on a set of recursive geometric algorithms for calculating the excavator dynamics that also calculates analytic gradients and Hessians of the dynamics as a by-product, we develop robust, reliable algorithms that generate such optimal trajectories. The resulting trajectories are compared with actual digging motions of experienced operators. © 2013 Elsevier B.V. All rights reserved.
1. Introduction Interest in unmanned excavators is driven by many considerations, from a desire to reduce the numerous fatal accidents that occur regularly to human operators—particularly in dangerous environments involving highly uneven terrain and unstable ground—to improving task efficiency by automating excavation in an optimal manner. To realize such potential benefits, an unmanned excavator must have the capability to produce optimal motions in a reliable and efficient manner. While there are many possible notions of optimality, our focus in this paper is on excavator motions that are fast, power-efficient, and smooth. These criteria naturally require careful consideration of the excavator dynamics. In some relevant previous work, [1] reports on a fully automated excavator that senses the dig face, recognizes and localizes the truck, detects any obstacles, and finally produces a feasible digging path via three planning algorithms: a coarse-to-fine dig point planning, templatebased dump planning, and script-based planning. It is important to note that none of these planning algorithms take into account the dynamics of the excavator; only the geometry of the terrain and kinematic constraints are considered. More recently in [2], a high-level hierarchical framework for autonomous construction machinery is proposed, in which the planning, measurement, and control functions are clearly identified and defined. While the general issues involved in planning are addressed, no specific planners are proposed. In this paper we report on a class of algorithms for generating dynamically optimal excavator trajectories, and more broadly, on an offline simulation and motion planning system for unmanned excavators. The benefits of a computer simulation and planning tool for such purposes are obvious: a means of evaluating and optimizing system performance, from design to motion planning and control, as well
⁎ Corresponding author. E-mail address: [email protected] (J. Ha). 0926-5805/$ – see front matter © 2013 Elsevier B.V. All rights reserved. http://dx.doi.org/10.1016/j.autcon.2013.01.007
as reduced development time and costs with fewer iterations of prototype construction and testing. Our main technical contribution is a class of motion optimization algorithms for generating the fastest possible minimum torque motions. As has been reported in some of our earlier work, [3], that motion optimization taking into account the dynamics is, although seemingly straightforward in principle, computationally and numerically highly challenging in practice. For one thing, the availability of analytic gradients of the objective function and constraints impacts numerical convergence in a significant way; finite difference approximations of the gradient often result in numerical instabilities and poor convergence behavior. Moreover, the presence of closed loop constraints—the kinematics of a typical excavator involves several closed loops and passive joints that are not actuated—complicates the calculation of the dynamics and the gradients. Drawing upon some of our previous work [3–5], we develop an optimization algorithm that robustly and efficiently obtains the fastest possible minimum torque motion—that is, without violating any actuator saturation limits, or other constraints on cylinder velocities or accelerations; we call such motions minimum torque/time motions—taking into account the closed-loop structure of excavators, and the soil dynamics during digging and dumping, i.e., in a way that reflects the influence of soil–tool interaction forces during digging, and the effects of soil loads during lifting and dumping. Such motions avoid the undesirable bang-bang characteristics of typical minimum-time trajectories, while being fast, smooth, and energy-efficient. The key to our algorithm lies in the ability to efficiently calculate analytic gradients (and if necessary analytic Hessians) of the objective function, all as a simple by-product of the recursive dynamics calculation—this is achieved by drawing upon the geometric algorithms first derived in [5]. More broadly, our motion optimization algorithm is part of an offline planning and simulation system that integrates disparate components, ranging from a dynamics simulation engine to methods for soil model parameter identification, into a coherent and easy-to-use framework. We describe the basic elements of our overall motion
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
Measurement data
569
Soil Parameter Estimation
Dynamics Digging
Lifting Comparison results between simulation and measurement
Returning
Boundary conditions
Dumping
Optimal Trajectories
Planning and Optimization
Fig. 1. Architecture for our simulation and motion planning system.
planning and simulation architecture. In particular, using soil models based on the fundamental earth moving equations (FEE) [6] and its variants [7], we describe a soil model parameter identification procedure that relies on measurements of digging forces in actual excavation tasks. The optimal motions generated by our method are compared with the actual excavation trajectories of experienced human operators. We find that the optimal digging motions, while for the most part similar to those performed by an experienced human operator, do show some important qualitative differences, particularly in the dumping and returning motions. These and other observations are discussed in detail. The remainder of the paper is organized as follows. Section 2 provides a high-level description of our simulation and motion planning architecture, together with a more detailed description of the main components. Section 3 derives the excavator dynamics for the four main tasks involved in earthmoving, and also describes both the soil model used for the study and a procedure for estimating its parameters. Section 4 presents details of the algorithm for generating minimum torque and minimum time motions. Section 5 presents a comparison of the optimal motions generated by our planner with the actual excavation trajectories performed by experienced human operators. We
conclude in Section 6 with a discussion of possible future extensions to our simulator and planner. 2. Simulation and planning architecture Fig. 1 depicts the overall architecture for our simulation and motion planning system. The three basic functions are dynamics simulation, motion planning and optimization, and soil parameter estimation. The dynamic simulation engine formulates the equations of motion for the four basic excavator tasks (digging, lifting, dumping, and returning). Motions obtained from dynamic simulations can be compared with measurement data taken from field experiments; a direct comparison between, e.g., the force profiles is one means of evaluating the accuracy and reliability of the dynamics simulation. Digging simulation requires a computational model for the soil dynamics, which in turn requires a method for estimating the soil model parameters. The soil parameter estimation module takes as input measurement data (obtained from field experiments), and estimates the soil model parameters via an optimization procedure. Among the external forces exerted on the excavator, digging forces are typically the largest, so that careful estimation of the soil model parameters is essential for realistic dynamics simulation.
Fig. 2. Excavator in reduced form.
570
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
λi
-λi Fig. 3. Forces at i-th cut points.
The planner invokes the dynamics simulation engine when generating motions, and takes as input the boundary conditions, i.e., the initial and final bucket pose and velocity, and produces optimal cylinder trajectories for the minimum torque/time criterion. Embedded within the planner are the requisite optimization algorithms, which require analytic gradients; these gradients can be obtained as a byproduct of the dynamics simulation as noted earlier.
αi
3. Excavator dynamic modeling For the purposes of this study we model only the rigid-body dynamics of the excavator, without taking into account the hydraulic actuator dynamics. The methodology presented in this paper can be straightforwardly extended to this case, although the governing equations of motion become more involved. The dynamic modeling of the excavator is complicated by the presence of four closed loops, and several passive joints. There are two main approaches to the dynamic modeling of such constrained multibody systems: (i) modeling the system by a set of differential equations subject to algebraic constraints (the differential-algebraic approach), and (ii) modeling the system exclusively by a set of differential equations formulated in terms of an independent set of generalized coordinates. For purely dynamic simulation purposes there is little difference between the two approaches. However, for motion optimization purposes, where analytic gradients of the objective function are extremely useful, the latter approach is computationally advantageous and more straightforward. Assuming that the excavator mechanism consists of a total of m one degree-of-freedom joints, the first step in the dynamic modeling is to distinguish the n actuated joints, denoted qa ∈Rn ; from the m-n m−n passive joints, denoted (for typical excavators, n = 4 and qp ∈R m = 16). Define q ¼ qa ; qp ∈Rm . The kinematic loop closure constraints can then be expressed in the following differential form (see, e.g., [8]): q_ ¼ Φq_ a Φ¼
I −J †p J a
Vs
ð1Þ ð2Þ
_ q_ þ Φq€ : q€ ¼ Φ a a
ð3Þ
αf
Bucket angle
Fig. 5. Dumping work.
an efficient screw-theoretic method for this differentiation. Below we shall have occasion to use the constraint Jacobian Jc, defined as J c ¼ J a J p ∈Rðm−nÞm : Having modeled the excavator kinematics in this form, we now convert the excavator into a topological tree structure (which we call a reduced system) by appropriately cutting all closed loops in the original system as shown in Fig. 2. For each cut point, we imagine a generalized force λi exerted at the passive joint that causes the reduced system to undergo exactly the same movement as the original system (see Fig. 3). Physically this generalized force can be identified with the internal force and bending moment at the corresponding point of the original closed-loop system. Denoting by the vector λ the collection of all the generalized forces λi, the dynamics of the excavator can be expressed in the form T
_ Þq_ þ V ðqÞ ¼ τ þ J c λ MðqÞq€ þ C ðq; q
ð4Þ
_ Þ∈Rmm and V ðqÞ∈Rm are where MðqÞ∈Rmm is the mass matrix, C ðq; q the terms associated with the Coriolis and gravitational forces of the reduced system, τ∈Rm is the vector of the torques, and λ∈Rm−n is the vector of constraint forces. Explicit formulations of these matrices, as well as efficient recursive algorithms for the evaluation of the dynamic equations, are provided in [5]. Following [5], τ˜ can then be cal_ ; €q Þ are assumed given: culated for the reduced system provided ðq; q _ Þq_ þ V ðqÞ τ≜M ˜ ðqÞq€ þ C ðq; q
ð5Þ
The matrices J a ∈Rðm−nÞn and J p ∈Rðm−nÞðm−nÞ above are obtained by time-differentiating the m-n kinematic loop closure equations g(qa,qp) = 0. q_ can be further differentiated to obtain q€ ; [8] describes
Ground
Measurement data
Soil Parameter Estimation
Vs Digging motion data
bucket-tip path Fig. 4. Bucket-tip path.
Soil-Tool Interaction Model
Digging force
Fig. 6. Block diagram illustrating calculation of digging forces.
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
571
Table 1 Optimization constraint for each working tasks.
terrain surface
cLf bucket blade Q
Equation number
Pmax
Qmax
_ max W
Vsmin
N.P.
Eq. Eq. Eq. Eq.
○ ○ ○ ○
○ ○ ○ ○
○ ○ ○ ○
○ × × ×
× ○ ○ ○
(12) (15) (16) (4)
The final remaining step is to compute τa (or λ) from the given _ q€ can be obtained from Eqs. (1) and (3), from which q; q_ a ; q€ a . Here q; τa and λ are then obtained from (5), (9), and (10). Note that Eq. (4) does not account for the soil forces. Taking these into account, the excavator dynamics equations assume the form
R
d
Working Digging Lifting Dumping Returning
fs
_ Þq_ þ V ðqÞ ¼ τ þ J Tc λ þ J Ts f s þ J Tk f k ; MðqÞq€ þ C ðq; q
caLt
failure plane
Fig. 7. Modified FEE model: pseudo-static model of earthmoving at critical point just before failure. α is the slop of the terrain, Lt is length of the tool, Lf is the length of the surface along which the wedge slides, Q is the surcharge, or the displaced soil that rests on the wedge, ca is the adhesion between the soil and blade, R is the force of the soil resisting the movement of the wedge, and fs is the force exerted by the tool to cause failure. The material in the shaded region constitutes all of the materials that have passed over the top of the bucket tip during the bucket's motion, and is referred to as the swept volume, Vs [9].
T
τ ¼ τ−J ˜ c λ:
ð6Þ
τ˜ here is the sum of the original torques τ with the constraint forces λ. Eq. (6) can be divided into two parts corresponding to the active and passive joint torques: T
τa ¼ τ˜ a −J a λ T
0 ¼ τ˜ p −J p λ:
†T
†T
3.1. Digging dynamics During digging, the soil lifting force fk in Eq. (11) is not exerted (that is, fk should be zero). In this case the equations of motion can be rewritten as follows: T
T
_ Þq_ þ V ðqÞ ¼ τ þ J c λ þ J s f s : MðqÞq€ þ C ðq; q
ð8Þ
The digging force fs can be calculated via the soil model, which we describe later in Section 3.5.
ð9Þ
τa ¼ τ˜ a −J a J p τp :
where Js is the soil–tool Jacobian, fs is the soil force exerted on the bucket during the digging, Jk is the soil–bucket Jacobian and fk is the force exerted on the bucket arising from soil weight during the lifting (details are given below). Thus far we have derived the rigid-body dynamics of the excavator in an independent context; we now examine the task-specific dynamics for truck loading, which consists of digging, lifting, dumping, and returning to the original digging position. The next subsections describe, for each subtask, how to formulate the inverse dynamics, which is needed in the motion optimization procedure.
ð7Þ
Note that the left-hand side of Eq. (8) is zero because the torques at passive joints are zero. From Eqs. (7) and (8), λ and τa can be obtained as follows: λ ¼ J p τp
ð11Þ
ð10Þ
ð12Þ
3.2. Lifting dynamics During lifting, the bucket is assumed to be filled with the soil obtained during the digging phase. The volume of soil, Vs, is calculated from the closed volume between the ground and bucket-tip path as Table 2 Minimum torque optimization algorithm. Minimun torque optimization
Digging force
Measurement
ΔF(t)
Simulation
ti
tf Fig. 8. Estimation of soil parameters.
Digging time
1 Input: Measurement data M, Motion script Φ = [Φdig,Φlift,Φdump,Φreturn], tf = [tf,dig,tf,lift,tf,dump,tf,return] 2 Soil parameter Ψ ← Soil parameter estimation(M) described in Section 3.5. ∗ 3 (Pdig ,soil volume Vs)← Digging motion minimum torque optimization (Φdig,Ψ,tf,dig) 3.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.1. 3.2 Constraints are given in Table 1. 3.3 The soil volume Vs is the closed volume as shown in Fig. 4. ∗ 4 Plift ← Lifting motion minimum torque optimization (Φlift,Vs,tf,lift) 4.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.2. 4.2 Constraints are given in Table 1. ∗ 5 Pdump ← Dumping motion minimum torque optimization (Φdump,tf,dump) 5.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.3. 5.2 Constraints are given in Table 1. ∗ 6 Preturn ← Returning motion minimum torque optimization (Φreturn,tf,return) 6.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.4. 6.2 Constraints are given in Table 1.
572
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
shown in Fig. 4. The soil carried in the bucket exerts a force fk on the bucket, whereas digging force fs should be zero during lifting phase. In general it is difficult to analytically calculate fk, since it depends on the interior shape of bucket and the motion of the bucket. However, since the soil mass can be readily computed, one can augment the bucket mass with the soil mass for a reasonable approximation of fk. To derive the equations of motion when the soil mass is augmented to the bucket mass in this form, the corresponding Euler–Lagrange equations can be written as follows: d ∂Lk ∂Lk _ Þq_ þ V k ðqÞ − ¼ M k ðqÞq€ þ C k ðq; q dt ∂q_ ∂q T
¼ Jk f k ;
ð13Þ ð14Þ
where Lk is the Lagrangian for the augmented soil. Using both Eqs. (11) and (14), the equations of motion for lifting can be written as follows: ðM ðqÞ þ Mk ðqÞÞq€ þ ðC ðq; q_ Þ þ C k ðq; q_ ÞÞq_ þ V ðqÞ þ V k ðqÞ ^ ðqÞq€ þ C^ ðq; q_ Þq_ þ V^ ðqÞ ¼ τ þ J T λ: ¼M c
During dumping, the soil volume in the bucket Vs decreases as a function of bucket angle α(q). As shown in Fig. 5, we assume that the soil volume Vs linearly decreases as the bucket angle α increases, in which case the equations of motion (16) can now be written ð16Þ
and the boundary conditions for each of terms in Eq. (16) are given as follows:
MðqÞ þ M k ðqÞ MðqÞ
at α ðqÞ ¼ α i at α ðqÞ ¼ α f
1 Input: Measurement data M, Motion script Φ = [Φdig,Φlift,Φdump,Φreturn] 2 Soil parameter Ψ ← Soil parameter estimation(M) described in Section 3.5. ∗ 3 (Pdig , tf,dig, soil volume Vs)← Digging motion minimum time optimization (Φdig,Ψ) 3.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.1. 3.2 Constraints are given in Table 1. 3.3 The soil volume Vs is the closed volume as shown in Fig. 4. ∗ 4 (Plift, tf,lift) ← Lifting motion minimum time optimization (Φlift,Vs) 4.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.2. 4.2 Constraints are given in Table 1. ∗ 5 (Pdump,tf,dump) ← Dumping motion minimum time optimization (Φdump) 5.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.3. 5.2 Constraints are given in Table 1. ∗ 6 (Preturn, tf,return) ← Returning motion minimum time optimization (Φreturn) 6.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.4. 6.2 Constraints are given in Table 1.
proposed in [7], illustrated in Fig. 7. This model specifies the soil– tool dynamic interaction in terms of the five soil parameters δ, ϕ, β, γ, and c:
3.3. Dumping dynamics
^ ðq; α Þ ¼ M
Minimun time optimization
ð15Þ
Note the similarity between Eqs. (15) and (11).
^ ðq; α Þq€ þ C^ ðq; q; _ α Þq_ þ V^ ðq; α Þ ¼ τ þ J Tc λ; M
Table 3 Minimum time optimization algorithm.
ð17Þ
• δ: the angle between bucket blade and soil force fs exerted on bucket blade as shown in Fig. 7 • ϕ: the angle between the failure plane and resistance force R as shown in Fig. 7 • β: the angle between the terrain surface and the failure plane as shown in Fig. 7 • γ: the mass density of the soil • c: soil cohesion parameter with the same unit as pressure. The soil force fs can be calculated as a function of five parameters, 2
f s ¼ d wγgN w þ cwdNc þ V s γgNq
ð20Þ
Nw ¼
ðcotβ−tanα Þðcosα þ sinαcot ðβ þ ϕÞÞ 2½cosðρ þ δÞ þ sinðρ þ δÞcot ðβ þ ϕÞ
ð21Þ
ð19Þ
Nc ¼
1 þ cotβcot ðβ þ ϕÞ cosðρ þ δÞ þ sinðρ þ δÞcot ðβ þ ϕÞ
ð22Þ
where αi, αf respectively denote the start and final angles for dump^ C^ and V^ in Eq. (16) depends on ing. Note that each of the terms M, the bucket angle α as well as the joint angle vector q.
Nq ¼
cosα þ sinαcot ðβ þ ϕÞ cosðρ þ δÞ þ sinðρ þ δÞcot ðβ þ ϕÞ
ð23Þ
_ αÞ ¼ C^ ðq; q; V^ ðq; α Þ ¼
_ Þ at α ðqÞ ¼ α i _ Þ þ C k ðq; q C ðq; q _Þ C ðq; q at α ðqÞ ¼ α f
V ðqÞ þ V k ðqÞ V ðqÞ
at α ðqÞ ¼ α i ; at α ðqÞ ¼ α f
ð18Þ
3.4. Returning dynamics If during the returning task no external forces other than gravity are exerted on the excavator, then the dynamic equations in this case are given by Eq. (4). 3.5. Soil modeling and identification In general, calculating soil digging forces is complicated by the difficulty in constructing accurate models that capture the static and dynamic properties of the soil. The choice of soil–tool interaction model crucially affects the accuracy and performance of digging simulation. The soil–tool interaction process is summarized in Fig. 6. Recent contributions to soil–tool interaction modeling include the fundamental earth-moving equation (FEE) [6], learning based models [10], spring-damper models [11], and energy-dissipation models [12]. For our purposes we choose a modified version of the FEE model
where Γ = (d,w,α,ρ,Vs) is a set of additional geometric parameters which correspond to the depth of the bucket tip, the width of the bucket, the terrain slope, rake angle, and swept volume. Γ can be calculated once bucket pose, bucket shape and terrain shape are known. The derivation of fs is given in [9]. We note that this model describes the static force-displacement relation between the soil and tool; while for our purposes this model is sufficient for the most part, for fast, highly dynamic movements of the bucket the accuracy of the dynamic simulation can be expected to degrade.
Table 4 Specifications for the hydraulic excavator used in experiments. Item
Value
Operating weight Boom length Arm length Bucket capacity
29,300 kg 6.25 m 3.05 m 1.27 m3
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
573
Fig. 9. Displacement and pressure sensors installed on the hydraulic excavator.
We now address the estimation of the soil model parameters. In this study we formulate this as an optimization problem, in which the following objective function is minimized: t
soil parameters ¼ arg min ∫tfi jΔF ðt Þj dt; δ;Φ;β;γ;c
ð24Þ
where ti is the time at which digging starts, tf is the time at which digging ends, and ΔF(t) is the difference between the measured force and the force calculated from the soil model. The optimization variables t are the soil model parameters, and the objective function ∫t fi jΔF ðt Þj dt is taken to be the integral of the absolute value of the force difference (which, as shown in Fig. 8, corresponds to the area between the two indicated curves). 4. Minimum torque/time optimization 4.1. Formulation The generation of minimum torque/time trajectories for excavators can be approached in a number of ways. One possible optimal control formulation involves augmenting the minimum torque functional with a penalty term for the final time tf, i.e., 1 t 2 min ϕ t f þ ∫0f jjτðt Þjj dt 2 τ ðt Þ
qðt; P Þ ¼
Bi ðt Þpi ;
ð25Þ
ð26Þ
i¼1
where Bi(t) is a B-spline basis function and P = [p1,p2, ⋯,pm] is the set of control points. The cost functional then reduces to a parameter optimization problem of the form 1 t 2 min ϕ t f þ ∫0f jjτðP; t Þjj dt P 2
• Equation number: governing dynamics equation number • Pmax: maximum pressure
Qmax: maximum flow rate _ max : maximum power W Vsmin: minimum amount of soil volume N.P. (No Penetration): whether penetration between ground and bucket is allowed (×) or not (○)
subject to the same constraints as described in Table 1. The basis functions must be chosen so as to satisfy all the terminal boundary conditions. For the minimum torque problem in which the final time tf is given, it is intuitively clear that for larger values of tf, the minimum torque profiles become increasingly flatter. If tf is allowed to vary, the first term ϕ(tf) acts as a penalty function to prevent the final time from becoming arbitrarily large in order to decrease torque consumption. The functional ϕ(tf) should be a monotonically increasing function of tf, and should be appropriately weighted for balancing between final time and torque consumption. Penalty methods are highly sensitive to weighting parameters, and in this paper we adopt an approach initially developed for open chain arms initially described in [14]. Specifically, we minimize the peak torque max τ P ; t ∞
ð27Þ
ð28Þ
tf
t
2
P ¼ arg min ∫0f jjτðP; t Þjj dt P
subject to boundary conditions and various physical constraints as summarized in Table 1. In [13] a local solution to the optimal control problem is found by assuming that the joint trajectories q(t) are parametrized in terms of B-splines: m X
• • • •
ð29Þ
where ||⋅ ||∞ denotes the L∞ norm, and subject to the same constraints given in Table 1. This formulation is based on the observation that the peak torque attained by the minimum torque trajectory decreases monotonically as tf is increased. A specific algorithm for solving this problem is described in Section 4.2. 4.2. Algorithms Numerical procedures for solving the minimum torque problem and minimum time problem are described in Tables 2 and 3. Here M denotes the measurement data containing the cylinder trajectories and corresponding cylinder forces for several digging motion trials, Φ is the motion script which contains the initial and final configurations of digging, lifting, dumping and returning motions specified by the user, and tf is the duration time for each motion. For the minimum time problem described in Table 3, the motion optimization procedure is as follows 1. Optimize the minimum torque for a fixed tf and check all constraints. 2. Using an appropriate line search algorithm, decrease the final time tf if all constraints are satisfied, otherwise increase tf.
574
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
3. Repeat the routine from step 1 to step 2 until some termination condition is satisfied. The final time tf can be determined via a standard line search procedure, e.g., golden section search, with an upper bound on admissible values of tf. To handle the constraints specified in Table 1, it is desirable to enforce hard constraints as much as possible, and to convert some constraints into soft constraints as required. 5. Experimental results In this section we present experimental results verifying the fidelity of our dynamics simulations, and a comparison of the trajectories
force (KGF)
10
produced by our planner with those produced by an experienced human operator. Physical specifications for the excavator used in the experiments are listed in Table 4. Displacement sensors and pressure sensors attached each of the cylinders at the boom, arm, and bucket measure the corresponding cylinder displacement and force (Fig. 9). 5.1. Verification of excavator dynamics simulation We first compare the cylinder force trajectories produced by our dynamic simulation with actual measurements. The cylinder force trajectories are measured for the two cases: (1) without soil–tool interaction, and (2) with soil–tool interaction. Fig. 10 compares the simulated
Boom cylinder force
x 104
measure simulation
5
0
−5 0
10
20
30
40
50
60
70
80
time (s)
force (KGF)
1
Arm cylinder force
x 104
measure simulation
0.5 0 −0.5 −1 −1.5 −2 0
10
20
30
40
50
60
70
80
time (s)
force (KGF)
1
Bucket cylinder force
x 104
measure simulation
0.5 0 −0.5 −1 0
10
20
30
40
50
60
70
80
time (s)
torque (KGF−m)
10
Swing torque
x 104
measure simulation
5
0
−5
0
10
20
30
40
50
60
70
time (s) Fig. 10. A comparison of simulated and measured cylinder forces without soil–tool interaction.
80
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
force (KGF)
10
575
Boom cylinder force
x 104
measure simulation
5
0
−5
0
20
40
60
80
100
120
140
time (s)
force (KGF)
6
Arm cylinder force
x 104
measure simulation
4 2 0 −2 −4
0
20
40
60
80
100
120
140
time (s)
force (KGF)
4
Bucket cylinder force
x 104
measure simulation
3 2 1 0 −1
0
20
40
60
80
100
120
140
time (s)
torque (KGF−m)
10
Swing torque
x 104
measure simulation
5
0
−5
0
20
40
60
80
100
120
140
time (s) Fig. 11. A comparison of simulated and measured cylinder forces with soil–tool interaction.
and measured forces for repeated trials of a circular reference bucket motion in three dimensions, performed by a human operator. The simulated forces show good agreement with the measurements; the offsets in arm and bucket forces can be traced to small discrepancies in the model parameters (i.e., masses and inertias of the links), as well as unmodelled dynamic effects such as friction, bending, and hysteresis. For the case with soil–tool interactions, we first estimate the five parameters in the modified FEE soil model using the procedure described earlier. Measurement data has been obtained for eight excavation cycles, in which each cycle consists of digging–lifting–dumping–returning. We use the first digging cycle to estimate the soil parameters, and then compare the predicted and measured cylinder forces over the remaining seven cycles; the results are shown in Figs. 11 and 12.
5.2. Optimized motion versus operator motion We now generate optimal motions using the models and algorithms described earlier, and compare them to the motions generated by an experienced human operator. Minimum torque/time motions are obtained for initial and final times that correspond to those of the human operator motion. Table 5 compares the task execution times and average torque sums of the human operator motion, the minimum-time motion (using a minimum-time motion planner), and the minimum torque/time motions generated by our planner (Figs. 15 and 16). The results suggest that the operator uses excessive torques compared to our planner's motions. Note that the minimum-time motion only results in a small improvement in task execution time
576
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
10
Boom cylinder force
x 104
5 measure simulation
x 104
Arm cylinder force measure simulation
4 3
force (KGF)
force (KGF)
5
0
2 1 0 −1 −2
−5 0
5
10
−3 0
15
5
time (s)
3
Bucket cylinder force
x 104
4 measure simulation
2.5
torque (KGF−m)
force (KGF)
15
Swing torque
x 104
measure simulation
3
2 1.5 1 0.5 0
2 1 0 −1 −2
−0.5 −1
10
time (s)
0
5
10
15
time (s)
−3 0
5
10
15
time (s)
Fig. 12. Cylinder forces over a single cycle. Dotted lines separate digging, lifting, dumping, and returning periods from left to right.
compared to our planner's motion, while requiring larger torques. The resulting force profiles are shown in Figs. 13 and 14.
6. Conclusions As a prelude to the realization of fully unmanned excavators, this study has attempted to determine, based on dynamic models of the excavator as well as soil–tool interaction models, excavator motions that are fast, power-efficient, and smooth. The main technical contribution of our paper is a set of analytic gradient-based motion optimization algorithms, specialized to earthmoving excavators, for generating the fastest possible minimum torque motions —that is, without violating any actuator saturation limits, or other constraints on cylinder velocities or accelerations; we call such motions minimum torque/time motions— taking into account the closed-loop structure of excavators, the soil dynamics during digging and dumping, and any collision avoidance constraints that arise during typical excavation tasks. Such motions avoid the undesirable bang-bang characteristics of typical minimum-time trajectories, while being fast, smooth, and energy-efficient. The optimal motions generated by the planner are also compared with the actual excavation trajectories of experienced human operators.
Table 5 Comparison of motions. Criteria
Measurement
Min time
Min torque
tf Average ||τ(t)||2
14.9 1.0145
12.6 0.61483
14.9 0.61128
One of the interesting findings of our experiments is that there is notable difference between the generated optimal motion and actual excavation trajectories performed by a human operator, particularly during the arm return motion. We suspect that these differences can be mitigated somewhat by imposing more physically realistic constraints on the hydraulics, and taking into account collision avoidance between the excavator and truck. At a higher level, this paper has also presented an offline simulation and planning system for unmanned excavators, that integrates an excavator dynamic simulation engine with our optimal motion planner. One of the key elements in both the planning and simulation phases is the use of a soil model. In this paper we have described a nonlinear least-squares soil model parameter identification procedure that relies on measurements of digging forces in actual excavation tasks. The presented work can be extended in a number of ways. Taking into account the dynamics of the hydraulic actuators is clearly useful, in particular considerations on, e.g., hydraulic actuator limits and heat loss considerations. Although [4] presents one realization of such a model, accurate hydraulic actuator dynamic modeling is a highly complicated task, and it may be profitable to examine hierarchical models that capture features of the dynamics at different resolution scales. Improved soil models, as well as excavation tasks in more extreme unstructured environments, are also being examined in the current context, as well as taking into account ground collision avoidance constraints that arise during typical excavation tasks. There are also several means of bootstrapping the above procedure into a class of methods for generating real-time suboptimal trajectories; one particular method is described in [14] that involves principal component analysis of a dataset of optimally generated motions.
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
Boom cylinder trajectory
Arm cylinder trajectory
3300
3200 measure min time min torque
3200 3100
measure min time min torque
3100
displacement (mm)
displacement (mm)
577
3000 2900 2800 2700 2600 2500
3000 2900 2800 2700 2600 2500 2400
2400 0
5
10
2300 0
15
5
10
time (s) Bucket cylinder trajectory
Swing trajectory
3000
80 measure min time min torque
2800
displacement (degree)
displacement (mm)
15
time (s)
2600 2400 2200 2000
measure min time min torque
70 60 50 40 30 20 10
1800 0
5
10
0 0
15
5
10
time (s)
15
time (s) Fig. 13. Cylinder trajectories.
1.5
x 105
Boom cylinder force 8 measure min time min torque
1
Arm cylinder force
x 104
measure min time min torque
6
force (KGF)
force (KGF)
4 0.5 0
−0.5
2 0 −2 −4
−1
−1.5
6
−6 0
x 104
5
time (s)
10
Bucket cylinder force 1
0
torque (KGF−m)
2 0 −2
time (s)
10
15
10
15
Swing torque
x 104
0.5
0
measure min time min torque
−0.5
−4 −6
5
measure min time min torque
4
force (KGF)
−8
15
0
5
10
15
−1
time (s)
0
5
time (s) Fig. 14. Cylinder forces.
578
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
Fig. 15. Minimum time solution. Red curve shows the bucket-tip path of the optimal motion; the blue curve shows the human operator's motion.
Fig. 16. Minimum torque solution. Red curve shows the bucket-tip path of the optimal motion; the blue curve shows the human operator's motion.
Acknowledgment This research was supported by a research grant from Hyundai Heavy Industries, with further partial support from the AIM Center, ROSAECERC, SNU-IAMD, and the SNU-BK21 Program in Mechanical Engineering. References [1] A.T. Stentz, J. Bares, S. Singh, P. Rowe, A robotic excavator for autonomous truck loading, Autonomous Robots 7 (1999) 175–186. [2] H. Yamamoto, M. Moteki, H. Shao, T. Ootuki, H. Kanazawa, Y. Tanaka, Basic technology toward autonomous hydraulic excavator, International Symposium on Automation and Robotics in Construction, 2009, pp. 288–295. [3] S.H. Lee, J. Kim, F.C. Park, M. Kim, J.E. Bobrow, Newton-type algorithms for dynamics-based robot movement optimization, IEEE Transactions on Robotics 21 (2005) 657–667. [4] S. Yoo, C.G. Park, B. Lim, K.I. Lee, F.C. Park, Bandwidth maximizing design for hydraulically actuated excavators, Journal of Vibration and Control 16 (2010) 2109–2130. [5] F.C. Park, J.E. Bobrow, S.R. Ploen, A lie group formulation of robot dynamics, International Journal Of Robotics Research 14 (1995) 609–618.
[6] A.R. Recee, The fundamental equation of earth-moving mechanics, Proceedings of the Institution of Mechanical Engineers, 179, 1964, pp. 16–22. [7] O. Luengo, S. Singh, H. Cannon, Modeling and identification of soil-tool interaction in automated excavation, IEEE/RSJ International Conference on Intelligent Robotic Systems, 1998, pp. 1900–1906. [8] F.C. Park, J. Choi, S. Ploen, Symbolic formulation of closed chain dynamics in independent coordinates, Mechanism and Machine Theory 34 (1999) 731–751. [9] H. Cannon, S. Singh, Models for automated earthmoving, Experimental Robotics VI, Lecture Notes in Control and Information Sciences, Springer Verlag, 1999, pp. 183–192. [10] S. Singh, Learning to predict resistive forces during robotic excavation, International Conference on Robotics and Automation, 1995, pp. 2102–2107. [11] S.P. DiMaio, S.E. Salcudean, C. Reboulet, A virtual environment for the simulation and programming of excavation trajectories, presence: teleoper, Virtual Environment 10 (2001) 465–476. [12] S.M. Vahed, X. Song, J.S. Dai, H.K. Lam, L.D. Seneviratne, K. Althoefer, Soil estimation based on dissipation energy during autonomous excavation, The International Federation of Automatic Control, 2008, pp. 13821–13826. [13] J.E. Bobrow, B. Martin, G. Sohl, E.C. Wang, J. Kim, F.C. Park, Optimal robot motions for physical criteria, Journal of Robotic Systems 18 (2001) 785–795. [14] S. Kim, F.C. Park, Fast robot motion generation using principal components: framework and algorithms, IEEE Transactions on Industrial Electronics 55 (2008) 2506–2516.
Contents lists available at ScienceDirect
Automation in Construction journal homepage: www.elsevier.com/locate/autcon
Dynamically optimal trajectories for earthmoving excavators Young Bum Kim, Junhyoung Ha ⁎, Hyuk Kang, Pan Young Kim, Jinsoo Park, F.C. Park a r t i c l e
i n f o
Article history: Accepted 10 December 2012 Available online 7 March 2013 Keywords: Excavator Earthmoving Motion optimization Minimum torque
a b s t r a c t We consider the problem of generating time-efficient, minimum torque motions for earthmoving excavators. Limits on actuator forces, as well as on joint velocities and accelerations, are assumed given, and the objective is to produce the fastest possible minimum torque motions while avoiding torque saturation limits. Relying on a set of recursive geometric algorithms for calculating the excavator dynamics that also calculates analytic gradients and Hessians of the dynamics as a by-product, we develop robust, reliable algorithms that generate such optimal trajectories. The resulting trajectories are compared with actual digging motions of experienced operators. © 2013 Elsevier B.V. All rights reserved.
1. Introduction Interest in unmanned excavators is driven by many considerations, from a desire to reduce the numerous fatal accidents that occur regularly to human operators—particularly in dangerous environments involving highly uneven terrain and unstable ground—to improving task efficiency by automating excavation in an optimal manner. To realize such potential benefits, an unmanned excavator must have the capability to produce optimal motions in a reliable and efficient manner. While there are many possible notions of optimality, our focus in this paper is on excavator motions that are fast, power-efficient, and smooth. These criteria naturally require careful consideration of the excavator dynamics. In some relevant previous work, [1] reports on a fully automated excavator that senses the dig face, recognizes and localizes the truck, detects any obstacles, and finally produces a feasible digging path via three planning algorithms: a coarse-to-fine dig point planning, templatebased dump planning, and script-based planning. It is important to note that none of these planning algorithms take into account the dynamics of the excavator; only the geometry of the terrain and kinematic constraints are considered. More recently in [2], a high-level hierarchical framework for autonomous construction machinery is proposed, in which the planning, measurement, and control functions are clearly identified and defined. While the general issues involved in planning are addressed, no specific planners are proposed. In this paper we report on a class of algorithms for generating dynamically optimal excavator trajectories, and more broadly, on an offline simulation and motion planning system for unmanned excavators. The benefits of a computer simulation and planning tool for such purposes are obvious: a means of evaluating and optimizing system performance, from design to motion planning and control, as well
⁎ Corresponding author. E-mail address: [email protected] (J. Ha). 0926-5805/$ – see front matter © 2013 Elsevier B.V. All rights reserved. http://dx.doi.org/10.1016/j.autcon.2013.01.007
as reduced development time and costs with fewer iterations of prototype construction and testing. Our main technical contribution is a class of motion optimization algorithms for generating the fastest possible minimum torque motions. As has been reported in some of our earlier work, [3], that motion optimization taking into account the dynamics is, although seemingly straightforward in principle, computationally and numerically highly challenging in practice. For one thing, the availability of analytic gradients of the objective function and constraints impacts numerical convergence in a significant way; finite difference approximations of the gradient often result in numerical instabilities and poor convergence behavior. Moreover, the presence of closed loop constraints—the kinematics of a typical excavator involves several closed loops and passive joints that are not actuated—complicates the calculation of the dynamics and the gradients. Drawing upon some of our previous work [3–5], we develop an optimization algorithm that robustly and efficiently obtains the fastest possible minimum torque motion—that is, without violating any actuator saturation limits, or other constraints on cylinder velocities or accelerations; we call such motions minimum torque/time motions—taking into account the closed-loop structure of excavators, and the soil dynamics during digging and dumping, i.e., in a way that reflects the influence of soil–tool interaction forces during digging, and the effects of soil loads during lifting and dumping. Such motions avoid the undesirable bang-bang characteristics of typical minimum-time trajectories, while being fast, smooth, and energy-efficient. The key to our algorithm lies in the ability to efficiently calculate analytic gradients (and if necessary analytic Hessians) of the objective function, all as a simple by-product of the recursive dynamics calculation—this is achieved by drawing upon the geometric algorithms first derived in [5]. More broadly, our motion optimization algorithm is part of an offline planning and simulation system that integrates disparate components, ranging from a dynamics simulation engine to methods for soil model parameter identification, into a coherent and easy-to-use framework. We describe the basic elements of our overall motion
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
Measurement data
569
Soil Parameter Estimation
Dynamics Digging
Lifting Comparison results between simulation and measurement
Returning
Boundary conditions
Dumping
Optimal Trajectories
Planning and Optimization
Fig. 1. Architecture for our simulation and motion planning system.
planning and simulation architecture. In particular, using soil models based on the fundamental earth moving equations (FEE) [6] and its variants [7], we describe a soil model parameter identification procedure that relies on measurements of digging forces in actual excavation tasks. The optimal motions generated by our method are compared with the actual excavation trajectories of experienced human operators. We find that the optimal digging motions, while for the most part similar to those performed by an experienced human operator, do show some important qualitative differences, particularly in the dumping and returning motions. These and other observations are discussed in detail. The remainder of the paper is organized as follows. Section 2 provides a high-level description of our simulation and motion planning architecture, together with a more detailed description of the main components. Section 3 derives the excavator dynamics for the four main tasks involved in earthmoving, and also describes both the soil model used for the study and a procedure for estimating its parameters. Section 4 presents details of the algorithm for generating minimum torque and minimum time motions. Section 5 presents a comparison of the optimal motions generated by our planner with the actual excavation trajectories performed by experienced human operators. We
conclude in Section 6 with a discussion of possible future extensions to our simulator and planner. 2. Simulation and planning architecture Fig. 1 depicts the overall architecture for our simulation and motion planning system. The three basic functions are dynamics simulation, motion planning and optimization, and soil parameter estimation. The dynamic simulation engine formulates the equations of motion for the four basic excavator tasks (digging, lifting, dumping, and returning). Motions obtained from dynamic simulations can be compared with measurement data taken from field experiments; a direct comparison between, e.g., the force profiles is one means of evaluating the accuracy and reliability of the dynamics simulation. Digging simulation requires a computational model for the soil dynamics, which in turn requires a method for estimating the soil model parameters. The soil parameter estimation module takes as input measurement data (obtained from field experiments), and estimates the soil model parameters via an optimization procedure. Among the external forces exerted on the excavator, digging forces are typically the largest, so that careful estimation of the soil model parameters is essential for realistic dynamics simulation.
Fig. 2. Excavator in reduced form.
570
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
λi
-λi Fig. 3. Forces at i-th cut points.
The planner invokes the dynamics simulation engine when generating motions, and takes as input the boundary conditions, i.e., the initial and final bucket pose and velocity, and produces optimal cylinder trajectories for the minimum torque/time criterion. Embedded within the planner are the requisite optimization algorithms, which require analytic gradients; these gradients can be obtained as a byproduct of the dynamics simulation as noted earlier.
αi
3. Excavator dynamic modeling For the purposes of this study we model only the rigid-body dynamics of the excavator, without taking into account the hydraulic actuator dynamics. The methodology presented in this paper can be straightforwardly extended to this case, although the governing equations of motion become more involved. The dynamic modeling of the excavator is complicated by the presence of four closed loops, and several passive joints. There are two main approaches to the dynamic modeling of such constrained multibody systems: (i) modeling the system by a set of differential equations subject to algebraic constraints (the differential-algebraic approach), and (ii) modeling the system exclusively by a set of differential equations formulated in terms of an independent set of generalized coordinates. For purely dynamic simulation purposes there is little difference between the two approaches. However, for motion optimization purposes, where analytic gradients of the objective function are extremely useful, the latter approach is computationally advantageous and more straightforward. Assuming that the excavator mechanism consists of a total of m one degree-of-freedom joints, the first step in the dynamic modeling is to distinguish the n actuated joints, denoted qa ∈Rn ; from the m-n m−n passive joints, denoted (for typical excavators, n = 4 and qp ∈R m = 16). Define q ¼ qa ; qp ∈Rm . The kinematic loop closure constraints can then be expressed in the following differential form (see, e.g., [8]): q_ ¼ Φq_ a Φ¼
I −J †p J a
Vs
ð1Þ ð2Þ
_ q_ þ Φq€ : q€ ¼ Φ a a
ð3Þ
αf
Bucket angle
Fig. 5. Dumping work.
an efficient screw-theoretic method for this differentiation. Below we shall have occasion to use the constraint Jacobian Jc, defined as J c ¼ J a J p ∈Rðm−nÞm : Having modeled the excavator kinematics in this form, we now convert the excavator into a topological tree structure (which we call a reduced system) by appropriately cutting all closed loops in the original system as shown in Fig. 2. For each cut point, we imagine a generalized force λi exerted at the passive joint that causes the reduced system to undergo exactly the same movement as the original system (see Fig. 3). Physically this generalized force can be identified with the internal force and bending moment at the corresponding point of the original closed-loop system. Denoting by the vector λ the collection of all the generalized forces λi, the dynamics of the excavator can be expressed in the form T
_ Þq_ þ V ðqÞ ¼ τ þ J c λ MðqÞq€ þ C ðq; q
ð4Þ
_ Þ∈Rmm and V ðqÞ∈Rm are where MðqÞ∈Rmm is the mass matrix, C ðq; q the terms associated with the Coriolis and gravitational forces of the reduced system, τ∈Rm is the vector of the torques, and λ∈Rm−n is the vector of constraint forces. Explicit formulations of these matrices, as well as efficient recursive algorithms for the evaluation of the dynamic equations, are provided in [5]. Following [5], τ˜ can then be cal_ ; €q Þ are assumed given: culated for the reduced system provided ðq; q _ Þq_ þ V ðqÞ τ≜M ˜ ðqÞq€ þ C ðq; q
ð5Þ
The matrices J a ∈Rðm−nÞn and J p ∈Rðm−nÞðm−nÞ above are obtained by time-differentiating the m-n kinematic loop closure equations g(qa,qp) = 0. q_ can be further differentiated to obtain q€ ; [8] describes
Ground
Measurement data
Soil Parameter Estimation
Vs Digging motion data
bucket-tip path Fig. 4. Bucket-tip path.
Soil-Tool Interaction Model
Digging force
Fig. 6. Block diagram illustrating calculation of digging forces.
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
571
Table 1 Optimization constraint for each working tasks.
terrain surface
cLf bucket blade Q
Equation number
Pmax
Qmax
_ max W
Vsmin
N.P.
Eq. Eq. Eq. Eq.
○ ○ ○ ○
○ ○ ○ ○
○ ○ ○ ○
○ × × ×
× ○ ○ ○
(12) (15) (16) (4)
The final remaining step is to compute τa (or λ) from the given _ q€ can be obtained from Eqs. (1) and (3), from which q; q_ a ; q€ a . Here q; τa and λ are then obtained from (5), (9), and (10). Note that Eq. (4) does not account for the soil forces. Taking these into account, the excavator dynamics equations assume the form
R
d
Working Digging Lifting Dumping Returning
fs
_ Þq_ þ V ðqÞ ¼ τ þ J Tc λ þ J Ts f s þ J Tk f k ; MðqÞq€ þ C ðq; q
caLt
failure plane
Fig. 7. Modified FEE model: pseudo-static model of earthmoving at critical point just before failure. α is the slop of the terrain, Lt is length of the tool, Lf is the length of the surface along which the wedge slides, Q is the surcharge, or the displaced soil that rests on the wedge, ca is the adhesion between the soil and blade, R is the force of the soil resisting the movement of the wedge, and fs is the force exerted by the tool to cause failure. The material in the shaded region constitutes all of the materials that have passed over the top of the bucket tip during the bucket's motion, and is referred to as the swept volume, Vs [9].
T
τ ¼ τ−J ˜ c λ:
ð6Þ
τ˜ here is the sum of the original torques τ with the constraint forces λ. Eq. (6) can be divided into two parts corresponding to the active and passive joint torques: T
τa ¼ τ˜ a −J a λ T
0 ¼ τ˜ p −J p λ:
†T
†T
3.1. Digging dynamics During digging, the soil lifting force fk in Eq. (11) is not exerted (that is, fk should be zero). In this case the equations of motion can be rewritten as follows: T
T
_ Þq_ þ V ðqÞ ¼ τ þ J c λ þ J s f s : MðqÞq€ þ C ðq; q
ð8Þ
The digging force fs can be calculated via the soil model, which we describe later in Section 3.5.
ð9Þ
τa ¼ τ˜ a −J a J p τp :
where Js is the soil–tool Jacobian, fs is the soil force exerted on the bucket during the digging, Jk is the soil–bucket Jacobian and fk is the force exerted on the bucket arising from soil weight during the lifting (details are given below). Thus far we have derived the rigid-body dynamics of the excavator in an independent context; we now examine the task-specific dynamics for truck loading, which consists of digging, lifting, dumping, and returning to the original digging position. The next subsections describe, for each subtask, how to formulate the inverse dynamics, which is needed in the motion optimization procedure.
ð7Þ
Note that the left-hand side of Eq. (8) is zero because the torques at passive joints are zero. From Eqs. (7) and (8), λ and τa can be obtained as follows: λ ¼ J p τp
ð11Þ
ð10Þ
ð12Þ
3.2. Lifting dynamics During lifting, the bucket is assumed to be filled with the soil obtained during the digging phase. The volume of soil, Vs, is calculated from the closed volume between the ground and bucket-tip path as Table 2 Minimum torque optimization algorithm. Minimun torque optimization
Digging force
Measurement
ΔF(t)
Simulation
ti
tf Fig. 8. Estimation of soil parameters.
Digging time
1 Input: Measurement data M, Motion script Φ = [Φdig,Φlift,Φdump,Φreturn], tf = [tf,dig,tf,lift,tf,dump,tf,return] 2 Soil parameter Ψ ← Soil parameter estimation(M) described in Section 3.5. ∗ 3 (Pdig ,soil volume Vs)← Digging motion minimum torque optimization (Φdig,Ψ,tf,dig) 3.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.1. 3.2 Constraints are given in Table 1. 3.3 The soil volume Vs is the closed volume as shown in Fig. 4. ∗ 4 Plift ← Lifting motion minimum torque optimization (Φlift,Vs,tf,lift) 4.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.2. 4.2 Constraints are given in Table 1. ∗ 5 Pdump ← Dumping motion minimum torque optimization (Φdump,tf,dump) 5.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.3. 5.2 Constraints are given in Table 1. ∗ 6 Preturn ← Returning motion minimum torque optimization (Φreturn,tf,return) 6.1 Objective function is given in Eq. (27) with the dynamics specified in Section 3.4. 6.2 Constraints are given in Table 1.
572
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
shown in Fig. 4. The soil carried in the bucket exerts a force fk on the bucket, whereas digging force fs should be zero during lifting phase. In general it is difficult to analytically calculate fk, since it depends on the interior shape of bucket and the motion of the bucket. However, since the soil mass can be readily computed, one can augment the bucket mass with the soil mass for a reasonable approximation of fk. To derive the equations of motion when the soil mass is augmented to the bucket mass in this form, the corresponding Euler–Lagrange equations can be written as follows: d ∂Lk ∂Lk _ Þq_ þ V k ðqÞ − ¼ M k ðqÞq€ þ C k ðq; q dt ∂q_ ∂q T
¼ Jk f k ;
ð13Þ ð14Þ
where Lk is the Lagrangian for the augmented soil. Using both Eqs. (11) and (14), the equations of motion for lifting can be written as follows: ðM ðqÞ þ Mk ðqÞÞq€ þ ðC ðq; q_ Þ þ C k ðq; q_ ÞÞq_ þ V ðqÞ þ V k ðqÞ ^ ðqÞq€ þ C^ ðq; q_ Þq_ þ V^ ðqÞ ¼ τ þ J T λ: ¼M c
During dumping, the soil volume in the bucket Vs decreases as a function of bucket angle α(q). As shown in Fig. 5, we assume that the soil volume Vs linearly decreases as the bucket angle α increases, in which case the equations of motion (16) can now be written ð16Þ
and the boundary conditions for each of terms in Eq. (16) are given as follows:
MðqÞ þ M k ðqÞ MðqÞ
at α ðqÞ ¼ α i at α ðqÞ ¼ α f
1 Input: Measurement data M, Motion script Φ = [Φdig,Φlift,Φdump,Φreturn] 2 Soil parameter Ψ ← Soil parameter estimation(M) described in Section 3.5. ∗ 3 (Pdig , tf,dig, soil volume Vs)← Digging motion minimum time optimization (Φdig,Ψ) 3.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.1. 3.2 Constraints are given in Table 1. 3.3 The soil volume Vs is the closed volume as shown in Fig. 4. ∗ 4 (Plift, tf,lift) ← Lifting motion minimum time optimization (Φlift,Vs) 4.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.2. 4.2 Constraints are given in Table 1. ∗ 5 (Pdump,tf,dump) ← Dumping motion minimum time optimization (Φdump) 5.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.3. 5.2 Constraints are given in Table 1. ∗ 6 (Preturn, tf,return) ← Returning motion minimum time optimization (Φreturn) 6.1 Objective function is given in Eqs. (28) and (29) with the dynamics specified in Section 3.4. 6.2 Constraints are given in Table 1.
proposed in [7], illustrated in Fig. 7. This model specifies the soil– tool dynamic interaction in terms of the five soil parameters δ, ϕ, β, γ, and c:
3.3. Dumping dynamics
^ ðq; α Þ ¼ M
Minimun time optimization
ð15Þ
Note the similarity between Eqs. (15) and (11).
^ ðq; α Þq€ þ C^ ðq; q; _ α Þq_ þ V^ ðq; α Þ ¼ τ þ J Tc λ; M
Table 3 Minimum time optimization algorithm.
ð17Þ
• δ: the angle between bucket blade and soil force fs exerted on bucket blade as shown in Fig. 7 • ϕ: the angle between the failure plane and resistance force R as shown in Fig. 7 • β: the angle between the terrain surface and the failure plane as shown in Fig. 7 • γ: the mass density of the soil • c: soil cohesion parameter with the same unit as pressure. The soil force fs can be calculated as a function of five parameters, 2
f s ¼ d wγgN w þ cwdNc þ V s γgNq
ð20Þ
Nw ¼
ðcotβ−tanα Þðcosα þ sinαcot ðβ þ ϕÞÞ 2½cosðρ þ δÞ þ sinðρ þ δÞcot ðβ þ ϕÞ
ð21Þ
ð19Þ
Nc ¼
1 þ cotβcot ðβ þ ϕÞ cosðρ þ δÞ þ sinðρ þ δÞcot ðβ þ ϕÞ
ð22Þ
where αi, αf respectively denote the start and final angles for dump^ C^ and V^ in Eq. (16) depends on ing. Note that each of the terms M, the bucket angle α as well as the joint angle vector q.
Nq ¼
cosα þ sinαcot ðβ þ ϕÞ cosðρ þ δÞ þ sinðρ þ δÞcot ðβ þ ϕÞ
ð23Þ
_ αÞ ¼ C^ ðq; q; V^ ðq; α Þ ¼
_ Þ at α ðqÞ ¼ α i _ Þ þ C k ðq; q C ðq; q _Þ C ðq; q at α ðqÞ ¼ α f
V ðqÞ þ V k ðqÞ V ðqÞ
at α ðqÞ ¼ α i ; at α ðqÞ ¼ α f
ð18Þ
3.4. Returning dynamics If during the returning task no external forces other than gravity are exerted on the excavator, then the dynamic equations in this case are given by Eq. (4). 3.5. Soil modeling and identification In general, calculating soil digging forces is complicated by the difficulty in constructing accurate models that capture the static and dynamic properties of the soil. The choice of soil–tool interaction model crucially affects the accuracy and performance of digging simulation. The soil–tool interaction process is summarized in Fig. 6. Recent contributions to soil–tool interaction modeling include the fundamental earth-moving equation (FEE) [6], learning based models [10], spring-damper models [11], and energy-dissipation models [12]. For our purposes we choose a modified version of the FEE model
where Γ = (d,w,α,ρ,Vs) is a set of additional geometric parameters which correspond to the depth of the bucket tip, the width of the bucket, the terrain slope, rake angle, and swept volume. Γ can be calculated once bucket pose, bucket shape and terrain shape are known. The derivation of fs is given in [9]. We note that this model describes the static force-displacement relation between the soil and tool; while for our purposes this model is sufficient for the most part, for fast, highly dynamic movements of the bucket the accuracy of the dynamic simulation can be expected to degrade.
Table 4 Specifications for the hydraulic excavator used in experiments. Item
Value
Operating weight Boom length Arm length Bucket capacity
29,300 kg 6.25 m 3.05 m 1.27 m3
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
573
Fig. 9. Displacement and pressure sensors installed on the hydraulic excavator.
We now address the estimation of the soil model parameters. In this study we formulate this as an optimization problem, in which the following objective function is minimized: t
soil parameters ¼ arg min ∫tfi jΔF ðt Þj dt; δ;Φ;β;γ;c
ð24Þ
where ti is the time at which digging starts, tf is the time at which digging ends, and ΔF(t) is the difference between the measured force and the force calculated from the soil model. The optimization variables t are the soil model parameters, and the objective function ∫t fi jΔF ðt Þj dt is taken to be the integral of the absolute value of the force difference (which, as shown in Fig. 8, corresponds to the area between the two indicated curves). 4. Minimum torque/time optimization 4.1. Formulation The generation of minimum torque/time trajectories for excavators can be approached in a number of ways. One possible optimal control formulation involves augmenting the minimum torque functional with a penalty term for the final time tf, i.e., 1 t 2 min ϕ t f þ ∫0f jjτðt Þjj dt 2 τ ðt Þ
qðt; P Þ ¼
Bi ðt Þpi ;
ð25Þ
ð26Þ
i¼1
where Bi(t) is a B-spline basis function and P = [p1,p2, ⋯,pm] is the set of control points. The cost functional then reduces to a parameter optimization problem of the form 1 t 2 min ϕ t f þ ∫0f jjτðP; t Þjj dt P 2
• Equation number: governing dynamics equation number • Pmax: maximum pressure
Qmax: maximum flow rate _ max : maximum power W Vsmin: minimum amount of soil volume N.P. (No Penetration): whether penetration between ground and bucket is allowed (×) or not (○)
subject to the same constraints as described in Table 1. The basis functions must be chosen so as to satisfy all the terminal boundary conditions. For the minimum torque problem in which the final time tf is given, it is intuitively clear that for larger values of tf, the minimum torque profiles become increasingly flatter. If tf is allowed to vary, the first term ϕ(tf) acts as a penalty function to prevent the final time from becoming arbitrarily large in order to decrease torque consumption. The functional ϕ(tf) should be a monotonically increasing function of tf, and should be appropriately weighted for balancing between final time and torque consumption. Penalty methods are highly sensitive to weighting parameters, and in this paper we adopt an approach initially developed for open chain arms initially described in [14]. Specifically, we minimize the peak torque max τ P ; t ∞
ð27Þ
ð28Þ
tf
t
2
P ¼ arg min ∫0f jjτðP; t Þjj dt P
subject to boundary conditions and various physical constraints as summarized in Table 1. In [13] a local solution to the optimal control problem is found by assuming that the joint trajectories q(t) are parametrized in terms of B-splines: m X
• • • •
ð29Þ
where ||⋅ ||∞ denotes the L∞ norm, and subject to the same constraints given in Table 1. This formulation is based on the observation that the peak torque attained by the minimum torque trajectory decreases monotonically as tf is increased. A specific algorithm for solving this problem is described in Section 4.2. 4.2. Algorithms Numerical procedures for solving the minimum torque problem and minimum time problem are described in Tables 2 and 3. Here M denotes the measurement data containing the cylinder trajectories and corresponding cylinder forces for several digging motion trials, Φ is the motion script which contains the initial and final configurations of digging, lifting, dumping and returning motions specified by the user, and tf is the duration time for each motion. For the minimum time problem described in Table 3, the motion optimization procedure is as follows 1. Optimize the minimum torque for a fixed tf and check all constraints. 2. Using an appropriate line search algorithm, decrease the final time tf if all constraints are satisfied, otherwise increase tf.
574
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
3. Repeat the routine from step 1 to step 2 until some termination condition is satisfied. The final time tf can be determined via a standard line search procedure, e.g., golden section search, with an upper bound on admissible values of tf. To handle the constraints specified in Table 1, it is desirable to enforce hard constraints as much as possible, and to convert some constraints into soft constraints as required. 5. Experimental results In this section we present experimental results verifying the fidelity of our dynamics simulations, and a comparison of the trajectories
force (KGF)
10
produced by our planner with those produced by an experienced human operator. Physical specifications for the excavator used in the experiments are listed in Table 4. Displacement sensors and pressure sensors attached each of the cylinders at the boom, arm, and bucket measure the corresponding cylinder displacement and force (Fig. 9). 5.1. Verification of excavator dynamics simulation We first compare the cylinder force trajectories produced by our dynamic simulation with actual measurements. The cylinder force trajectories are measured for the two cases: (1) without soil–tool interaction, and (2) with soil–tool interaction. Fig. 10 compares the simulated
Boom cylinder force
x 104
measure simulation
5
0
−5 0
10
20
30
40
50
60
70
80
time (s)
force (KGF)
1
Arm cylinder force
x 104
measure simulation
0.5 0 −0.5 −1 −1.5 −2 0
10
20
30
40
50
60
70
80
time (s)
force (KGF)
1
Bucket cylinder force
x 104
measure simulation
0.5 0 −0.5 −1 0
10
20
30
40
50
60
70
80
time (s)
torque (KGF−m)
10
Swing torque
x 104
measure simulation
5
0
−5
0
10
20
30
40
50
60
70
time (s) Fig. 10. A comparison of simulated and measured cylinder forces without soil–tool interaction.
80
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
force (KGF)
10
575
Boom cylinder force
x 104
measure simulation
5
0
−5
0
20
40
60
80
100
120
140
time (s)
force (KGF)
6
Arm cylinder force
x 104
measure simulation
4 2 0 −2 −4
0
20
40
60
80
100
120
140
time (s)
force (KGF)
4
Bucket cylinder force
x 104
measure simulation
3 2 1 0 −1
0
20
40
60
80
100
120
140
time (s)
torque (KGF−m)
10
Swing torque
x 104
measure simulation
5
0
−5
0
20
40
60
80
100
120
140
time (s) Fig. 11. A comparison of simulated and measured cylinder forces with soil–tool interaction.
and measured forces for repeated trials of a circular reference bucket motion in three dimensions, performed by a human operator. The simulated forces show good agreement with the measurements; the offsets in arm and bucket forces can be traced to small discrepancies in the model parameters (i.e., masses and inertias of the links), as well as unmodelled dynamic effects such as friction, bending, and hysteresis. For the case with soil–tool interactions, we first estimate the five parameters in the modified FEE soil model using the procedure described earlier. Measurement data has been obtained for eight excavation cycles, in which each cycle consists of digging–lifting–dumping–returning. We use the first digging cycle to estimate the soil parameters, and then compare the predicted and measured cylinder forces over the remaining seven cycles; the results are shown in Figs. 11 and 12.
5.2. Optimized motion versus operator motion We now generate optimal motions using the models and algorithms described earlier, and compare them to the motions generated by an experienced human operator. Minimum torque/time motions are obtained for initial and final times that correspond to those of the human operator motion. Table 5 compares the task execution times and average torque sums of the human operator motion, the minimum-time motion (using a minimum-time motion planner), and the minimum torque/time motions generated by our planner (Figs. 15 and 16). The results suggest that the operator uses excessive torques compared to our planner's motions. Note that the minimum-time motion only results in a small improvement in task execution time
576
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
10
Boom cylinder force
x 104
5 measure simulation
x 104
Arm cylinder force measure simulation
4 3
force (KGF)
force (KGF)
5
0
2 1 0 −1 −2
−5 0
5
10
−3 0
15
5
time (s)
3
Bucket cylinder force
x 104
4 measure simulation
2.5
torque (KGF−m)
force (KGF)
15
Swing torque
x 104
measure simulation
3
2 1.5 1 0.5 0
2 1 0 −1 −2
−0.5 −1
10
time (s)
0
5
10
15
time (s)
−3 0
5
10
15
time (s)
Fig. 12. Cylinder forces over a single cycle. Dotted lines separate digging, lifting, dumping, and returning periods from left to right.
compared to our planner's motion, while requiring larger torques. The resulting force profiles are shown in Figs. 13 and 14.
6. Conclusions As a prelude to the realization of fully unmanned excavators, this study has attempted to determine, based on dynamic models of the excavator as well as soil–tool interaction models, excavator motions that are fast, power-efficient, and smooth. The main technical contribution of our paper is a set of analytic gradient-based motion optimization algorithms, specialized to earthmoving excavators, for generating the fastest possible minimum torque motions —that is, without violating any actuator saturation limits, or other constraints on cylinder velocities or accelerations; we call such motions minimum torque/time motions— taking into account the closed-loop structure of excavators, the soil dynamics during digging and dumping, and any collision avoidance constraints that arise during typical excavation tasks. Such motions avoid the undesirable bang-bang characteristics of typical minimum-time trajectories, while being fast, smooth, and energy-efficient. The optimal motions generated by the planner are also compared with the actual excavation trajectories of experienced human operators.
Table 5 Comparison of motions. Criteria
Measurement
Min time
Min torque
tf Average ||τ(t)||2
14.9 1.0145
12.6 0.61483
14.9 0.61128
One of the interesting findings of our experiments is that there is notable difference between the generated optimal motion and actual excavation trajectories performed by a human operator, particularly during the arm return motion. We suspect that these differences can be mitigated somewhat by imposing more physically realistic constraints on the hydraulics, and taking into account collision avoidance between the excavator and truck. At a higher level, this paper has also presented an offline simulation and planning system for unmanned excavators, that integrates an excavator dynamic simulation engine with our optimal motion planner. One of the key elements in both the planning and simulation phases is the use of a soil model. In this paper we have described a nonlinear least-squares soil model parameter identification procedure that relies on measurements of digging forces in actual excavation tasks. The presented work can be extended in a number of ways. Taking into account the dynamics of the hydraulic actuators is clearly useful, in particular considerations on, e.g., hydraulic actuator limits and heat loss considerations. Although [4] presents one realization of such a model, accurate hydraulic actuator dynamic modeling is a highly complicated task, and it may be profitable to examine hierarchical models that capture features of the dynamics at different resolution scales. Improved soil models, as well as excavation tasks in more extreme unstructured environments, are also being examined in the current context, as well as taking into account ground collision avoidance constraints that arise during typical excavation tasks. There are also several means of bootstrapping the above procedure into a class of methods for generating real-time suboptimal trajectories; one particular method is described in [14] that involves principal component analysis of a dataset of optimally generated motions.
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
Boom cylinder trajectory
Arm cylinder trajectory
3300
3200 measure min time min torque
3200 3100
measure min time min torque
3100
displacement (mm)
displacement (mm)
577
3000 2900 2800 2700 2600 2500
3000 2900 2800 2700 2600 2500 2400
2400 0
5
10
2300 0
15
5
10
time (s) Bucket cylinder trajectory
Swing trajectory
3000
80 measure min time min torque
2800
displacement (degree)
displacement (mm)
15
time (s)
2600 2400 2200 2000
measure min time min torque
70 60 50 40 30 20 10
1800 0
5
10
0 0
15
5
10
time (s)
15
time (s) Fig. 13. Cylinder trajectories.
1.5
x 105
Boom cylinder force 8 measure min time min torque
1
Arm cylinder force
x 104
measure min time min torque
6
force (KGF)
force (KGF)
4 0.5 0
−0.5
2 0 −2 −4
−1
−1.5
6
−6 0
x 104
5
time (s)
10
Bucket cylinder force 1
0
torque (KGF−m)
2 0 −2
time (s)
10
15
10
15
Swing torque
x 104
0.5
0
measure min time min torque
−0.5
−4 −6
5
measure min time min torque
4
force (KGF)
−8
15
0
5
10
15
−1
time (s)
0
5
time (s) Fig. 14. Cylinder forces.
578
Y.B. Kim et al. / Automation in Construction 35 (2013) 568–578
Fig. 15. Minimum time solution. Red curve shows the bucket-tip path of the optimal motion; the blue curve shows the human operator's motion.
Fig. 16. Minimum torque solution. Red curve shows the bucket-tip path of the optimal motion; the blue curve shows the human operator's motion.
Acknowledgment This research was supported by a research grant from Hyundai Heavy Industries, with further partial support from the AIM Center, ROSAECERC, SNU-IAMD, and the SNU-BK21 Program in Mechanical Engineering. References [1] A.T. Stentz, J. Bares, S. Singh, P. Rowe, A robotic excavator for autonomous truck loading, Autonomous Robots 7 (1999) 175–186. [2] H. Yamamoto, M. Moteki, H. Shao, T. Ootuki, H. Kanazawa, Y. Tanaka, Basic technology toward autonomous hydraulic excavator, International Symposium on Automation and Robotics in Construction, 2009, pp. 288–295. [3] S.H. Lee, J. Kim, F.C. Park, M. Kim, J.E. Bobrow, Newton-type algorithms for dynamics-based robot movement optimization, IEEE Transactions on Robotics 21 (2005) 657–667. [4] S. Yoo, C.G. Park, B. Lim, K.I. Lee, F.C. Park, Bandwidth maximizing design for hydraulically actuated excavators, Journal of Vibration and Control 16 (2010) 2109–2130. [5] F.C. Park, J.E. Bobrow, S.R. Ploen, A lie group formulation of robot dynamics, International Journal Of Robotics Research 14 (1995) 609–618.
[6] A.R. Recee, The fundamental equation of earth-moving mechanics, Proceedings of the Institution of Mechanical Engineers, 179, 1964, pp. 16–22. [7] O. Luengo, S. Singh, H. Cannon, Modeling and identification of soil-tool interaction in automated excavation, IEEE/RSJ International Conference on Intelligent Robotic Systems, 1998, pp. 1900–1906. [8] F.C. Park, J. Choi, S. Ploen, Symbolic formulation of closed chain dynamics in independent coordinates, Mechanism and Machine Theory 34 (1999) 731–751. [9] H. Cannon, S. Singh, Models for automated earthmoving, Experimental Robotics VI, Lecture Notes in Control and Information Sciences, Springer Verlag, 1999, pp. 183–192. [10] S. Singh, Learning to predict resistive forces during robotic excavation, International Conference on Robotics and Automation, 1995, pp. 2102–2107. [11] S.P. DiMaio, S.E. Salcudean, C. Reboulet, A virtual environment for the simulation and programming of excavation trajectories, presence: teleoper, Virtual Environment 10 (2001) 465–476. [12] S.M. Vahed, X. Song, J.S. Dai, H.K. Lam, L.D. Seneviratne, K. Althoefer, Soil estimation based on dissipation energy during autonomous excavation, The International Federation of Automatic Control, 2008, pp. 13821–13826. [13] J.E. Bobrow, B. Martin, G. Sohl, E.C. Wang, J. Kim, F.C. Park, Optimal robot motions for physical criteria, Journal of Robotic Systems 18 (2001) 785–795. [14] S. Kim, F.C. Park, Fast robot motion generation using principal components: framework and algorithms, IEEE Transactions on Industrial Electronics 55 (2008) 2506–2516.