2.1. System model and ZMP constraint

We first derive the full kinematic model of a mobile manipulator (a.k.a., system), an example of which is shown schematically in Fig. 2 (left). The manipulator arm of the robot has $n \in \mathbb{Z}^{0+}$ joint DoFs, and the mobile base is modeled as a unicycle on arbitrary, but known terrain. A unicycle model is commonly used to represent the kinematics of differential drive robots and skid-steered machines [Reference Siciliano, Sciavicco, Villani and Oriolo40], which is the case of the feller buncher.

Figure 2. On the left, schematic diagram of a tracked mobile manipulator with 5 manipulator DoFs; the three axes of each link frame $\textbf{x}_i$ , $\textbf{y}_i$ , and $\textbf{z}_i$ are represented by red, green, and blue arrows, respectively. On the right, rectangular support polygon $Conv(S)$ .

Frame $\mathcal{O}$ is the inertial frame, and frame $\mathcal{F}_i$ is fixed to the $i$ -th link of the robot for $i\in \{0,1,\ldots,n\}$ . Here, link $0$ refers to the robot’s mobile base, links $1$ to $n-1$ refer to the manipulator arm components, and link $n$ refers to the robot’s end effector. We denote the angle of joint $i$ between link $i-1$ and $i$ with $q_i\in \mathbb{R}$ ( $i = 1,2,\ldots,n$ ) and collect all joint angles into a column vector $\textbf{q}\in \mathbb{R}^n$ . We consider the base-fixed frame $\mathcal{F}_0$ to undergo translation, as measured with $\textbf{p}_b^{\mathcal{O}}$ , and a $\textbf{z}$ - $\textbf{x}$ - $\textbf{y}$ rotation from initial attitude through angles $\psi$ (yaw), $\theta$ (pitch), and $\phi$ (roll), respectively; this allows the base to be located on arbitrary-sloped terrain. The generalized coordinates of the base and the manipulator are assembled in $\bar{\textbf{q}} = \left[\textbf{p}_b^{\mathcal{O}^T}, \psi, \theta, \phi, \textbf{q}^T\right]^T$ . Then, defining $\textbf{x} = \left[\bar{\textbf{q}}^T, \dot{\bar{\textbf{q}}}^T\right]^T$ , the mobile manipulator’s kinematics equations can be written in state space form as:

(1) \begin{equation} \dot{\textbf{x}} = g(\textbf{x},\textbf{u}). \end{equation}

where we introduced the kinematic control input $\textbf{u}=\left[u_a,u_\psi,\textbf{u}_q^T\right]^T$ , with $u_a$ , $u_\psi$ , and $\textbf{u}_q$ as accelerations along heading direction, yaw angular acceleration, and manipulator arm joint accelerations, respectively. Eq. (1) serves as the model of the robot for the overall optimal trajectory planning problem. Note that the terrain information is embedded in (1) through the orientation of the base.

In addition to the kinematics Eq. (1), a geometric construction called the support polygon will be used throughout this paper. The support polygon, denoted by $Conv(S)$ , is a convex hull formed by the contact points between the robot’s mobile base and the ground (see Fig. 2, right.)

The ZMP measure, central to this paper, allows to quantify the rollover stability margin of a mobile manipulator on arbitrary terrain using only its kinematic model and inertia parameters, instead of full dynamics model or force measurements. It is therefore our method of choice due to the potential benefit of faster trajectory planning calculations and its practicality. To compute the ZMP location, we define the Cartesian coordinates of each component’s center of mass relative to $\textbf{p}_b$ expressed in $\mathcal{F}_0$ as $\textbf{p}_i^{\mathcal{F}_0} = [x_i,y_i,z_i]^T$ and the corresponding accelerations $\ddot{\textbf{p}}_i^{\mathcal{F}_0} = [\ddot x_i,\ddot y_i,\ddot z_i]^T$ . By employing kinematics, these can be obtained from:

(2) \begin{equation} \begin{aligned} \textbf{p}_i^{\mathcal{F}_0}& = \textbf{f}_i(\bar{\textbf{q}}) \\[5pt] \ddot{\textbf{p}}_i^{\mathcal{F}_0} &= \dot{\textbf{J}}_{pi}(\bar{\textbf{q}})\dot{\bar{\textbf{q}}}+\textbf{J}_{pi}(\bar{\textbf{q}})\ddot{\bar{\textbf{q}}}, \end{aligned} \end{equation}

where we introduced the position kinematics Jacobian, $\textbf{J}_{pi}(\bar{\textbf{q}})=\dfrac{\partial{\textbf{f}_i}}{\partial{\bar{\textbf{q}}}}$ . Then, according to ref. [Reference Sugano, Huang and Kato15] and using the gravitational vector $\textbf{g}^{\mathcal{F}_0} = [g_x,g_y,g_z]^T$ , the coordinates of the ZMP in the base frame $\textbf{p}_{\text{zmp}}^{\mathcal{F}_0} = [x_{\text{zmp}}, y_{\text{zmp}}, 0]^T$ are given byFootnote ¹:

(3) \begin{equation} \begin{aligned} x_{\text{zmp}} &= \dfrac{\sum _{i=0}^n m_i(\ddot{z}_i-g_z)x_i-\sum _{i=0}^n m_i(\ddot{x}_i-g_x) z_i}{\sum _{i=0}^n m_i(\ddot{z}_i-g_z)}\\[5pt] y_{\text{zmp}} &= \dfrac{\sum _{i=0}^n m_i(\ddot{z}_i-g_z)y_i-\sum _{i=0}^n m_i(\ddot{y}_i-g_y) z_i}{\sum _{i=0}^n m_i(\ddot{z}_i-g_z)}. \end{aligned} \end{equation}

