Industrial excavators represent a vital component of heavy machinery across diverse industries such as construction, agriculture, mining, and forestry. Currently, there is a notable trend towards automating these excavators, driven by advancements in technology and equipment [1], [2]. While autonomous excavators offer several advantages, including extended operation hours and increased efficiency, a significant benefit is their potential to replace skilled human operators, whose training and employment can be costly and whose safety may be at risk for some tasks and sites.
For this automation of industrial excavators, the development in control and planning is necessary. One of the key issues of industrial excavator control is the adoption of hydraulic actuation (e.g., main control valve (MCV)), which consists of multiple pumps, hydraulic components (e.g., check valves), and their interconnection circuit with such complex phenomena as saturation, hysteresis, dead zone, etc. Due to this, typical commercial industrial excavators come only with a low-level digital kinematic controller, while a transparent continuous-time torque-level controller is not feasible. Recently, some schemes have been proposed to improve the accuracy of the velocity-level control for these MCV-actuated excavators via a model-based approach [3] or data-driven approach [4]. In this letter, we assume this low-level digital kinematic control and derive our trajectory planning and modulation framework explicitly based on that. On the other hand, for the excavation planning of autonomous excavators, results have been reported employing optimization, model predictive control (MPC), and data-based method (e.g., imitation learning and reinforcement learning (RL)) [5], [6], [7], [8]. However, these studies are focused on the excavation on a level surface and the risks caused by the excavation force are not considered.
In this letter, we focus on excavation trajectory planning on a slope, a more challenging task due to the increased risk of accidents such as slip and rollover. These accidents can have severe consequences due to the size and weight of industrial excavators. For this, we adopt to utilize the slope tangential/normal force ratio $\mu$ and zero moment point (ZMP) $\xi$ [9] to address slip and rollover prevention respectively. The ZMP and the required friction force measures have been utilized to prevent slip and rollover in motion planning and control of robots, in particular humanoids and legged robots [10], [11], [12], [13], [14], as well as for heavy machinery [15], [16]. However, they either assume that the interaction of the system is only through the ground-support [15], [16] or only from the locomotion [10], [11], [12], thus, the soil-excavation (or manipulation) force is negligible; or, when such excavation/manipulation force is substantial, assume that the system can be controlled at the dynamics-level with continuous-time torque control so that the excavation/manipulation force can be real-time reacted with no need of its prediction [13], [14].
More specifically, in this letter, we propose a novel real-time excavation trajectory modulation framework leveraging $\mu$ and $\xi$ to prevent slips and rollovers, which modulates the nominal excavation trajectory of [5] at the acceleration level (realized with the low-level digital kinematic control), that is, through the additional modulation term $u$ to the shaping force term [17], [18] of the dynamic movement primitive (DMP) method of [5]. Notably, we found that the expressions of both $\mu$ and $\xi$ share a fractional structure with the common denominator which can be assumed positive for the excavator on a slope. Using this acceleration level modulation and the shared fractional structure, the slip and rollover prevention conditions can be formulated as a single inequality which is linear in the modulation $u$ and the excavation force $f_{b}$.
This process of trajectory modulation with $u$ is then implemented in the structure of the low-level digital kinematic control. This then induces a peculiar causality-like issue: we need to solve for the modulation term $u_{k+1}$ at the current timestamp $t_{k}$ to ensure the slip and rollover preventing linear inequality condition for $\mu$ and $\xi$ for the next timestamp $t_{k+1}$ while optimizing a certain quadratic objective. This inequality contains not only the configuration and the velocity of the excavator (i.e., $q_{b_{k+1}}, \dot{q}_{b_{k+1}}$), but also the excavation force $f_{b_{k+1}}$ as well, all at the next timestamp $t_{k+1}$. With the good-enough low-level kinematic (i.e., velocity) control (e.g. [3], [4]), we can faithfully anticipate $q_{b_{k+1}},\dot{q}_{b_{k+1}}$. On the other hand, to estimate $f_{b_{k+1}}$, we devise a novel data-driven prediction model for the excavation force difference (i.e., for $\Delta f_{b_{k}}:=f_{b_{k+1}} - f_{b_{k}}$), since $f_{b_{k}}$ can be measured at the current timestamp $t_{k}$ and existing soil dynamics models are unsuitable for real-time application due to soil-parameter estimation and heavy computation [19], [20], [21]. In particular, we adopt the attention-based Transformer architecture [22], which is known to be effective in treating sequential data.
Our data-driven excavation force difference prediction model is of course not perfect and there in general still remains error $\Delta _{e}$. To address this, we utilize the technique of robust optimizaton (RO) [23] with the box uncertainty, which bounds $\Delta _{e}$ with some margin, thereby, guaranteeing safety against the slip and rollover on a slope. Combining all of these, we can then formulate our real-time excavation trajectory modulation framework as a quadratic programming (QP), which can be solved very efficiently. We also perform experimental validation and demonstration of our proposed framework with a scaled-down excavator, which is a customized version of a commercial radio-controlled hydraulic excavator.
A. Related Works
Various studies have addressed the problem of excavation planning along with the excavation force. In [5], the expert operation is modeled via DMP, and an empirical force threshold is adopted to modulate the excavation trajectory to avoid excessive excavation force. An RL framework is then utilized to learn the excavation policy. In [6], the excavation force in various soils is considered in agent training by implementing an analytical-based soil model through simulation. In [7], the fragmented rock excavation is modeled via recurrent neural network (RNN) whose state includes excavation force, and MPC is utilized to track reference trajectory. MPC is also utilized in [8] to plan local excavation trajectories with physical constraints. Along with model uncertainty and friction, the excavation force is modeled as a disturbance, which is assumed to be constant over the time horizon in MPC. These studies [5], [6], [7], [8] have focused on achieving excavation planning considering the excavation force. However, the risk of slip and rollover is not fully addressed for autonomous excavators.
In robotics, many studies have been utilizing the friction force and ZMP to address the problem of ensuring stability. In [10], ZMP is used to ensure stability in robots making multiple, non-coplanar contacts with their environment by generalizing support areas. ZMP velocity is used as a control input within a dynamically extended Linear Inverted Pendulum (LIP) model, which serves as the predictive model for real-time gait generation of humanoids in [11]. In [12], dynamic stability during locomotion is ensured by incorporating linearized ZMP balance criterion constraint into the optimization process. For the pushing manipulation task, ZMP is adopted as a balancing index for a humanoid with contact posture planning in [13]. The gravito-inertial wrench cone (GIWC), generalized ZMP support area, is leveraged to enable humanoid robots to maintain stability during high-force interaction tasks in [14]. In heavy machinery, ZMP constraint is applied in minimal time optimization for timber manipulation in [15]. For excavators, the fall prediction system is developed by introducing ZMP-based stability counter in [16]. In [10], [11], [12], [15], [16], the interaction is only from the ground-support or locomotion, thus, soil-excavation or manipulation force is assumed negligible. The case of significant manipulation force is considered in [13], [14], yet, they assume the robots can be controlled at the dynamics-level with continuous-time torque actuation, thus, not applicable to typical industrial hydraulic excavator with low-level digital kinematic control.
B. Contribution
The contribution of this work is real-time excavation trajectory modulation that can prevent slip and rollover on a slope for autonomous excavators with low-level digital kinematic control. The modulation is formulated as an efficient QP form with the slip and rollover prevention conditions written as one single linear inequality. A novel data-driven excavation force difference prediction model is proposed to address the aforementioned peculiar causality-like issue arising when the QP modulation is implemented with the low-level digital kinematic control. Experiments using the customized scaled-down excavator validate that the proposed real-time trajectory modulation framework can achieve the prevention of slip and rollover which is more dangerous for excavation on a slope. To our knowledge, our results here are the first to address real-time slip and rollover prevention during excavation on a slope for autonomous excavators with low-level digital kinematic control.
C. Organization
The rest of the letter is organized as follows. Some preliminary including DMP-based nominal trajectory, $\mu$, and $\xi$ are introduced in Section II. Our real-time trajectory modulation framework is proposed in Section III. The experimental results are presented in Section IV, and then Section V concludes the letter.
A. Modulation on DMP-Based Nominal Trajectory
In this letter, we consider an excavator on a slope as shown in Fig. 1. Here, we only focus on the sagittal-plane excavation, with a constant slope angle $\psi _{0} \in \mathbb {R}$. Our DMP-based nominal trajectory, as detailed in [5], is defined as a second-order differential equation s.t.,
\begin{equation*}
\ddot{q}_{b} +D \dot{q}_{b} +K (q_{b}-q_{g}) = f_{m}(s) +u \tag{1}
\end{equation*}
View Source
\begin{equation*}
\ddot{q}_{b} +D \dot{q}_{b} +K (q_{b}-q_{g}) = f_{m}(s) +u \tag{1}
\end{equation*}
where $q_{b}=(x_{b}, y_{b}, \phi _{b}) \in \mathbb {R}^{3}$ is the configuration of the bucket end-tip illustrated in Fig. 1, $q_{g} \in \mathbb {R}^{3}$ is the goal position of DMP which is a constant point in the excavator body-frame $\lbrace \mathcal {B}\rbrace$, $D, K \in \mathbb {R}^{3\times 3}$ are the diagonal constant gain matrices, $f_{m}(s) \in \mathbb {R}^{3}$ is the shaping force to emulate the experts' excavation trajectory as detailed in [5] with the clock signal $s$, and $u \in \mathbb {R}^{3}$ is the modulation to be designed in this letter. Here, we assume that this DMP-based nominal trajectory can be tracked well enough by a low-level hydraulic-based joint angle control (e.g., data-driven model inversion control [4]) so that we can real-time modulate the motion of the excavator by injecting $u$ in (1).
Let us also denote the configuration of the $i$-link by $q_{i}=(x_{i}, y_{i}, \phi _{i}) \in \mathbb {R}^{3}, i \in \lbrace 1,2,3\rbrace$, where $(x_{i}, y_{i}) \in \mathbb {R}^{2}$ is the center of mass (CoM) position of each link and $\phi _{i} \in \mathbb {R}$ is the orientation of each link as illustrated in Fig. 1. For the typical operation range of excavation, the inverse kinematics solution is uniquely given, implying that there is an injective mapping between $q_{b} \in \mathbb {R}^{3}$ and each $q_{i} \in \mathbb {R}^{3}$. In other words, given $q_{b}$, we have a well-defined solution $q_{i}(q_{b})$. By differentiating such inverse kinematics solution with (1), we can then obtain the following differential equation:
\begin{equation*}
\ddot{q}_{i} = J_{i}(q_{b}) u +n_{i}(q_{b}, \dot{q}_{b}) \tag{2}
\end{equation*}
View Source
\begin{equation*}
\ddot{q}_{i} = J_{i}(q_{b}) u +n_{i}(q_{b}, \dot{q}_{b}) \tag{2}
\end{equation*}
where $J_{i}(q_{b}) \in \mathbb {R}^{3 \times 3}$ is the Jacobian matrix from $q_{b}$ to $q_{i}$, and $n_{i}(q_{b}, \dot{q}_{b}) \in \mathbb {R}^{3}$ is the residual nonlinear term.
B. Slope Tangential/Normal Force Ratio $\mu$
We introduce the slope tangential/normal force ratio $\mu \in \mathbb {R}$ as the indicator of slip,
\begin{equation*}
\mu := \frac{f_{o_{x}}}{f_{o_{y}}}
\end{equation*}
View Source
\begin{equation*}
\mu := \frac{f_{o_{x}}}{f_{o_{y}}}
\end{equation*}
where $(f_{o_{x}},f_{o_{y}}) \in \mathbb {R}^{2}$ are respectively the slope tangential and normal force at the origin of $\lbrace \mathcal {B}\rbrace$ as shown in Fig. 1. Each of these forces can be calculated by applying D'Almbert principle along the axes of $\lbrace \mathcal {B}\rbrace$ s.t.,
\begin{equation*}
f_{o_{\star }} = { \sum _{i}} m_{i} (\ddot{\star }_{i}-g_{\star }) -f_{b_{\star }}, \,\star \in \lbrace x,y\rbrace \tag{3}
\end{equation*}
View Source
\begin{equation*}
f_{o_{\star }} = { \sum _{i}} m_{i} (\ddot{\star }_{i}-g_{\star }) -f_{b_{\star }}, \,\star \in \lbrace x,y\rbrace \tag{3}
\end{equation*}
where $m_{i}$ is the mass of each link, $(g_{x}, g_{y}) \in \mathbb {R}^{2}$ is the gravitational acceleration expressed in $\lbrace \mathcal {B}\rbrace$ (in this letter $g_{y}< 0$), and $(f_{b_{x}}, f_{b_{y}}) \in \mathbb {R}^{2}$ is the effective excavation force acting on the bucket end-tip. The slope tangential/normal force ratio $\mu$ can then be written as
\begin{align*}
\mu &= \frac{{ \sum _{i}} m_{i} (\ddot{x}_{i}-g_{x}) -f_{b_{x}}}{{ \sum _{i}} m_{i} (\ddot{y}_{i}-g_{y}) -f_{b_{y}}} \text{.} \tag{4}
\end{align*}
View Source
\begin{align*}
\mu &= \frac{{ \sum _{i}} m_{i} (\ddot{x}_{i}-g_{x}) -f_{b_{x}}}{{ \sum _{i}} m_{i} (\ddot{y}_{i}-g_{y}) -f_{b_{y}}} \text{.} \tag{4}
\end{align*}
In this letter, we aim to prevent the slip for the excavation slope operation by enforcing the following conditions:
\begin{align*}
|\mu | < \bar{\mu } < \mu _{s} \tag{5}
\end{align*}
View Source
\begin{align*}
|\mu | < \bar{\mu } < \mu _{s} \tag{5}
\end{align*}
where $\mu _{s} \in \mathbb {R}$ is the static friction coefficient and $\bar{\mu } \in \mathbb {R}$ is the bound for the slope tangential/normal force ratio $\mu$ with some safety margin from $\mu _{s}$.
D. Fractional Structure of $\mu$ and $\xi$
Notice that $\mu$ in (4) and $\xi$ in (6) both have fractional structure with numerator linear in acceleration $\ddot{q}_{i}$ and also share the same denominator ${ \sum _{i}} m_{i} (\ddot{y}_{i} -g_{y}) -f_{b_{y}}$, where $g_{y}< 0$ for the slope angle $\psi _{0} \in [-\frac{\pi }{2}, \frac{\pi }{2}]$. The summation of the link gravitational force ${ \sum _{i}} m_{i} g_{y}$, including the base link (e.g. the low frame and the cabin), is the dominant element of the denominator because the acceleration of the excavator is generally not so high and the excavator is typically not able to (or set to) generate excavation force larger than its weight, i.e. $|{ \sum _{i}} m_{i} \ddot{y}_{i}|+|f_{y}| < |{ \sum _{i}} m_{i} g_{y}|$. Therefore, for this letter, we assume that the common denominator is positive:
\begin{align*}
{ \sum _{i}} m_{i} (\ddot{y}_{i} -g_{y}) -f_{y} >0 \text{.} \tag{8}
\end{align*}
View Source
\begin{align*}
{ \sum _{i}} m_{i} (\ddot{y}_{i} -g_{y}) -f_{y} >0 \text{.} \tag{8}
\end{align*}
SECTION III.
Trajectory Modulation for Slip and Rollover Prevention
A. Linear Inequality Constraints of $\mu$ and $\xi$
For the slip and rollover prevention for the excavator on the slope, we will modulate the action $u$ of the DMP-based excavation trajectory (1) in real-time. For this, utilizing (2), we first write the slope tangential/normal force ratio $\mu$ (4) and the ZMP $\xi$ (6) as functions of $u$ s.t.,
\begin{align*}
\mu = \frac{A_{\mu } u +b_{\mu } -f_{b_{x}}}{C u +d -f_{b_{y}}}, \,\xi = \frac{A_{\xi } u +b_{\xi } +y_{b} f_{b_{x}} -x_{b} f_{b_{y}}}{C u +d -f_{b_{y}}} \tag{9}
\end{align*}
View Source
\begin{align*}
\mu = \frac{A_{\mu } u +b_{\mu } -f_{b_{x}}}{C u +d -f_{b_{y}}}, \,\xi = \frac{A_{\xi } u +b_{\xi } +y_{b} f_{b_{x}} -x_{b} f_{b_{y}}}{C u +d -f_{b_{y}}} \tag{9}
\end{align*}
where $A_{\mu }={ \sum _{i}} m_{i} e_{x} \cdot J_{i} \in \mathbb {R}^{3}$, $b_{\mu }={ \sum _{i}} m_{i} (e_{x} \cdot n_{i} -g_{x}) \in \mathbb {R}$, $A_{\xi }= { \sum _{i}} G_{i} J_{i} \in \mathbb {R}^{3}$, $b_{\xi }={ \sum _{i}} G_{i} n_{i} +{ \sum _{i}} m_{i} (y_{i} g_{x} -x_{i} g_{y}) \in \mathbb {R}$, $C={ \sum _{i}} m_{i} e_{y} \cdot J_{i} \in \mathbb {R}^{3}$, $d={ \sum _{i}} m_{i} (e_{y} \cdot n_{i} -g_{y}) \in \mathbb {R}$, and $e_{\star \in \lbrace x,y,z\rbrace } \in \mathbb {R}^{3}$ is the standard basis for $\mathbb {R}^{3}$.
Notice that the common denominator of $\mu$ and $\xi$, which is assumed to be positive (8) in Section II, is now linear in $u$. To avoid excessive modulation, here, we introduce the physically feasible modulation set $\mathcal {U}$. We also define this $\mathcal {U}$ so that the link acceleration $\ddot{y}_{i}$ is not so high that the condition (8) still holds. In other words, from (8), we can still ensure the denominators of (8) be positive by enforcing $u$ into the set ${\mathcal U}$, i.e.,
\begin{align*}
C u +d -f_{b_{y}} >0, \,\, \forall u \in \mathcal {U} \text{.} \tag{10}
\end{align*}
View Source
\begin{align*}
C u +d -f_{b_{y}} >0, \,\, \forall u \in \mathcal {U} \text{.} \tag{10}
\end{align*}
Also, the numerator and denominator of both $\mu$ and $\xi$ in (10) are linear in $(u, f_{b})$. Then, with their denominator to be always positive as assumed above, we can write the slip and rollover prevention conditions, (5) and (7), together in the following single linear inequality:
\begin{align*}
A(q_{b}) u +b(q_{b}, \dot{q}_{b}) +L(q_{b}) f_{b} \leq 0 \tag{11}
\end{align*}
View Source
\begin{align*}
A(q_{b}) u +b(q_{b}, \dot{q}_{b}) +L(q_{b}) f_{b} \leq 0 \tag{11}
\end{align*}
where $A(q_{b}) \in \mathbb {R}^{4 \times 3}$, $b(q_{b}, \dot{q}_{b}) \in \mathbb {R}^{4}$, and $L(q_{b}) \in \mathbb {R}^{4 \times 2}$.
For the slip and rollover prevention, we need to enforce this linear inequality (11) via $u$. We, however, cannot do this on a continuous-time basis, since, as typical for industrial excavators, here, we adopt the digital low-level hydraulic motion controller (e.g., [4]). More precisely, at the current timestamp $t_{k}$, we need to compute $u_{k+1}$ to satisfy the inequality (11) at the next timestamp $t_{k+1}$. This is then transmitted to the DMP computation (1) to modulate the trajectory, which is then tracked by the low-level hydraulic controller only for the next timestamp $t_{k+1}$. This means that, for the slip and rollover prevention, we need to act at $t_{k}$ to enforce the linear inequality (11) for $t_{k+1}$, where $k=0,1,\cdots$ is the discrete-time index. This linear inequality condition (11) at $t_{k+1}$ can be written by
\begin{align*}
A(q_{b_{k+1}}) u_{k+1} +b(q_{b_{k+1}}, \dot{q}_{b_{k+1}}) +L(q_{b_{k+1}}) f_{b_{k+1}} \leq 0 \text{.} \tag{12}
\end{align*}
View Source
\begin{align*}
A(q_{b_{k+1}}) u_{k+1} +b(q_{b_{k+1}}, \dot{q}_{b_{k+1}}) +L(q_{b_{k+1}}) f_{b_{k+1}} \leq 0 \text{.} \tag{12}
\end{align*}
Here, with the assumption that the excavation motion is not so fast and the low-level motion controller is good enough, we can adequately predict the terms, $q_{b_{k+1}}$ and $\dot{q}_{b_{k+1}}$, at $t_{k}$ via the Explicit Euler integrations:
\begin{align*}
q_{b_{k+1}} = q_{b_{k}} + \dot{q}_{b_{k}} \Delta t_{k}, \,\,\dot{q}_{b_{k+1}} = \dot{q}_{b_{k}} + \ddot{q}_{b_{k}} \Delta t_{k} \tag{13}
\end{align*}
View Source
\begin{align*}
q_{b_{k+1}} = q_{b_{k}} + \dot{q}_{b_{k}} \Delta t_{k}, \,\,\dot{q}_{b_{k+1}} = \dot{q}_{b_{k}} + \ddot{q}_{b_{k}} \Delta t_{k} \tag{13}
\end{align*}
where $\Delta t_{k} = t_{k+1} - t_{k}$. What is not complying with this procedure is the term $f_{b_{k+1}}$ in (12), since we cannot predict $f_{b_{k+1}}$ with the soil dynamics in general too complicated to be tractably modeled. To resolve this issue, we utilize the data-driven method to obtain $f_{b_{k+1}}$ by predicting the force difference $\Delta f_{b_{k}}=f_{b_{k+1}}-f_{b_{k}} \in \mathbb {R}^{2}$.
B. Data-Driven Excavation Force Difference Prediction
To model the excavation force difference $\Delta \hat{f}_{b_{k}}$, we employ the attention-based data-driven method. The structure of our developed data-driven model $\Delta \hat{f}_{b_{k}}$ is illustrated in Fig. 2, which is modified from the attention-based encoder-decoder Transformer architecture [22]. The encoder includes a multi-head self-attention layer and a feed-forward layer, and these two layers have a residual connection followed by layer normalization. The decoder includes one masked multi-head self-attention layer, another multi-head attention layer connected with the encoder output, and a feed-forward layer followed by an output linear layer. Also, these three decoder layers have a residual connection followed by layer normalization. The hyper-parameters are the same for both the encoder sub-layers and the decoder sub-layers: 2 heads in the multi-head attention layers and 64 nodes in the feedforward layers. The inputs and outputs of the encoder and decoder are time sequences of the same time horizon $w=10$ at $ 100Hz$ with no padding. The encoder input $\mathbf {x}^{enc}_{k} \in \mathbb {R}^{9 \times w}$ includes the position $q_{b_{k}}$ and velocity $\dot{q}_{b_{k}}$ of the bucket, the excavation force $f_{b_{k}}$, and the past trajectory area $a_{q_{k}}$, which is the area between the soil surface and the bucket traveled trajectory from the initial $t_{0}$ to $t_{k}$ as illustrated in Fig. 1. The decoder input $\mathbf {x}^{dec}_{k} \in \mathbb {R}^{2 \times w}$ is the excavation force difference right before $t_{k}$ and the decoder output $\mathbf {y}^{dec}_{k} \in \mathbb {R}^{2 \times w}$ is the excavation force difference prediction that last element is $\Delta \hat{f}_{b_{k}}$.
The data $\mathcal {D} = \lbrace (\mathbf {x}^{enc}_{k}, \mathbf {x}^{dec}_{k}, \mathbf {y}^{dec}_{k})\rbrace _{k=1}^{N}$ is represented in the inertial frame $\mathcal {O}$ since we only consider flat soil in this letter and its geometric shape is identical for the inertial frame rather than for the excavator body-frame. The total data (N $\approx 1 \times 10^{6}$) is collected from 470 excavation trajectories on both negative and positive slopes using scaled-down excavator described in next Section IV-A, and split into three subsets in a ratio of 12:3:1: training, validating, and testing. The ground truth for the decoder output $\mathbf {y}^{dec}_{k}$ is generated with experiment data and the data-driven model is trained using the mean square error loss function with convergence curve represented in Fig. 3. The data-driven model $\Delta \hat{f}_{b_{k}}$ is trained through TensorFlow with Adam optimizer. In the training process, the batch size is 256, the initial learning rate is 0.001 and the exponential learning rate scheduler is used. The testing results in Fig. 3 show that this model $\Delta \hat{f}_{b_{k}}$ can predict the excavation force difference.
This force difference prediction $\Delta \hat{f}_{b_{k}}$, of course, can assume some error $\Delta _{e} := \Delta f_{b_{k}} -\Delta \hat{f}_{b_{k}} \in \mathbb {R}^{2}$. However, it turns out that this prediction error $\Delta _{e}$ is rather small as shown in Fig. 3 due to the effectiveness of the Transformer architecture in Fig. 2. Since this error $\Delta _{e}$ is reasonably small, we can address it by the robust optimization framework (detailed discussion in Section III-C below). For that, we can model this error $\Delta _{e}$ by the box uncertainty, that is, we can write
\begin{align*}
f_{b_{k+1}} &= f_{b_{k}} +\Delta \hat{f}_{b_{k}} +\Delta _{e} \tag{14}
\end{align*}
View Source
\begin{align*}
f_{b_{k+1}} &= f_{b_{k}} +\Delta \hat{f}_{b_{k}} +\Delta _{e} \tag{14}
\end{align*}
where $\Delta _{e}$ can be captured by the box uncertainty s.t. $\lbrace \Delta _{e} \in \mathbb {R}^{2}: |\Delta _{e_\star }| \leq \zeta _{\star }, \star \in \lbrace x,y\rbrace \rbrace$ as illustrated in Fig. 3.
C. Real-Time Trajectory Modulation via Robust Optimization
With the predicted excavation force (14), the slip and rollover condition (12) can be rewritten as an uncertain linear inequality constraint:
\begin{align*}
A_{k+1} u_{k+1} + b_{k+1} +L_{k+1} (f_{b_{k}} +\Delta \hat{f}_{b_{k}} +\Delta _{e}) \leq 0 \tag{15}
\end{align*}
View Source
\begin{align*}
A_{k+1} u_{k+1} + b_{k+1} +L_{k+1} (f_{b_{k}} +\Delta \hat{f}_{b_{k}} +\Delta _{e}) \leq 0 \tag{15}
\end{align*}
where $A_{k+1}=A(q_{b_{k+1}}) \in \mathbb {R}^{4 \times 3}$, $b_{k+1}=b(q_{b_{k+1}}, \dot{q}_{b_{k+1}}) \in \mathbb {R}^{4}$, $L_{k+1}=L(q_{b_{k+1}}) \in \mathbb {R}^{4 \times 2}$. As $\Delta _{e}$ is considered as the box uncertainty in Section III-B, we utilize RO technique to address it. We split $L_{k+1}\Delta _{e}$ into $l^{j}_{k+1}\cdot \Delta _{e}$ where $l^{j}_{k+1} \in \mathbb {R}^{2}(j \in {1,2,3,4})$ is the j-th row vector of matrix $L_{k+1}$. Then, $l^{j}_{k+1}\cdot \Delta _{e}$ is always less than its worst-case:
\begin{align*}
l^{j}_{k+1}\cdot \Delta _{e} \leq ||l^{j}_{k+1} \cdot (\zeta _{x}, \zeta _{y}) ||_{1} \text{.}
\end{align*}
View Source
\begin{align*}
l^{j}_{k+1}\cdot \Delta _{e} \leq ||l^{j}_{k+1} \cdot (\zeta _{x}, \zeta _{y}) ||_{1} \text{.}
\end{align*}
Therefore, (15) is rewritten as follows,
\begin{align*}
A_{k+1} u_{k+1} + b_{k+1} +L_{k+1} (f_{b_{k}} +\Delta \hat{f}_{b_{k}}) +N_{k+1} \leq 0
\end{align*}
View Source
\begin{align*}
A_{k+1} u_{k+1} + b_{k+1} +L_{k+1} (f_{b_{k}} +\Delta \hat{f}_{b_{k}}) +N_{k+1} \leq 0
\end{align*}
where $N_{k+1} \in \mathbb {R}^{4}$ is stacked vector of the $l_{1}$-norm $||l^{j}_{k+1} \cdot (\zeta _{x}, \zeta _{y}) ||_{1}$. Finally, the real-time excavation trajectory modulation problem is formulated as the optimization problem as follows,
\begin{align*}
\min _{u_{k+1}} \, & \lambda _{u} || u_{k+1} ||_{2} +\lambda _{h} || u_{k+1} -u_{k} ||_{2} \tag{16}\\
\text{s.t.} \, & \underline{u}_{k+1} \leq u_{k+1} \leq \bar{u}_{k+1} \tag{17}\\
A_{k+1} &u_{k+1} + b_{k+1} +L_{k+1} (f_{b_{k}} +\Delta \hat{f}_{b_{k}}) +N_{k+1} \leq 0 \tag{18}
\end{align*}
View Source
\begin{align*}
\min _{u_{k+1}} \, & \lambda _{u} || u_{k+1} ||_{2} +\lambda _{h} || u_{k+1} -u_{k} ||_{2} \tag{16}\\
\text{s.t.} \, & \underline{u}_{k+1} \leq u_{k+1} \leq \bar{u}_{k+1} \tag{17}\\
A_{k+1} &u_{k+1} + b_{k+1} +L_{k+1} (f_{b_{k}} +\Delta \hat{f}_{b_{k}}) +N_{k+1} \leq 0 \tag{18}
\end{align*}
where $\lambda _{u}, \lambda _{h} \in \mathbb {R}$ are the weight parameters for the optimization cost. The magnitude of the modulation $u_{k+1}$ is minimized and the hierarchical cost $|| u_{k+1} -u_{k} ||_{2}$ is also added to avoid aggressive changes in modulation. The modulation limits $(\underline{u}_{k+1}, \bar{u}_{k+1})$ in (17) are from the low-level hydraulic actuation limits such as joint velocity and joint acceleration, and these limits are functions of $(q_{b_{k+1}},\dot{q}_{b_{k+1}},\ddot{q}_{b_{k+1}})$. The slip and rollover prevention condition (18) is applied as a constraint. The above optimization formulation (16)–(18) can then be transformed into the following quadratic programming:
\begin{align*}
\min _{u_{k+1}} \, & u_{k+1}^{T} \mathbf {P} u_{k+1} + \mathbf {q} u_{k+1} \\
\text{s.t.} \, &\mathbf {G} u_{k+1} \leq \mathbf {h} \tag{19}
\end{align*}
View Source
\begin{align*}
\min _{u_{k+1}} \, & u_{k+1}^{T} \mathbf {P} u_{k+1} + \mathbf {q} u_{k+1} \\
\text{s.t.} \, &\mathbf {G} u_{k+1} \leq \mathbf {h} \tag{19}
\end{align*}
which can be solved in real-time by utilizing open-source solvers (e.g., [24]).
In this section, we describe the experimental setup and present the results of two scenarios for preventing slips and rollovers. Simultaneous exceeding of the bound values for both $\mu$ and $\xi$ is rare, making experimental implementation challenging. Therefore, we considered two separate scenarios: one involving slip and the other involving rollover.
A. Experimental Setup
To verify our proposed framework, we first establish a scaled-down environment where the slips and rollovers of an excavator can be managed by human capability. In Fig. 4, the experimental setup in the laboratory is presented, including the soil container, the slope inclination, and the scaled-down excavator. The soil container is assembled with aluminum profiles and is filled with real soil (e.g., diatomite), and a linear actuator manipulates the slope inclination.
The developed scaled-down excavator, represented in Fig. 4, was built by customizing a radio-controlled hydraulic excavator, BA-B0001 model of LESU, which is for hobbyists and generally actuated by the hydraulic system such as the geared pump motor, the 3-way valve, and actuation cylinders. Various sensors are attached to obtain measurements as shown in Fig. 4. The RLS encoder is attached to each joint to obtain the excavator's state, the PhidgetSpatial inertial measurement unit (IMU) is mounted to measure the orientation of the excavator, and the Robotous force-torque sensor is mounted with a printed bracket to measure the excavation force. The measurement rates are 200–250 Hz for the above sensors. The VL53L4CD range sensor is attached to measure the relative slip displacement of the excavator with the 200 Hz rate, and the Intel Realsense D435 depth camera is mounted on the boom link to obtain point cloud data of the soil with the 30 Hz rate.
We develop the data-driven low-level controller for the scaled-down excavator, which maps the hydraulic valve control input to the joint velocity. The hydraulic valve control is obtained by the inverse of the map with the 300 Hz rate. The proportional-integral joint position feedback is added to the desired joint velocity from the DMP trajectory via inverse kinematics.
All algorithms (including sensor data processing, the low-level control, the excavation force difference prediction $\Delta \hat{f}_{b_{k}}$, and the proposed trajectory modulation framework) are implemented onboard in Python with ROS in Ubuntu 20.04. using Intel NUC Mini PC @3.8GHz, 16GB of RAM. The real-time trajectory modulation is applied at 100 Hz including the prediction $\Delta \hat{f}_{b_{k}}$, whose calculation time is under $10^{-3}$ seconds.
B. Scenario 1: Slip Prevention
At first, the static friction coefficient is calculated by gradually increasing the inclination respectively in positive and negative values until the scaled-down excavator slips. As a result, the scaled-down excavator starts to slip nearly 12.8$^{\circ }$, so in this experiment section, we set $\mu _{s} \approx 0.227$. For the proposed algorithm to prevent slippage, we consider a scenario where the excavator is located on a negative slope $\psi _{0} \in \lbrace -6.0^{\circ }, -7.5^{\circ }\rbrace$ as in Fig. 5, reflecting the friction coefficient $\mu _{s}$ measured above. In each negative slope scenario, we apply two bound values ($\bar{\mu } \in \lbrace 0.18, 0.16\rbrace$) that consider a large and small safety margin from the physical limit $\mu _{s}$. The results of the slip prevention scenario experiments for two negative slopes are presented in Fig. 6, and they show the following common results. When there is no trajectory modulation, $\mu$ exceeds $\mu _{s}$ at around 10 seconds and the excavator starts to slip resulting in a final slip displacement of about $0.147[m]$. Two bound values ($\bar{\mu } \in \lbrace 0.16, 0.18\rbrace$) are applied and the trajectory is successfully regulated so that the magnitude of $\mu$ is less than the static friction coefficient. The smaller $\bar{\mu }$ results in a longer modulation duration and a shallower excavation trajectory. In addition, the results of 10 iteration experiments on both negative slope scenarios are summarized in Fig. 8 specifically representing summary statistics of the slip displacement $d_{x}$. The slip displacement is average $\text{0.127 [m]}$ for $\psi _{0}=-6^{\circ }$ and average $\text{0.133 [m]}$ for $\psi _{0}=-7.5^{\circ }$, and nearly zero displacement through our proposed framework for both $\bar{\mu } \in \lbrace 0.18, 0.16\rbrace$, indicating that the slip can be prevented with a high level of confidence.
C. Scenario 2: Rollover Prevention
For the proposed algorithm to prevent rollover, we consider a scenario where the excavator is located on a positive slope $\psi _{0} \in \lbrace 30^{\circ }, 35^{\circ }\rbrace$ as in Fig. 5. We reduce the size of the stable region to $\xi _{s} \approx \text{0.147 [m]}$ by rotating the swing angle $\frac{\pi }{2}$ to be vulnerable to rollover. In each positive slope scenario, we apply two bound values ($\bar{\xi } \in \lbrace \text{0.12, 0.1}\rbrace$) that consider a large and small safety margin from the physical limit $\xi _{s}$. The results of the rollover prevention scenario experiments are presented in Fig. 7, and they show the following common results. When there is no trajectory modulation, $\xi$ starts to exceed $\xi _{s}$ at around $2-5$ seconds as the excavation force increases and the bucket tip goes deeper resulting in the large pitch $\psi$, which indicates rollover. Two bound values ($\bar{\xi } \in \lbrace 0.12, 0.1\rbrace [m]$) are applied and the trajectory is successfully regulated so that $\xi$ is within the stable region in the case of all ZMP boundary values. The smaller $\bar{\xi }$ results in a longer modulation duration, causing a shallower excavation trajectory in both $x,y$ direction and a smaller excavation force. In addition, the results of 10 iteration experiments on both positive slope scenarios are summarized in Fig. 8 specifically representing summary statistics of maximum pitch angle $\psi$ to represent rollover. The maximum pitch angle is obtained with respect to the slope angle $\psi _{0}$ for comparison and is average $\text{2.37}e^{-2}\text{[rad]}$ for $\psi _{0}=35^{\circ }$ and average $\text{1.64}e^{-2}\text{[rad]}$ for $\psi _{0}=30^{\circ }$. In both positive slope scenarios, the maximum pitch angle decreases to smaller through our proposed framework for both $\bar{\xi } \in \lbrace 0.12, 0.1\rbrace$, indicating that the rollover can be prevented with a high level of confidence.
Contents 
![Fig. 3. - Training and prediction results of $\Delta \hat{f}_{b_{k}}$: Loss function convergence curve of the training process (left), prediction result of single test trajectory (center), and prediction error distribution (right). The solid line represents the box uncertainty boundary $(\zeta _{x}, \zeta _{y})=0.05[N]$, and the dashed lines represent each confidence region of three-sigma and five-sigma.](/mediastore/IEEE/content/media/7083369/10893718/10878475/lee3-3540389-large.gif)