Note, by definition, ZMP lies in the plane of the support polygon so that its z-coordinate in $\mathcal{F}_0$ is zero. Also, the mass of the manipulated object, assumed to be known in this work, can be easily accounted for in (3). The robot is dynamically (ZMP-) stable when the constraint $\textbf{p}_{\text{zmp}}\in Conv(S)$ is satisfied; it has a tendency to rollover otherwise. Equation (3) can be evaluated by substituting from the kinematics equations (2), once the base and the manipulator motions have been planned, to produce the ZMP locus, that is, the trajectory of ZMP in $\mathcal{F}_0$ during the motion of the robot. The ZMP stability constraint is a major contributor to the high computational cost when enforced in optimal control formulations as it is nonlinear and non-convex with respect to the system’s states.

2.2. Optimal control problem formulation

We consider this problem in full generality by allowing the robot to be situated on slopes of different grades. With the view to optimizing the efficiency of manipulation tasks, the motions should be carried out within the shortest possible time, without the robot rolling over. Naturally, to minimize the overall time required for the robot to reach final state $\textbf{x}_f$ from an initial state $\textbf{x}_0$ safely, we formulate a constrained NOCP of the form:

(4) \begin{equation} \begin{aligned} \min _{\textbf{u}} \;\;&\int _{t_0}^{t_f} 1\; \text{d} t.\\[5pt] \text{s.t.}\;\;& \dot{\textbf{x}}=g(\textbf{x},\textbf{u})\;\;\textbf{x}(t_0) = \textbf{x}_0\;\;\textbf{x}(t_f) = \textbf{x}_f\\[5pt] &\textbf{p}_{\text{zmp}}(\textbf{x},\textbf{u})\in Conv(S)\\[5pt] &\underline{\textbf{x}} \leq \textbf{x}\leq \overline{\textbf{x}}\\[5pt] &\underline{\textbf{u}} \leq \textbf{u}\leq \overline{\textbf{u}}, \end{aligned} \end{equation}

where $\preccurlyeq$ is defined as vector component-wise inequality, $\underline{\textbf{x}}$ and $\overline{\textbf{x}}$ stand for the lower and upper bounds on the system state $\textbf{x}$ , and $\underline{\textbf{u}}$ and $\overline{\textbf{u}}$ stand for the lower and upper bounds on the control input $\textbf{u}$ , respectively. The state and input constraints ensure that the robot’s configuration, rates, and accelerations are realistic and therefore feasible when tracked; we also point out the inclusion of the ZMP stability constraint in (4).

Problem (4) is a NOCP with standard initial and final state constraints and the additional non-convex path constraint to enforce ZMP stability. Without pre-defined constraint arc sequence and boundaries (i.e., no a priori knowledge of ZMP-constrained motion occurrence), the optimality condition becomes impossible to formulate, and indirect formulations that aim to approach optimality conditions cannot be applied [Reference Betts41]. Hence, the two-point boundary value problem constructed from the NOCP problem has to be solved with a direct formulation using numerical methods, such as the shooting method.

Additionally, because of the nonlinear terrain function and the highly nonlinear ZMP constraint, the numerical methods must rely on a “close enough” initialization to find a solution in reasonable time [Reference Guibout and Scheeres42]. Since finding an appropriate initialization is a nontrivial task, it is impossible to solve (4) reliably using existing solution techniques for NOCP alone. To address this issue, the rough terrain mobile manipulation problem will be decomposed into two sub-problems: a robot arm manipulation planning problem and a mobile base relocation planning problem. Further, the dimension reduction technique to speed up the manipulation planning solution will be introduced in Section 3, and the techniques involved with generating the quasi-static path to warm start the relocation planning will be introduced in Section 4.

4.1. Quasi-static path planning algorithm

Sampling-based approach is chosen to solve the path and reconfiguration planning problem due to its theoretical completeness, ability to deal with nonlinear constraints, and efficiency when search space is purposefully constructed. The sampling-based planning structure used in our implementation is based on the RRT algorithm proposed in ref. [Reference LaValle and Kuffner23]. The pseudo-code of what we call “RRT $\mathcal{S}$ ” is shown in Algorithm 1, and it includes two main modifications: the first to accommodate the ZMP stability constraint on line 9 highlighted in blue and the second modification made to allow manipulator reconfiguration, lines 13-14 highlighted in red. The RRT algorithm, specifically, is chosen due to its ability to quickly find a path when new terrain and obstacles such as human, fallen trees, or large rocks appear on the map. However, the ZMP constraint accommodation can also be directly implemented within other sampling-based planning algorithms.

4.2. Finite-time feasibility guarantee

Due to the quasi-static nature of the path planning stage, the inertial terms in the ZMP constraints are ignored. Therefore, a quasi-static path generated with Algorithm 1 does not guarantee that there exists a trajectory for the robot to travel to its destination without violating the dynamic stability constraint when the robot’s components experience non-zero linear and angular accelerations. In order to address this issue, the following theorem is introduced:

A proof of Theorem 4.1 is provided in Appendix A. With Theorem 4.1 backing, the method to find a quasi-static path that satisfies the ZMP constraint is presented in the following subsections. It is worth pointing out that Theorem 4.1 also shows that a ZMP-constrained quasi-static path is a controlled invariant set of robot state $\textbf{x}$ as defined in ref. [Reference Blanchini46].

4.3. Stability-guaranteed sampling-based path search for continuous motion

To plan a robot path on 3D terrain using a sampling-based algorithm, such as RRT, nonlinear constraints are checked at each sampling point to ensure constraint violations do not occur. However, to keep the computation time reasonable, constraint satisfaction cannot be verified continuously in between sampling points. Since the ZMP constraint (3) is non-convex with respect to the robot’s base orientation, even if the robot is dynamically stable at two neighboring sampling points, it is not guaranteed to be dynamically stable between two sampling points. To resolve this issue, an analytical method to determine the ZMP stability between sampling points is desired.

In the stability-constrained RRT implementation, ZMP constraint satisfaction is unchecked in the following two scenarios. The first is when the robot’s base changes its heading direction at a particular sampling location and the second is the robot quasi-statically “slides” from one sampling location to the next. Since the two instances happen separately in the RRT implementation, they can be dealt with individually. In the following analysis, we will make use of $g_z\gt 0$ , since we do not expect the robot to drive up vertical walls. In addition, under the quasi-static condition, (3) reduces to

(7) \begin{equation} x_{\text{zmp}} = \dfrac{M_x g_z-M_z g_x}{M g_z},\;\; y_{\text{zmp}} = \dfrac{M_y g_z-M_z g_y}{M g_z}, \end{equation}

where $M_x=\sum _i m_i x_i$ , $M_y=\sum _i m_i y_i$ , $M_z=\sum _i m_i z_i$ , and $M=\sum _i m_i$ . When the same robot is located on the horizontal terrain, according to (7), its ZMP location is further simplified to:

(8) \begin{equation} x_{\text{zmp,0}}=\frac{M_x}{M},\;\; y_{\text{zmp,0}}=\frac{M_y}{M}. \end{equation}

4.3.1. Mobile base attitude

To analyze the ZMP stability of the two aforementioned scenarios, we first need to define the robot base attitude on arbitrary 3D terrain. With $\textbf{p}_b=[s_x, s_y, s_z]^T$ representing the 3D coordinate of terrain surface in inertial frame $\mathcal{O}$ at the location of the mobile base, the terrain can be expressed by the following nonlinear function:

(9) \begin{equation} s_z = h(s_x, s_y). \end{equation}

Let us denote the rotation matrix that describes the attitude change of the base-fixed frame $\mathcal{F}_0$ from the inertial frame $\mathcal{O}$ as $R=\{r_{ij}\}\;(i,j = 1,2,3)$ , and since the base-fixed $z_0$ axis of a mobile manipulator remains perpendicular to terrain surface under normal operations, defining the directional gradients of the terrain $\textbf{h}_x = \left [1,0,\dfrac{\partial h}{\partial s_x}\Bigr |_{s_x,s_y}\right ]^T$ and $\textbf{h}_y = \left [0,1,\dfrac{\partial h}{\partial s_y}\Bigr |_{s_x,s_y}\right ]^T$ , the third column of $R$ can be represented as:

\begin{equation*} \hat {\textbf{r}}_3 = \frac {\textbf{h}_x \times \textbf{h}_y}{\left |\left | \textbf{h}_x \times \textbf{h}_y \right | \right |}. \end{equation*}

Let us denote the heading angle defined by the projection of the mobile base’s heading vector onto the 2-D plane $\textbf{x}_{\mathcal{O}}$ - $\textbf{y}_{\mathcal{O}}$ as $\bar{\psi }$ . The second column of $R$ describes the mobile base’s heading direction on the 3D surface and can then be written as:

\begin{equation*} \hat {\textbf{r}}_2 = \frac {[\textbf{h}_x, \textbf{h}_y]\left[\!-\!\sin {\bar {\psi }}, \cos {\bar {\psi }}\right]^T}{\left |\left |[\textbf{h}_x, \textbf{h}_y]\left[\!-\!\sin {\bar {\psi }}, \cos {\bar {\psi }}\right]^T\right |\right |}. \end{equation*}

Hence, the first column of $R$ can be found using:

\begin{equation*} \hat {\textbf{r}}_1 = \frac { \hat {\textbf{r}}_2 \times \hat {\textbf{r}}_3}{\left |\left | \hat {\textbf{r}}_2 \times \hat {\textbf{r}}_3\right |\right |}. \end{equation*}

4.3.2. Stability guarantee during base turn

Inspecting (7), it is clear that $g_x$ , $g_y$ , and $g_z$ are the only quantities that vary with the robot’s orientation. As a robot whose attitude is described by the rotation matrix $R$ executes a skid-steered turn through angle $\psi ^t$ , the gravity components are affected by the yaw angle $\psi ^t$ and can be expressed as:

(10) \begin{equation} \begin{aligned} \begin{bmatrix} g^t_x\\[5pt] g^t_y\\[5pt] g^t_z \end{bmatrix}&= \begin{bmatrix} \cos{\psi ^t}&\sin{\psi ^t}&0\\[5pt] -\!\sin{\psi ^t}&\cos{\psi ^t}&0\\[5pt] 0&0&1 \end{bmatrix} \underbrace{\begin{bmatrix} \hat{\textbf{r}}_1^T\\[5pt] \hat{\textbf{r}}_2^T\\[5pt] \hat{\textbf{r}}_3^T \end{bmatrix}}_{R^T} \begin{bmatrix} 0\\[5pt]0\\[5pt]g \end{bmatrix}\\[5pt] &=\begin{bmatrix} r_{13}\cos{\psi ^t}g+r_{23}\sin{\psi ^t}g\\[5pt] -r_{13}\sin{\psi ^t}g+r_{23}\cos{\psi ^t}g\\[5pt] r_{33}g \end{bmatrix}. \end{aligned} \end{equation}

The ZMP after the turn is then found from:

(11) \begin{equation} x^t_{\text{zmp}} = \dfrac{M_x g^t_z-M_z g^t_x}{M g^t_z},\; y^t_{\text{zmp}} = \dfrac{M_y g^t_z-M_z g^t_y}{M g^t_z}. \end{equation}

To aid in the illustration of stability guarantee during a skid-steered turn, the following claim is presented with a proof that follows:

Proposition 1. As a wheeled/tracked robot executes a continuous unidirectional quasi-static turn through angle $\psi ^t$ about its yaw axis, the ZMP of the robot traces, monotonically on the support polygon, an arc of angle $\psi ^t$ , radius $\dfrac{r_{13}^2M_z^2+r_{23}^2M_z^2}{r_{33}^2M^2}$ , and center $[x_{\text{zmp,0}},\;y_{\text{zmp,0}}]^T$ .

Proof 1. By defining $\Delta x_{\text{zmp}}=x^t_{\text{zmp}}-x_{\text{zmp,0}}$ , and $\Delta y_{\text{zmp}}=y^t_{\text{zmp}}-y_{\text{zmp,0}}$ , the difference between (11) and (8) can be represented as:

(12) \begin{equation} \Delta x_{\text{zmp}} = \frac{M_zg^t_x}{-Mg^t_z}, \;\; \Delta y_{\text{zmp}} = \frac{M_zg^t_y}{-Mg^t_z}. \end{equation}

Substituting (10) into (12), we obtain

(13) \begin{equation} \begin{bmatrix} \Delta x_{\text{zmp}}\\[5pt] \Delta y_{\text{zmp}} \end{bmatrix} = \begin{bmatrix} \cos{\psi ^t} & \sin{\psi ^t}\\[5pt] -\sin{\psi ^t} & \cos{\psi ^t} \end{bmatrix} \left[\begin{matrix} \dfrac{r_{13}M_z}{-r_{33}M}\\[12pt] \dfrac{r_{23}M_z}{-r_{33}M} \end{matrix}\right]. \end{equation}

The above represents a rotation in the $x_0$ - $y_0$ plane of ZMP location by angle $\psi ^t$ with respect to $[x_{\text{zmp,0}},\;y_{\text{zmp,0}}]^T$ .

With the help of Proposition 1, graphically illustrated in Fig. 4(a), the ZMP stability of a robot during a quasi-static turn can be checked analytically for a given initial base attitude, arm configuration, and turn angle. As a result, the path planning algorithm is able to generate turns that are guaranteed to be continuously ZMP-stable.

Figure 4. Graphical illustration of ZMP trajectory during robot base turn and mobile relocation.

4.3.3. Stability guarantee during base relocation

During base relocation between two neighboring sampling points, it is reasonable to represent the terrain-induced attitude change of the robot’s base as a pitch-roll sequence. By restricting the distance between two sampling points to be “small” during sampling point generation, we expect the changes of pitch and roll angle to be small and monotonic. With pitch and roll angle changes from the given base orientation $R$ represented by $\theta ^r$ and $\phi ^r$ , respectively, the perturbed gravity components can be written as:

(14) \begin{equation} \begin{aligned} \begin{bmatrix} g^r_x\\[5pt] g^r_y\\[5pt] g^r_z \end{bmatrix}& = \begin{bmatrix} \cos{\phi ^r} & \sin{\phi ^r}\sin{\theta ^r} & -\sin{\phi ^r}\cos{\theta ^r}\\[5pt] 0 & \cos{\theta ^r} & \sin{\theta ^r}\\[5pt] \sin{\phi ^r} & -\cos{\phi ^r}\sin{\theta ^r} & \cos{\phi ^r}\cos{\theta ^r} \end{bmatrix} \underbrace{\begin{bmatrix} \hat{\textbf{r}}_1^T\\[5pt] \hat{\textbf{r}}_2^T\\[5pt] \hat{\textbf{r}}_3^T \end{bmatrix}}_{R^T} \begin{bmatrix} 0\\[5pt]0\\[5pt]g \end{bmatrix}\\[5pt]& = \begin{bmatrix} r_{13}\; \cos{\phi ^r} g + r_{23}\sin{\phi ^r}\sin{\theta ^r}g - r_{33}\sin{\phi ^r}\cos{\theta ^r} g\\[5pt] r_{23}\;\cos{\theta ^r}g+r_{33}\sin{\theta ^r}g\\[5pt] r_{13} \sin{\phi ^r} g - r_{23}\cos{\phi ^r}\sin{\theta ^r}g + r_{33}\cos{\phi ^r}\cos{\theta ^r} g \end{bmatrix}. \end{aligned} \end{equation}

Substituting (14) into (7), the following is obtained to describe the ZMP location as a function of pitch and roll angles due to relocation:

(15) \begin{equation} \begin{aligned} x^r_{\text{zmp}}&=\frac{M_x}{M}-\frac{M_z}{M}G_1\\[5pt] y^r_{\text{zmp}}&=\frac{M_y}{M}-\frac{M_z}{M}G_2, \end{aligned} \end{equation}

where

(16) \begin{equation} \begin{aligned} G_1 &= \frac{r_{13}\cos{\phi ^r}+r_{23}\sin{\phi ^r}\sin{\theta ^r}-r_{33}\sin{\phi ^r}\cos{\theta ^r}}{r_{13}\sin{\phi ^r}-r_{23}\cos{\phi ^r}\sin{\theta ^r}+r_{33}\cos{\theta ^r}\cos{\phi ^r}}\\[5pt] G_2 &= \frac{r_{23}\cos{\theta ^r}+r_{33}\sin{\theta ^r}}{r_{13}\sin{\phi ^r}-r_{23}\cos{\phi ^r}\sin{\theta ^r}+r_{33}\cos{\theta ^r}\cos{\phi ^r}}. \end{aligned} \end{equation}

The stability guarantee during base relocation between neighboring sampling points is presented as the following proposition with a proof in Appendix B:

Proposition 2. As a wheeled/tracked robot relocates from one sampling point to the next through a continuous motion, the ZMP locations are guaranteed to be bounded by a rectangle whose edges are parallel and perpendicular to the robot’s heading direction; as well, the rectangle’s non-neighboring corners are the ZMPs at the two sampling points that the robot is transitioning between.

With Proposition 2 introduced and graphically illustrated in Fig. 4(b), we claim that as long as the rectangle derived above is contained within the convex support polygon, the robot is guaranteed to be stable during relocation on the quasi-static path. Hence, in addition to verifying ZMP constraint satisfaction at each sampling step, the planner also checks if the “arc” of Proposition 1 and the “rectangle” of Proposition 2, as defined by the sampling point and its parent node, are within the support polygon to form a quasi-static path that is guaranteed to be continuously ZMP-stable.

4.4. Traction optimization

The results obtained thus far focused on the rollover stability constraint for the robot. However, relocating on 3D terrain, which may be steep and loose, also requires us to consider traction limits in order to ensure the robot does not experience traction loss and unstable sliding. Since mobile manipulators with only two actuated wheels or tracks are more prone to traction loss and resulting slippage, this Section will be focused on this type of robot with common symmetrical drive layout, including both wheeled and tracked robots.

To conduct a force analysis for the robot’s two wheels/tracks, we denote the total normal force experienced by the robot as $\,\textbf{f}_n$ , the static friction coefficient as $\mu$ , and the friction force generated by the robot’s wheels/tracks to remain static on a hill as $\,\textbf{f}_{f}^0$ , where $\|\,\textbf{f}_{f}^0\|_2\leq \|\mu \textbf{f}_n\|_2$ . In order to avoid unplanned sliding as the robot accelerates, the minimum available friction force $f_f^{avail}$ along the heading direction of the mobile base should be optimally distributed between the two wheels/tracks.

To avoid sliding, the following inequality has to hold:

(17) \begin{equation} \|\,\textbf{f}_f^0+\textbf{f}_f^a\|_2\leq \|\mu \textbf{f}_n\|_2, \end{equation}

where $\,\textbf{f}_f^a$ is the friction force additional to $\,\textbf{f}_f^0$ that results in base acceleration. Applying a more restrictive bound, we get

(18) \begin{equation} \begin{aligned} \|\,\textbf{f}_f^0\|_2+\|\,\textbf{f}_f^a\|_2 &\leq \|\mu \textbf{f}_n\|_2\\[5pt] \|\,\textbf{f}_f^a\|_2 &\leq \|\mu \textbf{f}_n\|_2-\|\,\textbf{f}_f^0\|_2\\[5pt] f_f^{avail}&=\|\mu \textbf{f}_n\|_2-\|\,\textbf{f}_f^0\|_2. \end{aligned} \end{equation}

Note that (18) gives the magnitude of traction force $\,\textbf{f}_f^a$ a more restrictive bound than (17), and hence, it is named “minimum available traction force.” It is a conservative quantity that is ideal for safety assurance.

Let the distance between the left wheel/track and the ZMP be denoted with $l$ , and the distance between the right wheel/track and the ZMP be denoted with $r$ . We also denote the magnitude of $\,\textbf{f}_{n}$ and $\,\textbf{f}_{f}^0$ with scalars $\,f_n$ and $\,f_f^0$ , respectively. Then, the normal force distribution on each of the left and right wheel/track can be respectively expressed as:

(19) \begin{equation} \begin{aligned} \|\mu \textbf{f}_{n}\|_2^L = \mu f_n\frac{r}{l+r},\;\;\; \|\mu \textbf{f}_{n}\|_2^R = \mu f_n\frac{l}{l+r}. \end{aligned} \end{equation}

The friction forces on the two sides that counter sliding due to gravity can be written as

(20) \begin{equation} \begin{aligned} \|\textbf{f}_f^0\|_2^L = f_f^0\frac{r}{l+r},\;\;\; \|\textbf{f}_f^0\|_2^R = f_f^0\frac{l}{l+r}. \end{aligned} \end{equation}

The amount of minimum available traction force on each side can then be found using (18) by subtracting (20) form (19):

(21) \begin{equation} \begin{aligned} f_{f}^{avail,L} = \left(\mu f_n-f_f^0\right)\frac{r}{l+r},\; f_{f}^{avail,R} = \left(\mu f_n-f_f^0\right)\frac{l}{l+r}. \end{aligned} \end{equation}

Then, traction optimization can be achieved by maximizing the minimum available traction forces on both the left and right wheels/tracks using the following formulation:

(22) \begin{equation} \max _{p_{zmp}} \min\! \left(\,f_{a}^{avail,L}(\textbf{p}_{zmp}),f_{a}^{avail,R}(\textbf{p}_{zmp})\right). \end{equation}

In order to achieve (22), $l=r$ allows available friction forces in (21) to be equal on both wheels. Note that (22) does not guarantee the elimination of slippage. Rather, optimally positioning the ZMP in a way to reduce the occurrence of slipping and maintain the control authority of the drives on both sides.

It is therefore claimed that vehicle traction is optimized when ZMP lies along the longitudinal center line of $Conv(S)$ . However, constraining the ZMP to the center line eliminates the flexibility of robot reconfiguration and would drastically reduce the robot’s ability to traverse through terrain. A varying factor that allows the ZMP to deviate from the support polygon’s center line on milder terrain will be implemented in the planning phase.

A comparison between a robot’s path generated with and without traction optimization is shown in Fig. 5. With traction optimization, the robot’s path in Fig. 5(a) is smoother when compared to that in Fig. 5(b). This means that with traction optimization, the robot will tend to descend/ascend the steep sections along the direction of steepest descent/ascent in order to evenly distribute available traction forces between the two driving wheels/tracks to avoid sliding.

Figure 5. Two sets of path generated to compare the effect of traction optimization.

5.1. Kinematics model of a feller buncher

The feller buncher machine is modeled after the Tigercat 855E, and its kinematics can be described with the schematic diagram in Fig. 2 and the Denavit-Hartenberg parameters in Table II. The machine is considered to be holding onto a tree in the manipulation planning example, and the length $l_i$ and mass $m_i$ of each of the machine’s links and the tree are summarized in Table I. The machine’s support polygon is a $3.23\;\text{m }\!\!\!\times 5\;\text{m}$ rectangle.

Table I. Mobile manipulator parameters for the test case.

Table II. Denavit-Hartenberg parameters table of the feller buncher manipulator.

The optimal control results in this Section are achieved by running the general optimal control solver GPOPS [Reference Patterson and Rao47] under MATLAB environment on a Windows desktop with Intel Core i7-4770 3.40 GHz processor. The solver formulates a NOCP, through direct method, into a nonlinear programming problem. Despite the theoretical advantage of indirect method, due to the non-convexity of the ZMP inequality constraint, the necessary optimality condition is impossible to find without a priori knowledge on constraint occurrence [Reference Betts41]. Hence, direct method is chosen to solve problem (4) based on a quasi-static path.

Since the simplified manipulator model has $2$ arm DoFs while the full model has $5$ arm DoFs, to ensure that the ZMP location of the full model follows that of the simplified model, a relationship between the DoFs of the simplified model and those of the full model of the machine needs to be established. To satisfy Assumption 1(b), for the specific machine geometry in this example, we choose to constrain the machine’s joint angles such that

(23) \begin{equation} [q_3, q_4,q_5]^T = [\!-\!\pi -\!2q_2, \dfrac{\pi }{2}+q_2, \text{constant}]^T, \end{equation}

and the corresponding relationship between joint accelerations follows:

(24) \begin{equation} [\ddot{q}_3,\ddot{q}_4,\ddot{q}_5]^T = [\!-\!2\ddot{q}_2, \ddot{q}_2,0]^T. \end{equation}

The relationship between $d$ and $q_2$ can be written as:

(25) \begin{equation} q_2=\sin ^{-1}\left(\frac{-d}{2l_2}\right). \end{equation}

Then, taking the first and second derivative of (25) with respect to time, we can obtain:

(26) \begin{equation} \begin{aligned} \dot{q}_2 & = \dfrac{-\dot{d}}{2l_2\sqrt{1-\dfrac{d^2}{4l_2^2}}}\\[5pt] \ddot{q}_2 & = -\dfrac{\ddot{d}}{2l_2\sqrt{1-\dfrac{d^2}{4l_2^2}}}-\dfrac{d\dot{d}^2}{8l_2^3\sqrt{1-\dfrac{d^2}{4l_2^2}}^3}. \end{aligned} \end{equation}

Therefore, using the mapping provided in (23) and (24), and the relation between joint angles and accelerations as given in (25) and (26), the ZMP location of the full kinematics model can be written as $ \textbf{p}_{\text{zmp}}(\tilde{\textbf{x}},\tilde{\textbf{u}})$ .

5.2. Full kinematics vs. simplified formulation vs. phase plane method

In the first test case, we consider the feller buncher to be situated statically on a slope that results in a 30-degree vehicle roll to the left. The machine starts from this initial condition and the goal is to have the cabin yaw 180 degrees towards left. The initial joint angles and velocities are:

(27) \begin{equation} \begin{aligned} \textbf{q}(t_0) &= [0, -\pi/6, -2\pi/3, \pi/6, -\pi/2]^T \text{ rad}\\[5pt] \dot{\textbf{q}}(t_0) &= [0,0,0,0,0]^T \text{ rad/s}, \end{aligned} \end{equation}

and the desired final joint angles and velocities are:

(28) \begin{equation} \begin{aligned} \textbf{q}(t_f) &= [\pi, -\pi/6, -2\pi/3, \pi/6, -\pi/2]^T \text{ rad}\\[5pt] \dot{\textbf{q}}(t_f) &= [0,0,0,0,0]^T \text{ rad/s}. \end{aligned} \end{equation}

The joint rate and acceleration constraints are:

\begin{align*} -\frac{\pi }{4} &\leq \dot{q}_i \leq \frac{\pi }{4} \text{ rad/s}\;\;\;\;\;\forall \; i\\[5pt] -\frac{\pi }{2} &\leq \ddot{q}_i \leq \frac{\pi }{2} \text{ rad/s}^2\;\;\;\;\forall \; i. \end{align*}

The ZMP loci of motions generated using the optimal trajectory planning formulation (4) with full kinematics model, the simplified kinematics model, and the classic phase plane method that considers joint angle, rate, and acceleration limits without stability constraints [Reference Shin and McKay25] are presented in Fig. 6. These results show that if the feller buncher follows the time-optimal trajectory prescribed by the phase plane method, the ZMP locus (green) of the machine will travel outside of the support polygon. In this case, the machine is at risk of rolling over. However, the motion generated through solving (4) with full kinematics model and the simplified kinematics model would result in safe (red and blue) ZMP loci. It is noted that although the starting and ending states of the machine for the three trajectory planning methods are the same, the ZMP loci do not start and end at the same points due to different initial accelerations. Additionally, even though the ZMP locus generated from the simplified model (blue) displays chattering behavior, the corresponding joint accelerations shown in Fig. 7(a) are sufficiently smooth. The unsafe motion generated by the phase plane method takes 4.5 s to complete; the safe motion generated by solving the NOCP (4) also takes 4.5 s to complete; and the safe motion generated with the simplified kinematics model takes 5.0 s to complete.

Figure 6. ZMP loci of constrained trajectory planning vs. phase plane method.

Figure 7. Joint accelerations (a), velocities (b), and angles (c) plots. Blue line indicates results from the simplified model, red line indicates results from the full model, black dashed lines indicate constraints. Note that all blue values of $q_5$ are zero due to this DoF being reduced in the simplified model.

For the solutions of (4) and (4) with simplified kinematics, the planned joint accelerations, rates, and angles are displayed in Fig. 7(a), (b), and (c), respectively. It is noted that all initial and final conditions, and state and input constraints are satisfied for both the full and simplified formulations. However, for this example, the computation time to solve (4) is more than 5 h, while the computation time to solve with simplified kinematics is 1.34 s.

5.3. Monte Carlo simulations for reconfiguration on different slopes

To test the performance of the simplified model for different initial and final conditions, additional simulations were carried out for four different machine base attitudes of 0,15, 20, and 30-degree roll, each with 3000 pairs of randomized initial and final cabin yaw angles from the set $[\!-\!2\pi,2\pi ]$ with other initial and final joint angles the same as in (27) and (28). The trajectory planning success rate is presented in Table III. The distribution of successful trajectory planning times for the four base attitudes is presented in Fig. 8.

It can be observed from these results that the planning success rate decreases with increasing slope angle, and computation time increases with increasing slope angle. The decreasing trend in success rate is due to the more frequent appearance of infeasible initial and final conditions. The distribution in Fig. 8 shows that the dimension reduction applied in the simplified model allows the computation time to be low enough for online guidance in most cases.

5.4. Mobile relocation planning

To test the performance of mobile relocation trajectory planning of Section 4, a test terrain described by the sinusoidal function:

(29) \begin{equation} s_z=10\cos \left(\frac{\sqrt{{s_x}^2+{s_y}^2}}{10}\right) \end{equation}

is created. To formulate the NOCP (4) of this example, the terrain surface function (29) is integrated into the feller buncher’s simplified kinematics (5). It is also noted that the simplified kinematics will be used for the remainder of the simulation results.

Table III. Trajectory planning success rate with simplified kinematics model for four base attitudes.

Figure 8. Computation time for reconfiguration maneuver of four slope angles using simplified model.

Figure 9. Planned path and point of reconfiguration of the feller buncher machine on a sinusoidal test terrain.

The machine itself, without the tree, is then commanded to start from the peak of a mountain ( $s_x = 0$ , $s_y = 0$ , $s_z = 10$ ) and to relocate to a point ( $s_x = 0$ , $s_y = 63$ , $s_z \approx 10$ ) on the surrounding ridge (see Fig. 9). Denoting the base’s linear velocity as $v$ , the initial and final states for the machine’s base and arm are:

\begin{equation*} \begin {aligned} [x_0,y_0,v,\bar \psi,\dot {\bar {\psi }}]^T(t_0)&=[0,0,0,0,0]^T\\[2pt] \textbf{q}(t_0) &= [0, -\pi/6, -2\pi/3, \pi/6, -\pi/2]^T\\[2pt] \dot {\textbf{q}}(t_0) &=[0,0,0,0,0]^T\\[2pt] [x_0,y_0,v,\bar \psi,\dot {\bar {\psi }}]^T(t_f)&=[0,63,0,\text {free},0]^T\\[2pt] \dot {\textbf{q}}(t_f) &=[0,0,0,0,0]^T, \end {aligned} \end{equation*}

and we note that the final heading angle of the base and arm joint angles are left free. The constraints on the mobile base’s linear and angular rates and accelerations are:

\begin{equation*} \begin {aligned} 0\leq &v\leq 2.78\;\;\text {m}/\text {s}\\[5pt] -1\leq &u_a\leq 1\;\;\text {m}/\text {s}^2\\[5pt] -2\leq &\dot {\psi }\leq 2\;\;\text {rad}/\text {s}\\[5pt] -\frac {2}{3}\leq &u_{\psi }\leq \frac {2}{3}\;\;\text {rad}/\text {s}^2. \end {aligned} \end{equation*}

Algorithm 1 first generates a quasi-static path with guaranteed stability for continuous motion for the machine indicated by the blue crosses in Fig. 9. The run time for path generation is $2.00$ s. The resulting path requires the machine to reconfigure its cabin angle $q_1$ , as shown by Fig. 9(a) and (b), to shift the ZMP so the machine does not rollover towards the front as it is descending the slope. The sampling point (sampling point #13) where this configuration change happens is highlighted in Fig. 9(c) by a red circle.

Then, based on the quasi-static path and configuration change generated by Algorithm 1, an initial guess is given to the NOCP solver to generate linear and angular accelerations for the machine to allow for time-optimal relocation. The solver generates the trajectory of each segment with the initial and final location of a segment being two consecutive sampling points. To create an initial guess of each segment for the NOCP solver, the machine is constrained to be static at each sampling point but is given small constant linear velocity and heading rate to satisfy the quasi-static property. The resulting trajectories of all segments are then combined. The continuous path of the machine is represented by the red line in Fig. 9, the motion trajectory is shown in Fig. 10, and the ZMP trajectory is shown in Fig. 11. Note that the machine’s velocity remains 0 in segment 14 due to arm reconfiguration.

Figure 10. Motion trajectory generated by iteratively solving NOCP of each segment. Dashed vertical lines correspond to segments.

Figure 11. ZMP trajectory generated by iteratively solving NOCP of each segment.

The result in Fig. 10 shows that all state and stability constraint variables remained within their bounds, indicated by horizontal black dashed lines, and the overall trajectory takes $\approx 180$ s to complete. Each motion segment separated by vertical dashed lines took $\approx 0.30$ s to calculate, and the entire trajectory took $18.30$ s to plan. This shows that the framework proposed in this paper can be implemented to provide online stability-constrained trajectory planning.

The resulting ZMP trajectory is shown in Fig. 11 with black dashed horizontal lines indicating the boundaries of $Conv(S)$ . During sampling-based path planning, the deviation of $x_{zmp}$ from the centerline of $Conv(S)$ is constrained to change linearly with respect to the slope angle from $50\%$ of the total width of $Conv(S)$ on flat ground to $10\%$ on $45^o$ slopes. The machine’s $x_{zmp}$ is shown to have stayed close to $0$ , that is, the centerline of the support polygon, during ascent and descent as a result of the traction-optimized path as mentioned in Section 4.4. The location of $y_{zmp}$ is shown to generally move forward as the machine is moving down the slope and then move backward as the machine is climbing the slope. However, due to the quasi-static inter-segment constraint, the overall motion of the machine is not satisfactory: the velocities and accelerations are highly oscillatory (see the subfigures for $a$ and $\ddot{\psi }$ ) and can cause less than ideal performance, along with excessive hardware wear. To remedy this, a receding horizon NOCP solution scheme is implemented.

For the result shown in Fig. 10, more than one trajectory segment can be treated as a whole and thus used as the initial guess of a new NOCP segment. The length of time interval in the new combined segment will be called “horizon length.” For this specific example, the horizon length is chosen to be $\approx 4$ s. For each segment, when the solver cannot converge to a solution within a user specified time threshold, the re-planning is considered “unsuccessful,” and the solver is terminated for the current segment and the re-planning carries on to the next segment. When a re-planning is unsuccessful, the iterative result computed for the original segment is kept. This way, the motion trajectory can be optimized where possible to reduce the occurrence of machine stop-and-go.

The receding horizon scheme enhanced motion trajectory is shown in Fig. 12, with the corresponding ZMP trajectory shown in Fig. 13. It can be observed that, compared to Fig. 10, the motion trajectory has become significantly smoother. The relocation completion time has also been shortened to $\approx 93$ s due to the enhancement. Both the state and dynamic stability constraints are shown to be respected. The joint motion trajectories of the arm are shown in Fig. 14. The joint variables vary only in one segment which corresponds to 0 velocity of the machine’s base, when the robot reconfigures itself. The successful receding horizon re-planning segments took a total $10.52$ s to complete, while the 20 unsuccessful segments took a total of $10.00$ s with the computational time threshold set to $0.5$ s. The time-optimal trajectories exhibit behaviors similar to bang-bang control except when ZMP constraint violations are imminent.

Figure 12. Motion trajectory generated through iteratively solving NOCP with existing motion trajectory and a horizon length of 20 time steps.

Figure 13. ZMP trajectory generated through iteratively solving NOCP with existing motion trajectory and a horizon length of 20 time steps.

Figure 14. Joint motion trajectories of the machine with joint angles shown in red and joint velocities shown in blue.

5.5. Comparison to existing methods

As discussed in Section 1.2 and to the best of the authors knowledge, there are no existing methods to address the time-optimal, dynamic-stability-constrained, point-to-point trajectory planning of a mobile robot on 3D rough terrain. Hence, comparison to the state of the art presented here is carried out in a more general context, by comparing the hierarchical framework solution proposed in this paper to other general methods used in mobile robot motion planning. Since as noted in Section 1.2, the sampling-based planning methods are generally used for geometric planning only, because of their exponential growth in computation time with planning dimension, NOCP-based methods are considered for our comparison.

5.5.3. Quantitative comparison results with MPC

For the implementation of MPC, we set the sampling period to $0.2$ s, prediction horizon to $5$ steps, and control horizon to $2$ steps. Further details on the setup, parameters, and performance of the MPC scheme can be found in Appendix C. Two MPC examples are provided in this comparison: for the first, referred to as “RRT $\mathcal{S}$ +MPC (slow),” the reference input to MPC is a constant velocity of $0.1$ m/s, while for the second, named “RRT $\mathcal{S}$ +MPC (fast),” the reference is a higher constant velocity of $1$ m/s. The resulting paths and velocity profiles are shown in Fig. 15(a) and (b), respectively. In these plots, the color map represents the velocity profile ranging from $0$ m/s to $1.6$ m/s as the corresponding color turns from cold (blue) to warm (yellow). The corresponding ZMP trajectories resulting from the two MPC trajectories are shown in Fig. 15(c) where the horizontal (time) axis has been normalized for ease of comparison. The trajectories take $180$ s and $21$ s to complete, for the slow and fast examples, respectively. It can be observed that even though “RRT $\mathcal{S}$ +MPC (fast)” took significantly less time to reach the goal point, it resulted in large ZMP constraint violation near $0.6$ time unit. On the path shown in Fig. 15(b), this ZMP constraint violation corresponds to the second turn where the machine is commanded to retain higher velocity compared to “RRT $\mathcal{S}$ +MPC (slow)” and “Proposed” before exiting the turn. This is a direct result of the controller only following a quasi-statically ZMP-stable path, without taking the dynamic ZMP stability into consideration.

Figure 15. Comparison between MPC path tracking results with slow and fast uniform velocity reference trajectories and the trajectory generated using the proposed framework.

Following the proposed framework, the path generated with Algorithm 1 is treated as a quasi-static trajectory with a low velocity of $0.1$ m/s to warm start (4). The computation time to generate the iterative solution for all $15$ segments is $4.62$ s, and the receding horizon re-planning then takes a total of $5.67$ s with $8$ unsuccessful segments’ computational time going over the threshold of $0.5$ s. The average time to initially plan a segment is $0.31$ s, and the average successful re-planning time for each segment is $0.24$ . The planned trajectory of each segment takes an average of $1.7$ s to complete. The final trajectory takes $24$ s to complete, and the final path is plotted in Fig. 15(b), for direct comparison to “RRT $\mathcal{S}$ +MPC (fast)” solution. Figure 15(b) shows that the velocity profile for the “Proposed” solution trajectory decreased to a lower value, as compared to “RRT $\mathcal{S}$ +MPC (fast),” before reaching the apex of the second turn. The ZMP trajectory corresponding to the proposed framework solution remains within the support polygon for the whole maneuver (see Fig. 15(c)).

The comparison presented here clearly demonstrates that the dynamic stability constraint has to be respected in both path and trajectory generation for safe and time-efficient robot motion. The proposed hierarchical solution framework can directly accomplish this goal. Although the reference trajectory for MPC can be tuned through trial and error to have a sufficiently slow velocity to approach the quasi-static ZMP-stable conditions, this can drastically reduce the motion efficiency.