# Stability Constrained Mobile Manipulation Planning on Rough Terrain

Jiazhi Song(宋佳智)1 and Inna Sharf2 Both authors are with department of
Mechanical Engineering, McGill University, Montreal, QC H3A 0C3, Canada
1jiazhi.song<EMAIL_ADDRESS>
###### Abstract

This paper presents a framework that allows online dynamic-stability-
constrained optimal trajectory planning of a mobile manipulator robot working
on rough terrain. First, the kinematics model of a mobile manipulator robot,
and the Zero Moment Point (ZMP) stability measure are presented as theoretical
background. Then, a sampling-based quasi-static planning algorithm modified
for stability guarantee and traction optimization in continuous dynamic motion
is presented along with a mathematical proof. The robot’s quasi-static path is
then used as an initial guess to warm-start a nonlinear optimal control solver
which may otherwise have difficulties finding a solution to the stability-
constrained formulation efficiently. The performance and computational
efficiency of the framework are demonstrated through an application to a
simulated timber harvesting mobile manipulator machine working on varying
terrain. The results demonstrate feasibility of online trajectory planning on
varying terrain while satisfying the dynamic stability constraint.

##  I Introduction

Mobile manipulation is a popular topic in robotics research due to the
omnipresence of this task in robotic applications. From indoor assistance [1]
to Mars exploration [2], mobile manipulators find application in various
industries. The versatility of mobile manipulator robots is due to their
simple yet effective construction: a certain number of manipulator arms
mounted on top of a mobile platform that is either wheeled or tracked. In
order to expand the application of mobile manipulators to uncontrolled outdoor
environments, research efforts have also been spent on increasing the
resiliency of mobile manipulators to their operating environment. Among
different types of disturbances that can arise in outdoor environments,
terrain variation [3] is among the most common, and it can have a large
negative impact on the performance of a mobile robot if rollover or sliding
occurs.

Heavy equipment commonly used in mining, logging, and construction such as
excavators, feller bunchers, and loaders can also be treated as mobile
manipulators. Due to the slow development of automation in these industries,
the needs related to terrain variation have rarely been addressed by the
mobile manipulation research community. The aforementioned machines are prone
to roll over as they have a high center of mass, manipulate heavy objects, and
inevitably have to operate on slopes and in adverse weather conditions. The
prevention of rollover and sliding is also of great importance to the machines
employed in these industries, as this type of failure poses numerous risks to
the operator, machine itself, and the environment. This paper aims to
introduce an approach to allow robots to work in terrain-varying environments
while maintaining or increasing their efficiency to also benefit the safety,
productivity, and energy efficiency of future autonomous industrial
applications.

In order to ensure the upright stability of robots during mobile manipulation,
passive methods such as novel mechanical design [4, 5] and steep slope
avoidance [6, 7] can be utilized. However, complicated mechanical designs may
not be practical in industrial applications, and slope avoidance misses out on
the reconfiguration capability of mobile manipulators and may lead to a
drastic reduction of their operational range. Therefore, in this article, we
take a more proactive approach to plan the motion and configuration of a
mobile manipulator robot to best allow stability constraint satisfaction so
that a desired task is more likely to be completed. Before the proposed
approach is introduced, past literature on stability measures, trajectory
planning, stability constrained trajectory planning, and autonomous industrial
machines is reviewed.

###  I-A Stability Measures

Several measures designed to quantify the proximity to a rollover condition
through force and moment measurement include the static force and moment
analysis introduced in [8], the force-angle stability measure in [3, 9, 10],
and the lateral load transfer (LLT) parameter employed in [11, 12, 13].
However, these measures are based on the forces/moments experienced at joints
or wheels and cannot provide direct guidelines for trajectory planning.
Considering the complex structure of humanoid robots and the effect of
momentum caused by different motions, the zero moment point (ZMP) stability
measure was introduced in [14] to achieve quantitative stability inference
based on joint motions and robot inertia parameters.

Since the mobile manipulation planning has to account for the robot’s and the
manipulated object’s inertia parameters and the robot’s joint motions, the ZMP
stability measure naturally becomes the criterion of choice in this paper. The
ZMP measure may be considered as too conservative in a walking robot scenario
as it focuses on the overall moment along a supporting edge. In other words,
ZMP instability corresponds to the beginning of rolling but does not
necessarily mean the robot will rollover. However, the criterion is
appropriate for a mobile manipulator robot with tracked/wheeled base since the
base is intended to maintain full contact with the ground during ordinary
operation.

###  I-B Mobile Manipulation Planning

There is a variety of results on manipulator and mobile manipulator optimal
trajectory planning [15, 16, 17, 18] but none of the previous works address
the problem of rollover and sliding prevention.

With an emphasis on arm-base coordination, theoretical results for coordinated
end-effector trajectory tracking with holonomic and nonholonomic base using a
continuous-time kinematic model of the robot were provided in [17]. Building
on the results in [17], a 5-DoF arm mounted on a nonholonomic mobile base to
draw lines on walls is modeled as a redundant system in [19]. Redundancy of
the system was utilized to maximize manipulability, reduce impact force from
contact with wall, and avoid obstacles. A coordinated motion generation method
for a mobile manipulator grasping an object with uncertainty is presented in
[20]. The mobile base is controlled to maximize arm reachability by
positioning the arm so that the object is located at a “sweet” spot within the
arm’s workspace. This method only considers the kinematic model of a robot and
would require another layer of control in order to track the prescribed
trajectory.

Using optimal control formulation, in [21], a mobile manipulator with a 3-DoF
holonomic base and a 5-DoF arm was able to achieve trajectory tracking under
various constraints while avoiding obstacles, by employing model predictive
control. In order to obtain linear constraints for the online optimization,
object avoidance constraints were approximated by assuming the robot is moving
at maximum velocity. However, this technique would drastically reduce the
mobile capability of a robot in a cluttered environment, such as a forest.

###  I-C ZMP Measure and Mobile Robots

An early implementation of the ZMP measure to a mobile manipulator is
presented in [22], and further work on mobile robots with stability constraint
evolved from it. Guided by the ZMP formulation, a mobile manipulator’s base is
utilized to generate stability-compensating motions, while the manipulator arm
is executing tasks [1, 23]. By using potential functions derived from the ZMP
formulation, stability-compensating motion can also be generated for
manipulator arms, as demonstrated in [24, 25, 26]. However, requiring a robot
to continuously generate motion that compensates for stability during
operations will unnecessarily reduce a robot’s efficiency for task completion.

The ZMP measure can also be employed as a constraint in the path following
optimal control algorithms as demonstrated in [27]. But for paths generated
without paying attention to stability, applying the ZMP constraint at the path
following stage can cause infeasibility when the optimal control formulation
tries to find a solution.

###  I-D Robotics in Timber Harvesting

Timber harvesting, which is the targeted industry of application of our work,
is a very important industry for many countries including Canada. The majority
of modern timber harvesting businesses worldwide employ feller-bunchers and
timber harvesters to fell trees, and use skidders and forwarders to transport
felled trees for further processing. The aforementioned machines can be
considered as a type of mobile manipulator since they consist of mobile bases
and hydraulically powered mechanized arms with multiple degrees of freedom.
Nowadays, the machines still fully rely on operator judgement and control in
order to function [4]. The lack of machine automation in the timber harvesting
industry results in high operator training costs and suboptimal efficiency in
harvesting; there is a strong impetus for developing an autonomous harvesting
system.

Some progress has been made to increase the timber harvesting machines’
autonomy. The dynamics model of a timber harvester is presented in [28]; the
teleoperation of a forestry manipulator is showcased in [29]; the hydraulic
actuator control of a forwarder machine has been discussed in [30] and [31],
and the motion control of a forestry manipulator along a fixed path is
presented in [32]. However, a versatile trajectory planning algorithm that is
tailored to the terrain related challenges of timber harvesting is yet to be
developed.

###  I-E Contributions

Building on our earlier work in [33] that mainly deals with stability
constrained manipulation with simple mobile capability, the framework
presented in this paper consists of the following major additions:

  * •

The stability-constrained path and trajectory planning framework for mobile
manipulation on varying 3-D terrain is developed.

  * •

A modification to sampling-based path planning algorithms that guarantees the
satisfaction of non-convex stability constraints in continuous time is
developed.

  * •

A first theoretical analysis of the well-known ZMP stability measure with
respect to motion on 3-D terrain with known topography is carried out.

To the best of the authors’ knowledge, the framework presented in this paper
is the first to enable 3-D terrain mobile manipulation planning with a focus
on task completion instead of stability compensation. In other words, a
stability measure acts as a constraint that does not interfere with the
robot’s motion unless the constraint is violated. The main benefits of the
proposed framework include higher task completion rate and higher motion
efficiency compared to the previous work. This differs drastically from
planning methods in [23, 24, 25, 26], and [1] that mainly focus on generating
motions that improve stability.

###  I-F Organization

In this paper, a feller buncher machine employed for timber harvesting is
considered as an example of a mobile manipulator robot. In Section II, the
kinematics model of the machine and the ZMP measure formulation are introduced
as theoretical background for the paper. In Section III, the optimal control
formulation of the trajectory planning problem is presented subject to the ZMP
stability constraint. In light of the high dimensionality of the full model
and the resulting computational complexity of solving the optimal trajectory
planning problem, we propose a simplified model of the feller buncher; this
model also allows us to obtain an analytical solution to the trajectory
planning problem under certain assumptions. Simulation results showcasing the
motion plan will be presented in Section IV. Discussion and potential future
work will be mentioned in Section V.

##  II Theoretical Background

This paper provides a solution to the problem of rough terrain optimal
trajectory planning of mobile manipulators subject to dynamic stability
constraint. Some theoretical background including the kinematics of a mobile
manipulator robot, the mathematical definition of dynamic stability, and the
general formulation of the problem are introduced in this section.

###  II-A Kinematics of Mobile Manipulator

In order to accommodate the constraints imposed by the robot’s joint
properties and ZMP stability, we first derive the full kinematic model of a
mobile manipulator whose example is shown schematically in Figure 1. 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 terrain. The method
presented in this paper is also applicable to car-like mobile robots without
manipulators by treating them as mobile manipulators with $n=0$.

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,\dots,n\\}$. Here, link
$0$ refers to the robot’s mobile base, and links $1$ to $n-1$ refer to the
manipulator arm components, and link $n$ refers to the robot’s end effector.
The three axes of each link frame $\boldsymbol{x}_{i}$, $\boldsymbol{y}_{i}$,
and $\boldsymbol{z}_{i}$ are represented by red, green, and blue arrows,
respectively.

Figure 1: Schematic diagram of a tracked mobile manipulator with 5 manipulator
DoFs and its rectangular support polygon $Conv(S)$.

We denote the angle of joint $i$ between link $i-1$ and $i$ with
$q_{i}\in\mathbb{R}$ ($i=1,2,\dots,n$), and collect all joint angles into a
column vector $\boldsymbol{q}\in\mathbb{R}^{n}$, and consider the base-fixed
frame $\mathcal{F}_{0}$ to undergo a
$\boldsymbol{z}$-$\boldsymbol{x}$-$\boldsymbol{y}$ rotation from initial
attitude with respective angles represented by $\psi$ (yaw), $\theta$ (pitch),
and $\phi$ (roll). The Cartesian coordinates of each component’s center of
mass relative to $\boldsymbol{p}_{b}$ expressed in $\mathcal{F}_{0}$,
$\boldsymbol{p}_{i}^{\mathcal{F}_{0}}$, can be found similarly and written
succinctly by introducing
$\bar{\boldsymbol{q}}=[\boldsymbol{p}_{b}^{\mathcal{O}^{T}},\psi,\theta,\phi,\boldsymbol{q}^{T}]^{T}$
as:

$\boldsymbol{p}_{i}^{\mathcal{F}_{0}}=\boldsymbol{f}_{i}(\bar{\boldsymbol{q}}).$
(1)

From (1), we also obtain the inertial accelerations of links’ centers of mass
expressed in $\mathcal{F}_{0}$, which will be needed for the dynamics analysis
later:

$\displaystyle\dot{\boldsymbol{p}}_{i}^{\mathcal{F}_{0}}$
$\displaystyle=\boldsymbol{J}_{pi}(\bar{\boldsymbol{q}})\dot{\bar{\boldsymbol{q}}},$
(2) $\displaystyle\ddot{\boldsymbol{p}}_{i}^{\mathcal{F}_{0}}$
$\displaystyle=\dot{\boldsymbol{J}}_{pi}(\bar{\boldsymbol{q}})\dot{\bar{\boldsymbol{q}}}+\boldsymbol{J}_{pi}(\bar{\boldsymbol{q}})\ddot{\bar{\boldsymbol{q}}},$

where we introduced the position kinematics Jacobian,
$\boldsymbol{J}_{pi}(\bar{\boldsymbol{q}})=\frac{\partial{\boldsymbol{f}_{i}}}{\partial{\bar{\boldsymbol{q}}}}$.

As noted earlier, we consider mobile base as a unicycle model and the robot is
therefore subject to control input
$\boldsymbol{u}=[u_{a},u_{\psi},\boldsymbol{u}_{q}^{T}]^{T}$, where $u_{a}$,
$u_{\psi}$, and $\boldsymbol{u}_{q}$ represent acceleration along heading
direction, yaw angular acceleration, and manipulator arm joint accelerations,
respectively. Defining
$\boldsymbol{x}=[\bar{\boldsymbol{q}}^{T},\dot{\bar{\boldsymbol{q}}}^{T}]^{T}$,
the mobile manipulator’s kinematics equations can be written in state-space
form as:

$\dot{\boldsymbol{x}}=g(\boldsymbol{x},\boldsymbol{u}).$ (3)

Eq. (3) serves as the model of the robot for the overall optimal trajectory
planning problem. The terrain information is also considered to be embedded in
(3).

In addition to the kinematics equation (3), 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. A graphical illustration of the
support polygon is shown in Figure 1.

###  II-B Dynamic Stability of Mobile Manipulator

The ZMP measure 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.

According to [22], letting
$\boldsymbol{p}_{i}^{\mathcal{F}_{0}}=[x_{i},y_{i},z_{i}]^{T}$,
$\ddot{\boldsymbol{p}}_{i}^{\mathcal{F}_{0}}=[\ddot{x}_{i},\ddot{y}_{i},\ddot{z}_{i}]^{T}$,
and the gravitational vector
$\boldsymbol{g}^{\mathcal{F}_{0}}=[g_{x},g_{y},g_{z}]^{T}$, the coordinates of
ZMP location in the base frame
$\boldsymbol{p}_{\text{zmp}}^{\mathcal{F}_{0}}=[x_{\text{zmp}},y_{\text{zmp}},0]^{T}$
are expressed as:

$\begin{aligned}
x_{\text{\text{zmp}}}&=\dfrac{\sum_{i}m_{i}(\ddot{z}_{i}-g_{z})x_{i}-\sum_{i}m_{i}(\ddot{x}_{i}-g_{x})z_{i}}{\sum_{i}m_{i}(\ddot{z}_{i}-g_{z})}\\\
y_{\text{\text{zmp}}}&=\dfrac{\sum_{i}m_{i}(\ddot{z}_{i}-g_{z})y_{i}-\sum_{i}m_{i}(\ddot{y}_{i}-g_{y})z_{i}}{\sum_{i}m_{i}(\ddot{z}_{i}-g_{z})},\end{aligned}~{}~{}\forall~{}~{}i$
(4)

Note that differently from the formulations presented in previous works [22],
the signs in front of the gravitational acceleration components are negative
since gravity should be treated as an external force on the system. Also, by
definition, ZMP lies in the plane of the support polygon so that its
z-coordinate in $\mathcal{F}_{0}$ is zero. The mass of the manipulated object
and the terrain topography are assumed to be known in this work and can be
accounted for in (4). With $Conv(S)$ representing the mobile manipulator’s
support polygon, the robot is dynamically stable when
$\boldsymbol{p}_{\text{zmp}}\in Conv(S)$, but has the tendency to rollover
otherwise. Equation (4) can be evaluated by substituting from the kinematics
equations (1) and (2) represented in $\mathcal{F}_{0}$, once the joint motions
have been planned, to produce the ZMP locus, i.e., the trajectory of ZMP in
$\mathcal{F}_{0}$ during the motion of the robot.

###  II-C 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
$\boldsymbol{x}_{f}$ from an initial state $\boldsymbol{x}_{0}$ safely, we
formulate a constrained nonlinear optimal control problem (OCP) of the form:

$\displaystyle\min_{\boldsymbol{u}}{}{}$
$\displaystyle\int_{t_{0}}^{t_{f}}1~{}\text{d}t.$ (5) s.t.
$\displaystyle\dot{\boldsymbol{x}}=g(\boldsymbol{x},\boldsymbol{u})~{}~{}\boldsymbol{x}(t_{0})=\boldsymbol{x}_{0}~{}~{}\boldsymbol{x}(t_{f})=\boldsymbol{x}_{f}$
$\displaystyle\boldsymbol{p}_{\text{zmp}}(\boldsymbol{x},\boldsymbol{u})\in
Conv(S)$
$\displaystyle\underline{\boldsymbol{x}}\preccurlyeq\boldsymbol{x}\preccurlyeq\overline{\boldsymbol{x}}$
$\displaystyle\underline{\boldsymbol{u}}\preccurlyeq\boldsymbol{u}\preccurlyeq\overline{\boldsymbol{u}},$

where $\preccurlyeq$ is defined as vector component-wise inequality,
$\underline{\boldsymbol{x}}$ and $\overline{\boldsymbol{x}}$ stand for the
lower and upper bounds on the system state $\boldsymbol{x}$, and
$\underline{\boldsymbol{u}}$ and $\overline{\boldsymbol{u}}$ stand for the
lower and upper bounds on the control input $\boldsymbol{u}$, respectively.
The state and input constraints ensure that the robot’s configuration, joint
rates, and joint accelerations are realistic and therefore feasible when
tracked.

The optimal control problem defined in (5) can be solved by using a nonlinear
optimal control solver such as GPOPS [34], but the computational time required
is too long for online motion guidance. The ZMP stability constraint (4) is an
important contributor to high computational cost as it is nonlinear and non-
convex with respect to the system’s states, including the robot’s joint angles
and orientation.

##  III Local Manipulation Planning Under Dynamic Stability Constraint

In most manipulation scenarios, a common task for a mobile manipulator is to
interact with objects by securing, lifting, and placing them at a location
either nearby or at a distance. When dissected into two general modes of
operation – mobile relocation and local manipulation – any mobile manipulation
task can be defined as a combination of these two modes, either taking place
simultaneously or separately. Therefore, it is desirable to solve the two
stages separately in order to avoid the computational burden caused by a high-
dimensional nonlinear OCP.

The local manipulation planning of the mobile manipulator robot is addressed
in this section. During this stage, the robot has to execute joint motions in
order to reconfigure itself or interact with an object. The dynamic stability
of the robot is likely to be compromised when the robot has high center of
mass, when the joint motions are aggressive, when the terrain is steep, and
when the object is heavy. Trajectory planning for the mobile relocation stage
will be introduced in Section IV.

###  III-A Simplified Model of Mobile Manipulator with Reduced Mobile
Capability

Due to the high dimensionality of the kinematics model (3), the nonlinearity
that resides in the forward kinematics equations (1) and the ZMP formulation
(4), the solution of the optimal manipulation planning problem (5) requires
longer than practical computation time for online guidance, even for a case
where the base does not move. In order to develop a online trajectory planner
for the robot, a model simplification is called upon.

Figure 2: Schematic diagram of a simplified mobile manipulator.

To simplify the optimal control problem by reducing the dimensionality of the
problem, and considering typical modes of operation such as of a feller-
buncher in timber harvesting or excavators in mining and construction, the
$n$-joint-DoF manipulator arm is simplified to have 2 DoFs including the first
link yaw angle $q_{1}(t)$ and variable end effector distance $d(t)$ from the
yaw joint axis. The simplified model is illustrated schematically in Figure 2.
The simplification is also based on the following assumptions:

######  Assumption 1.

a) The mobile base has a fixed attitude and accelerates along
$\boldsymbol{y}_{0}$ direction. b) The $z$-coordinate of end effector does not
change in $\mathcal{F}_{0}$. c) The inverse kinematics mapping between end
effector distance $d$ and full model arm joint angles $\boldsymbol{q}$ except
for $q_{1}$ is known up to the second order derivative with respect to time.

Note that in practice, local manipulation tasks such as picking and placing
rarely require complex movements of the mobile base, Assumption 1a) can be
justified for cases such as excavation and timber harvesting. A formulation
that allows the execution of complex mobile maneuvers is presented in Sections
IV in the context of a relocation problem. To mitigate the limitations caused
by Assumption 1b), a specially designed motion planner can manipulate the end
effector into or out of the $x$-$y$ plane of $\mathcal{F}_{0}$, without
significant loss in performance as the vertical movements required are usually
small. Assumption 1c) is easily satisfied as inverse kinematics resolution
method mentioned in [35] is well established.

In the simplified model, the control inputs of the simplified model are
$u_{q_{1}}$, acceleration of $q_{1}$, and $u_{d}$, the second derivative of
$d$ with respect to time. The kinematics model of the simplified model is
straightforward to derive and can be written in state space form:

$\dot{\tilde{\boldsymbol{x}}}=\tilde{g}(\tilde{\boldsymbol{x}},\tilde{\boldsymbol{u}}),$
(6)

where $\tilde{\boldsymbol{x}}=[q_{1},\dot{q}_{1},d,\dot{d}]^{T}$, and
$\tilde{\boldsymbol{u}}=[u_{q_{1}},u_{d}]$.

###  III-B Mapping from Simplified Model to Full Model

To ensure that the ZMP location of the full model follows that of the
simplified model, a mapping between the DoFs of the simplified model and those
of the full model of the robot needs to be established. From Assumption 1c),
the robot’s joint angles, angular rates, and accelerations can be expressed
using the states and control inputs of the simplified model as:

$\displaystyle\boldsymbol{q}$ $\displaystyle=[q_{1},\mathcal{IK}(d)]^{T}$ (7)
$\displaystyle\boldsymbol{\dot{q}}$
$\displaystyle=[\dot{q}_{1},\mathcal{IK}^{1}(\dot{d},d)]^{T}$
$\displaystyle\boldsymbol{\ddot{q}}$
$\displaystyle=[u_{q_{1}},\mathcal{IK}^{2}(u_{d},\dot{d},d)]^{T}.$

Here, $\mathcal{IK}$ represents the inverse kinematics mapping referred to in
Assumption 1c) equation and the superscript signifies its order of derivative
with respect to time. Therefore, using the mapping provided in (3), (4), and
(7), the ZMP location of the full kinematics model can be expressed using the
simplified kinematic quantities $\tilde{\boldsymbol{x}}$ and
$\tilde{\boldsymbol{u}}$ as
$\tilde{\boldsymbol{p}}_{\text{zmp}}(\tilde{\boldsymbol{x}},\tilde{\boldsymbol{u}})$.

For certain limited scenarios, a further physical interpretation of the ZMP
measure that allows the analytical derivation of a bang-bang type time optimal
local manipulation plan is shown in our previous work in [33].

###  III-C Simplified Optimal Control Problem Formulation

####  III-C1 Manipulation Planning

To shorten the computational time so that the trajectory planner can provide
online guidance, the optimal control problem (5) can be reformulated for the
simplified model (6) as:

$\displaystyle\min_{\tilde{\boldsymbol{u}}}{}{}$
$\displaystyle\int_{t_{0}}^{t_{f}}1~{}\text{d}t.$ (8) s.t.
$\displaystyle\dot{\tilde{\boldsymbol{x}}}=\tilde{g}(\tilde{\boldsymbol{x}},\tilde{\boldsymbol{u}})~{}~{}\tilde{\boldsymbol{x}}(t_{0})=\tilde{\boldsymbol{x}}_{0}~{}~{}\tilde{\boldsymbol{x}}(t_{f})=\tilde{\boldsymbol{x}}_{f}$
$\displaystyle\tilde{\boldsymbol{p}}_{\text{zmp}}(\tilde{\boldsymbol{x}},\tilde{\boldsymbol{u}})\in
Conv(S)$
$\displaystyle\underline{\tilde{\boldsymbol{x}}}\preccurlyeq\tilde{\boldsymbol{x}}\preccurlyeq\overline{\tilde{\boldsymbol{x}}}$
$\displaystyle\underline{\tilde{\boldsymbol{u}}}\preccurlyeq\tilde{\boldsymbol{u}}\preccurlyeq\overline{\tilde{\boldsymbol{u}}}.$

It is noted that although the number of variables has been reduced, the ZMP
constraint employed in (8) is still derived from (4) using information of all
joints obtained from (7). Hence, the motions generated by solving the OCP (8)
will guarantee ZMP constraint satisfaction for the full model. Also, we point
out that in replacing the optimization problem (5) with (8), the dimension of
the OCP kinematic constraints has been reduced from $2n+12$ to $4$, and the
number of control inputs has been reduced from $n+2$ to 2. However, the input
constraints can no longer be placed directly on joint accelerations, although
in practice, this can be resolved by tightening the bound on
$\bar{\boldsymbol{u}}$ until all joint accelerations are feasible.

####  III-C2 Manipulation-coordinated Base Acceleration Planning

Since the configuration space of the simplified model does not include the
coordinates of the mobile base, planning of the base motion must be treated
separately. Under Assumption 1, we have
$\ddot{\boldsymbol{p}}_{0}^{\mathcal{F}_{0}}=[0,\ddot{y}_{0},0]^{T}$, and
therefore, by taking the partial derivative of $x_{\text{zmp}}$ and
$y_{\text{zmp}}$ with respect to $\ddot{y}_{0}$, we can see that
$x_{\text{zmp}}$ does not change when the base accelerates, and the variation
in $y_{\text{zmp}}$ can be described by:

$\frac{\partial
y_{\text{zmp}}}{\partial\ddot{y}_{0}}=\frac{-\sum_{i}m_{i}z_{i}}{\sum_{i}m_{i}(\ddot{z}_{i}-g_{z})}.$

In common manipulation operations, it is reasonable to assume that
$\ddot{z}_{i}$’s are negligible and $z_{i}$’s are constants. Once the optimal
control problem is solved using the formulation (8), the margin between the
upper and lower edges of the support polygon to the planned ZMP trajectory,
$d_{u}$ and $d_{l}$, respectively (see Figure 5), can be found, and upper and
lower bounds on the mobile base’s linear acceleration can be derived as:

$d_{u}\frac{\sum_{i}m_{i}g_{z}}{\sum_{i}m_{i}z_{i}}\leq\ddot{y}_{0}\leq-
d_{l}\frac{\sum_{i}m_{i}g_{z}}{\sum_{i}m_{i}z_{i}}.$ (9)

Acceleration of the mobile base can then be planned using bang-bang type
control with the above as input constraint.

##  IV Rough Terrain Relocation Planning

In this section, trajectory planning of the relocation stage of mobile
manipulation is addressed. During relocation, a mobile manipulator has to
navigate a constantly undulating terrain and, sometimes, shift its ZMP through
reconfiguring the manipulator to safely move through steep slope. The
topographic map of the 3D terrain that contains the robot and goal point is
assumed to be known. A sampling-based path and reconfiguration planning
algorithm with dynamic stability guarantee will now be introduced. The result
from path planning will then be used as an initial guess to warm-start a
nonlinear optimal control problem for fast convergence.

Algorithm 1 ZMP-constrained Path Planning

1:procedure TerrainRRT($p_{init},p_{goal},q_{robot}$)

2: $V\leftarrow p_{init}$

3: $E\leftarrow\\{\\}$

4: $Q\leftarrow q_{robot}$

5: for $i=1,2,\ldots,n$ do

6: $p_{rand}\leftarrow SampleFree(i)$

7: $p_{nearest}\leftarrow Nearest(V,p_{rand})$

8: $p_{node}\leftarrow Steer(p_{nearest},p_{rand})$

9: if ObstacleFree($p_{nearest},p_{node}$) and
ZMPStable($p_{nearest},p_{node},q_{robot}$) then

10: $V\leftarrow V\cup\\{p_{node}\\}$

11: $E\leftarrow E\cup\\{(p_{nearest},p_{node})\\}$

12: $Q\leftarrow Q\cup\\{q_{robot}\\}$

13: if TreeGrowth($V,i$) $\leq$ thereshold then

14: $q_{robot}\leftarrow Reconfigure(q_{robot})$ $\triangleright$ Change robot
configuration when size of $V$ grows slowly.  return $G=(V,E)$

###  IV-A Quasi-static Path Planning Algorithm

Sampling-based approach is chosen to solve the path 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 is based on the RRT algorithm proposed in [36]. The pseudo-
code that illustrates the procedure of path planning with ZMP constraint is
shown in Algorithm 1. The modification made to accommodate stability
constraint is highlighted in blue, and the modification made to allow
reconfiguration is highlighted in red.

The RRT algorithm, specifically, is chosen due to its ability to quickly find
a path when new terrain features 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.

The robot is considered to be quasi-statically moving through the terrain in
order to remove its linear and angular acceleration from the search space. The
robot only reconfigures its joint angles when the tree growth is slow. As a
result, the search space dimension is further reduced on mild terrain, and the
path searching time can be drastically reduced.

####  IV-A1 Stability Guarantee

To ensure the sampling based algorithm can generate a quasi-static path that
is guaranteed to be ZMP-stable in continuous execution, the ZMPStable function
on line 9 of Algorithm 1 checks that the stability conditions to be introduced
in Section IV-C are met.

####  IV-A2 Reconfiguration

As the path planning tree growth reaches the steep section in a terrain map,
new nodes will become less likely to be added to the tree. This is due to
stability constraints becoming harder to satisfy. To remedy this problem,
random joint reconfiguration is executed whenever the number of new nodes
added to the tree is lower than a threshold after certain number of
iterations. In this way, the robot will be able to navigate through steep
slopes by reconfiguring its arm. The method introduced in Section III is used
to further plan the motion trajectory of each reconfiguration.

###  IV-B 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 a robot can 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:

######  Theorem 1.

Along a path $\tau:I\to\boldsymbol{x}$, where $I=[0,1]$ is a unit interval and
the robot’s states $\boldsymbol{x}$ are quasi-static with
$\dot{\boldsymbol{x}}=0$, if the ZMP location $\boldsymbol{p}_{zmp}(\tau)$
belongs to the interior of $Conv(S)~{}\forall~{}\tau$, $\exists$
$\boldsymbol{u}$ for the trajectory planning problem (5) such that
$t_{f}=T<<\infty$.

A proof of Theorem 1 is provided in the Appendix section. With Theorem 1
backing, the method to find a quasi-static path that satisfies the ZMP
constraint is presented in the following subsections.

###  IV-C Stability Guaranteed Sampling-based Path Search for Continuous
Motion

To plan a robot path on 3-D 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 (4) 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 as 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}>0$, since we do not expect the robot to drive up vertical walls.
In addition, under the quasi-static condition, (4) reduces to

$x_{\text{zmp}}=\dfrac{M_{x}g_{z}-M_{z}g_{x}}{Mg_{z}},~{}~{}y_{\text{zmp}}=\dfrac{M_{y}g_{z}-M_{z}g_{y}}{Mg_{z}},$
(10)

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 (10), its ZMP location is
further simplified to:

$x_{\text{zmp,0}}=\frac{M_{x}}{M},~{}~{}y_{\text{zmp,0}}=\frac{M_{y}}{M}.$
(11)

####  IV-C1 Mobile Base Attitude

To analyze the ZMP stability of the two aforementioned scenarios, we first
need to define the robot base attitude on arbitrary 3-D terrain. With
$\boldsymbol{p}_{b}=[s_{x},s_{y},s_{z}]^{T}$ representing the 3-D 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:

$s_{z}=h(s_{x},s_{y}).$ (12)

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
$\boldsymbol{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 $\boldsymbol{h}_{x}=\left[1,0,\frac{\partial h}{\partial
s_{x}}\Bigr{|}_{s_{x},s_{y}}\right]^{T}$ and
$\boldsymbol{h}_{y}=\left[0,1,\frac{\partial h}{\partial
s_{y}}\Bigr{|}_{s_{x},s_{y}}\right]^{T}$, the third column of $\boldsymbol{R}$
can be represented as:

$\hat{\boldsymbol{r}}_{3}=\frac{\boldsymbol{h}_{x}\times\boldsymbol{h}_{y}}{\left|\left|\boldsymbol{h}_{x}\times\boldsymbol{h}_{y}\right|\right|}.$

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

$\hat{\boldsymbol{r}}_{2}=\frac{[\boldsymbol{h}_{x},\boldsymbol{h}_{y}][-\sin{\bar{\psi}},\cos{\bar{\psi}}]^{T}}{\left|\left|[\boldsymbol{h}_{x},\boldsymbol{h}_{y}][-\sin{\bar{\psi}},\cos{\bar{\psi}}]^{T}\right|\right|}.$

Hence, the first column of $\boldsymbol{R}$ can be found using:

$\hat{\boldsymbol{r}}_{1}=\frac{\hat{\boldsymbol{r}}_{2}\times\hat{\boldsymbol{r}}_{3}}{\left|\left|\hat{\boldsymbol{r}}_{2}\times\hat{\boldsymbol{r}}_{3}\right|\right|}.$

####  IV-C2 Stability Guarantee During Base Turn

Inspecting (10), 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 $\boldsymbol{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:

$\displaystyle\begin{bmatrix}g^{t}_{x}\\\ g^{t}_{y}\\\ g^{t}_{z}\end{bmatrix}$
$\displaystyle=\begin{bmatrix}\cos{\psi^{t}}&\sin{\psi^{t}}&0\\\
-\sin{\psi^{t}}&\cos{\psi^{t}}&0\\\
0&0&1\end{bmatrix}\underbrace{\begin{bmatrix}\hat{\boldsymbol{r}}_{1}^{T}\\\
\hat{\boldsymbol{r}}_{2}^{T}\\\
\hat{\boldsymbol{r}}_{3}^{T}\end{bmatrix}}_{\boldsymbol{R}^{T}}\begin{bmatrix}0\\\
0\\\ g\end{bmatrix}$ (13)
$\displaystyle=\begin{bmatrix}r_{13}\cos{\psi^{t}}g+r_{23}\sin{\psi^{t}}g\\\
-r_{13}\sin{\psi^{t}}g+r_{23}\cos{\psi^{t}}g\\\ r_{33}g\end{bmatrix}.$

The ZMP after the turn is then found from:

$x^{t}_{\text{zmp}}=\dfrac{M_{x}g^{t}_{z}-M_{z}g^{t}_{x}}{Mg^{t}_{z}},~{}y^{t}_{\text{zmp}}=\dfrac{M_{y}g^{t}_{z}-M_{z}g^{t}_{y}}{Mg^{t}_{z}}.$
(14)

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}$ along its yaw axis, the ZMP of the robot
traverses, monotonically on the support polygon, an arc of angle $\psi^{t}$,
radius $\frac{r_{13}^{2}M_{z}^{2}+r_{23}^{2}M_{z}^{2}}{r_{33}^{2}M^{2}}$, and
center $[x_{\text{zmp,0}},~{}y_{\text{zmp,0}}]^{T}$.

###### Proof.

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 (14) and (11) can be represented as:

$\Delta x_{\text{zmp}}=\frac{M_{z}g^{t}_{x}}{-Mg^{t}_{z}},~{}~{}\Delta
y_{\text{zmp}}=\frac{M_{z}g^{t}_{y}}{-Mg^{t}_{z}}.$ (15)

Substituting (13) into (15), we obtain

$\begin{bmatrix}\Delta x_{\text{zmp}}\\\ \Delta
y_{\text{zmp}}\end{bmatrix}=\begin{bmatrix}\cos{\psi^{t}}&\sin{\psi^{t}}\\\
-\sin{\psi^{t}}&\cos{\psi^{t}}\end{bmatrix}\begin{bmatrix}\frac{r_{13}M_{z}}{-r_{33}M}\\\
\frac{r_{23}M_{z}}{-r_{33}M}\end{bmatrix}.$ (16)

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 Figure 3(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.

(a) Arc formed by ZMP trajectory during a turn.

(b) ZMP trajectory and its rectangular bound (red) during relocation.

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

####  IV-C3 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 $\boldsymbol{R}$ represented by
$\theta^{r}$ and $\phi^{r}$, respectively, the perturbed gravity components
can be written as:

$\displaystyle\begin{bmatrix}g^{r}_{x}\\\ g^{r}_{y}\\\ g^{r}_{z}\end{bmatrix}$
$\displaystyle\\!\\!=\\!\\!\begin{bmatrix}\cos{\phi^{r}}&\sin{\phi^{r}}\sin{\theta^{r}}&-\sin{\phi^{r}}\cos{\theta^{r}}\\\
0&\cos{\theta^{r}}&\sin{\theta^{r}}\\\
\sin{\phi^{r}}&-\cos{\phi^{r}}\sin{\theta^{r}}&\cos{\phi^{r}}\cos{\theta^{r}}\end{bmatrix}\\!\\!\\!\underbrace{\begin{bmatrix}\hat{\boldsymbol{r}}_{1}^{T}\\\
\hat{\boldsymbol{r}}_{2}^{T}\\\
\hat{\boldsymbol{r}}_{3}^{T}\end{bmatrix}}_{\boldsymbol{R}^{T}}\\!\\!\\!\begin{bmatrix}0\\\
0\\\ g\end{bmatrix}$ (17)
$\displaystyle\\!\\!=\\!\\!\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\\\
r_{23}\cos{\theta^{r}}g+r_{33}\sin{\theta^{r}}g\\\
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}\\!\\!.$

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

$\displaystyle x^{r}_{\text{zmp}}$
$\displaystyle=\frac{M_{x}}{M}-\frac{M_{z}}{M}G_{1}$ (18) $\displaystyle
y^{r}_{\text{zmp}}$ $\displaystyle=\frac{M_{y}}{M}-\frac{M_{z}}{M}G_{2},$

where

$\displaystyle G_{1}$
$\displaystyle=\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}}}$
(19) $\displaystyle G_{2}$
$\displaystyle=\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}}}.$

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

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

###### Proof.

To find the variation in ZMP location with respect to $\phi^{r}$ and
$\theta^{r}$ each, we take the corresponding partial derivatives of $G_{1}$
and $G_{2}$. Furthermore, with the previously stated assumption that the
distances between neighboring sampling points are small, we evaluate the
partial derivatives at zero angles $\phi^{r}\approx 0$ and $\theta^{r}\approx
0$, and obtain:

$\displaystyle\frac{\partial G_{1}}{\partial\phi^{r}}$
$\displaystyle=-1-\frac{r_{13}^{2}}{r_{33}^{2}}$ (20)
$\displaystyle\frac{\partial G_{1}}{\partial\theta^{r}}$
$\displaystyle=-\frac{r_{13}r_{23}}{r_{33}^{2}}$ $\displaystyle\frac{\partial
G_{2}}{\partial\phi^{r}}$ $\displaystyle=-\frac{r_{13}r_{23}}{r_{33}^{2}}$
$\displaystyle\frac{\partial G_{2}}{\partial\theta^{r}}$
$\displaystyle=1+\frac{r_{23}^{2}}{r_{33}^{2}}.$

Recalling the orthonormality of the rotation matrix so that
$r_{13}^{2}+r_{23}^{2}+r_{33}^{2}=1$, the following can be inferred from (20):

$\displaystyle\frac{\partial G_{1}}{\partial\phi^{r}}$
$\displaystyle\leq-1,~{}~{}\frac{\partial G_{1}}{\partial\theta^{r}}\approx 0$
(21) $\displaystyle\frac{\partial G_{2}}{\partial\phi^{r}}$
$\displaystyle\approx 0,~{}~{}~{}~{}\frac{\partial
G_{2}}{\partial\theta^{r}}\geq 1.$

By substituting (21) into (18), we obtain the following constraints on the
variation of ZMP coordinates:

$\displaystyle\frac{\partial x^{r}_{\text{zmp}}}{\partial{\phi^{r}}}$
$\displaystyle\geq\frac{M_{z}}{M}>0,~{}~{}\frac{\partial
x^{r}_{\text{zmp}}}{\partial{\theta^{r}}}\approx 0$ (22)
$\displaystyle\frac{\partial y^{r}_{\text{zmp}}}{\partial{\phi^{r}}}$
$\displaystyle\approx 0,~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\frac{\partial
y^{r}_{\text{zmp}}}{\partial{\theta^{r}}}\leq-\frac{M_{z}}{M}<0.$

∎

With Proposition 2 introduced and graphically illustrated in Figure 3(b), we
claim that as long as the rectangle derived above is contained within the
convex support polygon, the quasi-static mobile motion of the robot is
guaranteed to be stable.

###  IV-D Traction Optimization

The results obtained thus far focused on the rollover stability constraint for
the robot. However, relocating on 3-D 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 $\boldsymbol{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 $\boldsymbol{f}_{f}^{0}$,
where $\|\boldsymbol{f}_{f}^{0}\|_{2}\leq\|\mu\boldsymbol{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:

$\|\boldsymbol{f}_{f}^{0}+\boldsymbol{f}_{f}^{a}\|_{2}\leq\|\mu\boldsymbol{f}_{n}\|_{2},$
(23)

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

$\displaystyle\|\boldsymbol{f}_{f}^{0}\|_{2}+\|\boldsymbol{f}_{f}^{a}\|_{2}$
$\displaystyle\leq\|\mu\boldsymbol{f}_{n}\|_{2}$ (24)
$\displaystyle\|\boldsymbol{f}_{f}^{a}\|_{2}$
$\displaystyle\leq\|\mu\boldsymbol{f}_{n}\|_{2}-\|\boldsymbol{f}_{f}^{0}\|_{2}$
$\displaystyle f_{f}^{avail}$
$\displaystyle=\|\mu\boldsymbol{f}_{n}\|_{2}-\|\boldsymbol{f}_{f}^{0}\|_{2}.$

Note that (24) gives the magnitude of traction force $\boldsymbol{f}_{f}^{a}$
a more restrictive bound than (23) 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 $\boldsymbol{f}_{n}$ and
$\boldsymbol{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:

$\displaystyle\|\mu\boldsymbol{f}_{n}\|_{2}^{L}=\mu
f_{n}\frac{r}{l+r},~{}~{}~{}\|\mu\boldsymbol{f}_{n}\|_{2}^{R}=\mu
f_{n}\frac{l}{l+r}.$ (25)

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

$\displaystyle\|\boldsymbol{f}_{f}^{0}\|_{2}^{L}=f_{f}^{0}\frac{r}{l+r},~{}~{}~{}\|\boldsymbol{f}_{f}^{0}\|_{2}^{R}=f_{f}^{0}\frac{l}{l+r}.$
(26)

The amount of minimum available traction force on each side can then be found
using (24) by subtracting (26) form (25):

$\displaystyle f_{f}^{avail,L}\\!=\\!(\mu
f_{n}-f_{f}^{0})\frac{r}{l+r},~{}f_{f}^{avail,R}\\!=\\!(\mu
f_{n}-f_{f}^{0})\frac{l}{l+r}.$ (27)

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

$\max_{p_{zmp}}\min(f_{a}^{avail,L}(\boldsymbol{p}_{zmp}),f_{a}^{avail,R}(\boldsymbol{p}_{zmp})).$
(28)

In order to achieve (28), $l=r$ allows available friction forces in (27) to be
equal on both wheels.

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 Figure 4. With traction optimization, the robot’s
path in Figure 4(a) is smoother when compared to that in Figure 4(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.

(a) Path generated with optimized traction.

(b) Path generated without optimized traction.

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

##  V Simulation Results

In order to showcase the importance of ZMP constraints in mobile manipulator
trajectory planning, and the effectiveness of our formulations, the trajectory
planning framework is applied to a feller buncher machine. In timber
harvesting scenarios, the purpose of a feller buncher is to cut trees, grip
them, drop them off, and relocate to another spot to continue performing these
tasks. The machine is operated throughout the year, in greatly varying terrain
conditions and often on slopes of different grades. In this section, the local
manipulation capability of the framework (per Section III) will be
demonstrated by an example where the machine tries to relocate a heavy tree on
a slope. Then, the reconfigurable relocation capability (per Section IV) will
be demonstrated by commanding the machine to relocate on a test terrain
without carrying a tree.

###  V-A Kinematics Model of a Feller Buncher

The feller buncher machine is modeled after the Tigercat 855E, and can be
described with the schematic diagram in Figure 1. The machine is considered to
be holding onto a tree in the manipulation planning example, and the machine
and the tree’s geometric and mass parameters are summarized in Table II. The
machine’s support polygon is a $3.23~{}\text{m }\\!\\!\\!\times 5~{}\text{m}$
rectangle.

| $a_{i}$ | $\alpha_{i}$ | $d_{i}$ | $\theta_{i}$
---|---|---|---|---
1 | $0$ | $0$ | $d_{1}$ | $q_{1}$
2 | $0$ | $q_{2}$ | $0$ | $0$
3 | $0$ | $q_{3}$ | $d_{3}$ | $0$
4 | $0$ | $q_{4}$ | $d_{4}$ | $0$
5 | $0$ | $0$ | $d_{5}$ | $q_{5}$
TABLE I: Denavit-Hartenberg parameters table of the feller buncher.

The optimal control results in this section are achieved by running the
nonlinear optimal control solver GPOPS [34] under MATLAB environment on a
Windows desktop with Intel Core i7-4770 3.40 GHz processor.

###  V-B Mapping from Simplified Model to Full Model

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

$[q_{3},q_{4},q_{5}]^{T}=[-\pi-2q_{2},\dfrac{\pi}{2}+q_{2},\text{constant}]^{T},$
(29)

and the corresponding relationship between joint accelerations follows:

$[\ddot{q}_{3},\ddot{q}_{4},\ddot{q}_{5}]^{T}=[-2\ddot{q}_{2},\ddot{q}_{2},0]^{T}.$
(30)

Defining the lengths of link 2 and link 4 as $l_{2}$ and $l_{4}$,
respectively, the relationship between $d$ and $q_{2}$ can be written as:

$q_{2}=\sin^{-1}(\frac{-d}{2l_{2}}).$ (31)

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

$\displaystyle\dot{q}_{2}$
$\displaystyle\\!=\\!\dfrac{-\dot{d}}{2l_{2}\sqrt{1-\frac{d^{2}}{4l_{2}^{2}}}}$
(32) $\displaystyle\ddot{q}_{2}$
$\displaystyle=-\dfrac{\ddot{d}}{2l_{2}\sqrt{1-\frac{d^{2}}{4l_{2}^{2}}}}-\dfrac{d\dot{d}^{2}}{8l_{2}^{3}\sqrt{1-\frac{d^{2}}{4l_{2}^{2}}}^{3}}.$

Therefore, using the mapping provided in (29) and (30), and the relation
between joint angles and accelerations as given in (31) and (32), the ZMP
location of the full kinematics model can be written as
$\boldsymbol{p}_{\text{zmp}}(\tilde{\boldsymbol{x}},\tilde{\boldsymbol{u}})$.

Link Number | $0$ | $1$ | $2$ | $3$ | $4$ | $5$ | Tree
---|---|---|---|---|---|---|---
$\text{Length, }l_{i}\text{ (m)}$ | $1.60$ | $0.96$ | $3.27$ | $3.27$ | $0.458$ | $0.677$ | $8$
$\text{Mass, }m_{i}\text{ (kg)}$ | $13000$ | $5000$ | $2000$ | $1000$ | $50$ | $2600$ | $4000$
TABLE II: Mobile manipulator parameters for the test case.

###  V-C 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:

$\displaystyle\boldsymbol{q}(t_{0})$
$\displaystyle=[0,-\pi/6,-2\pi/3,\pi/6,-\pi/2]^{T}\text{ rad}$ (33)
$\displaystyle\dot{\boldsymbol{q}}(t_{0})$
$\displaystyle=[0,0,0,0,0]^{T}\text{ rad/s},$

and the desired final joint angles and velocities are:

$\displaystyle\boldsymbol{q}(t_{f})$
$\displaystyle=[\pi,-\pi/6,-2\pi/3,\pi/6,-\pi/2]^{T}\text{ rad}$ (34)
$\displaystyle\dot{\boldsymbol{q}}(t_{f})$
$\displaystyle=[0,0,0,0,0]^{T}\text{ rad/s}.$

The joint rate and acceleration constraints are:

$\displaystyle-\frac{\pi}{4}$
$\displaystyle\leq\dot{q}_{i}\leq\frac{\pi}{4}\text{
rad/s}~{}~{}~{}~{}~{}\forall~{}i$ $\displaystyle-\frac{\pi}{2}$
$\displaystyle\leq\ddot{q}_{i}\leq\frac{\pi}{2}\text{
rad/s}^{2}~{}~{}~{}~{}\forall~{}i.$

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

(a) Joint accelerations.

(b) Joint velocities.

(c) Joint angles.

Figure 6: 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.

The ZMP loci of motions generated using the full kinematics optimal trajectory
planning formulation (5), the simplified formulation (8), and the classic
phase plane method that considers joint angle, rate, and acceleration limits
without stability constraints [15] are presented in Figure 5. 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 (5) and (8) 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. The unsafe motion generated by the phase
plane method takes 4.5 seconds to complete; the safe motion generated by
solving the OCP (5) also takes 4.5 seconds to complete; and the safe motion
generated by (8) takes 5.0 seconds to complete.

Figure 7: ZMP locus throughout mobile manipulation task.

For the solutions of (5) and (8), the planned joint accelerations, rates, and
angles are displayed in Figures 6(a), 6(b), and 6(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 (5) is more than 5 hours, while the
computation time to solve (8) is 1.34 seconds.

Figure 8: Base and joint acceleration of the mobile manipulator.

###  V-D Simplified Kinematics Model Manipulation-coordinated Base
Acceleration Planning

In the second test case, we consider the feller buncher to be situated on a
slope that results in a 15-degree vehicle pitch towards the front and a
15-degree roll to the left. The machine’s manipulator arm has to complete the
same motion as in the first test case while the mobile base begins at rest,
backs up, and goes back to rest during the manipulation. This mobile
manipulation task corresponds to a typical occurrence during timber harvesting
where the feller buncher has cut a tree on a slope but needs to back up in
order to find space to place the tree down. With all joint constraints the
same, the mobile base’s acceleration has to satisfy
$\ddot{y}_{0}\in[-2,2]~{}\text{m/s}^{2}$, and its velocity has to satisfy
$\dot{y}_{0}\in[-10,10]~{}\text{m/s}$.

It is after obtaining a solution for the optimal trajectory of the
manipulator, the safe base acceleration bound is found using equation (24).
Figure 7 shows that throughout the task, the ZMP locus never exceeds the edges
of the support polygon. Time history of the mobile manipulator’s motion is
shown in Figure 8. The top plot in Figure 8 shows that the mobile base
accelerated and decelerated at its maximum magnitude allowed by the
corresponding state constraints and the acceleration bounds to achieve time-
optimal arrival at the destination. The manipulation motion takes 4.5 seconds
to complete and the computation time for arm manipulation is 1.29 seconds,
while the time for calculating the base acceleration bound and base trajectory
planning is negligible since analytical solutions (24) exist.

Base Attitude | Success Rate
---|---
0-degree Roll | $0.989$
15-degree Roll | $0.988$
20-degree Roll | $0.796$
30-degree Roll | $0.406$
TABLE III: Trajectory planning success rate of four base attitudes

###  V-E 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 (33) and (34). The
trajectory planning success rate is presented in Table III. The distribution
of successful trajectory planning times of the four base attitudes is
presented in Figure 9.

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
Figure 9 shows that the dimension reduction applied in the simplified model
allows the computation time to be low enough for online guidance in most
cases.

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

###  V-F Mobile Relocation Planning

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

$s_{z}=10\cos(\frac{\sqrt{{s_{x}}^{2}+{s_{y}}^{2}}}{10})$ (35)

is created. To formulate the OCP (5) of this example, the terrain surface
function (35) is integrated into the feller buncher’s kinematics (3).

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 Figure 10). Denoting
the base’s linear velocity as $v$, the initial and final states for the
machine’s base and arm are:

$\displaystyle{}^{T}(t_{0})$ $\displaystyle=[0,0,0,0,0]^{T}$
$\displaystyle\boldsymbol{q}(t_{0})$
$\displaystyle=[0,-\pi/6,-2\pi/3,\pi/6,-\pi/2]^{T}$
$\displaystyle\dot{\boldsymbol{q}}(t_{0})$ $\displaystyle=[0,0,0,0,0]^{T}$
$\displaystyle[x_{0},y_{0},v,\bar{\psi},\dot{\bar{\psi}}]^{T}(t_{f})$
$\displaystyle=[0,63,0,\text{free},0]^{T}$
$\displaystyle\dot{\boldsymbol{q}}(t_{f})$ $\displaystyle=[0,0,0,0,0]^{T},$

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:

$\displaystyle 0\leq$ $\displaystyle v\leq 2.78~{}~{}\text{m}/\text{s}$
$\displaystyle-1\leq$ $\displaystyle u_{a}\leq 1~{}~{}\text{m}/\text{s}^{2}$
$\displaystyle-2\leq$ $\displaystyle\dot{\psi}\leq 2~{}~{}\text{rad}/\text{s}$
$\displaystyle-\frac{2}{3}\leq$ $\displaystyle
u_{\psi}\leq\frac{2}{3}~{}~{}\text{rad}/\text{s}^{2}.$

Algorithm 1 first generates a quasi-static path with guaranteed stability for
continuous motion for the machine indicated by the blue crosses in Figure 10.
The resulting path requires the machine to reconfigure its cabin angle
$q_{1}$, as shown by Figures 10(a) and 10(b), to shift the ZMP so the machine
does not rollover towards the front as it is descending the slope. The
sampling point where this configuration change happens is highlighted in
Figure 10(c) by a red circle.

(a) Machine configuration before change.

(b) Machine configuration after change.

(c) Overview of the terrain, quasi-static path generated from the sampling-
based planner (blue cross), and trajectory (red line).

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

Then, based on the quasi-static path and configuration change generated by
Algorithm 1, an initial guess is given to GPOPS[34], a nonlinear OCP 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 OCP
solver, the machine is constrained to be static at each sampling point but is
given small constant linear velocity and heading rate when it is between
sampling points to allow dynamic stability constraint satisfaction. The
resulting trajectories of all segments are then combined. The continuous path
of the machine is represented by the red line in Figure 10, the motion
trajectory is shown in Figure 11, and the ZMP trajectory is shown in Figure
12. Note that the machine’s velocity remains 0 in segment 14 due to arm
reconfiguration.

Figure 11: Motion trajectory generated by iteratively solving OCP of each
segment. Figure 12: ZMP trajectory generated by iteratively solving OCP of
each segment.

The result in Figure 11 shows that all state and stability constraint
variables remained within their bounds, indicated by black dashed lines, and
the overall trajectory takes $\approx 180$ seconds to complete. Each motion
segment separated by vertical dashed lines took $\approx 0.3$ seconds to
calculate, and the entire trajectory took 18.30 seconds to plan. This shows
that the framework proposed in this paper has the potential to be implemented
to achieve online dynamically stable mobile manipulation planning.

The resulting ZMP trajectory is shown in Figure 12 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 withth 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$, i.e., the centerline
of the support polygon, during ascent and descent as a result of the traction-
optimized path as mentioned in Section 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 OCP solution scheme is implemented.

For the result shown in Figure 11, more than one trajectory segment can be
treated as a whole and thus used as the initial guess of a new OCP 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$ seconds. When GPOPS cannot converge to a solution within a user
specified time threshold, the solver is terminated and result computed for the
original segment is kept for the machine’s execution. This way, the motion
trajectory can be optimized where possible without forcing the machine to stop
and wait for a result.

The receding horizon scheme enhanced motion trajectory is shown in Figure 13,
with the corresponding ZMP trajectory shown in Figure 14. It can be observed
that, compared to Figure 11, the motion trajectory has become significantly
smoother. The relocation completion time has also been shortened to $\approx
105$ seconds 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 Figure 15. 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 8.78
seconds to complete, while the unsuccessful segments took a total of 14
seconds with the computational time threshold set of 0.5 seconds.

Figure 13: Motion trajectory generated through iteratively solving OCP with
existing motion trajectory and a horizon length of 20 time steps. Figure 14:
ZMP trajectory generated through iteratively solving OCP with existing motion
trajectory and a horizon length of 20 time steps. Figure 15: Joint motion
trajectories of the machine with joint angles shown in red and joint
velocities shown in blue.

##  VI Conclusions and Future Work

To conclude, this paper presented a framework that allows online optimal
mobile manipulation planning to allow for online guidance for a robot to work
safely and efficiently on rough terrain. This is achieved through the
formulation of a trajectory planning problem as an optimal control problem
with an additional dynamic stability constraint. To reduce the problem
dimension, the mobile manipulation task is first divided into two separate
stages: the manipulation stage and the relocation stage. The online trajectory
planning of the manipulation stage is achieved through dimension reduction
with the help of the known inverse kinematics mapping of the robot. Then, to
solve the relocation problem, a sampling-based path planning algorithm that
optimizes slope traction availability and generates results guaranteed to meet
nonlinear dynamic stability constraints when continuously executed is
implemented. The robot path and reconfiguration command generated by the
sampling-based algorithm are then taken to warm start a nonlinear OCP solver.
The resulting trajectory is further optimized and smoothed with the receding
horizon OCP re-planning scheme.

The framework presented in this paper is the first attempt at a task-based
online optimal planning algorithm for a mobile manipulator that accommodates
the nonlinear dynamic stability constraint. Some improvements can still be
made to the framework to be implemented in a wider variety of scenarios. For
example, the terrain map used in this paper only assumes a rough knowledge of
the terrain features. However, as the robot traverses through the terrain,
more detailed terrain features can be captured by its on-board sensors. With a
more detailed local terrain map, a re-planning method or a stability-
guaranteed control law can be developed to help the robot deal with local
terrain complexities.

###### Proof of Theorem 1.

During the execution of a quasi-statically planned path that is made of
straight line segments, the mobile robot’s motion can be decomposed into two
motion primitives: forward and turning. Differentiating static variables and
dynamic variables at the same location using superscript
$\cdot^{\text{static}}$ and $\cdot^{\text{dynamic}}$, the distance between
$\boldsymbol{p}_{zmp}^{\text{dynamic}}$ and
$\boldsymbol{p}_{zmp}^{\text{static}}$ can be characterized using the 2-norm:
$||\boldsymbol{p}_{zmp}^{\text{dynamic}}-\boldsymbol{p}_{zmp}^{\text{static}}||_{2}$.
Here $\boldsymbol{p}_{zmp}^{\text{dynamic}}$ and
$\boldsymbol{p}_{zmp}^{\text{static}}$ can both be found by solving (4).

Under the assumption that straight line segments are small so that the robot’s
attitude change during forward motion is negligible,we have
$\ddot{x}_{i}^{\text{dynamic}}=\ddot{x}_{i}^{\text{static}}=0$, and
$\ddot{z}_{i}^{\text{dynamic}}=\ddot{z}_{i}^{\text{static}}=0$ $\forall
i\in\\{0,\cdots,n\\}$. Therefore,
$x_{zmp}^{\text{dynamic}}=x_{zmp}^{\text{static}}$. As a result, we get the
following:

$\displaystyle\left|\left|\boldsymbol{p}_{zmp}^{\text{dynamic}}-\boldsymbol{p}_{zmp}^{\text{static}}\right|\right|_{2}$
$\displaystyle=\left|y_{zmp}^{\text{dynamic}}-y_{zmp}^{\text{static}}\right|$
(36)
$\displaystyle=\left|\frac{\sum_{i}m_{i}\ddot{y}_{i}^{\text{dynamic}}z_{i}}{\sum_{i}m_{i}g_{z}}\right|$
$\displaystyle\leq\left|\frac{\sum_{i}m_{i}z_{i}}{\sum_{i}m_{i}g_{z}}\right|\left|\ddot{y}^{\text{dynamic}}\right|$
$\displaystyle=\left|\frac{\sum_{i}m_{i}z_{i}}{\sum_{i}m_{i}g_{z}}\right|\left|u_{a}\right|$

Derivation in (36) shows that, the distance between
$\boldsymbol{p}_{zmp}^{\text{static}}$ and
$\boldsymbol{p}_{zmp}^{\text{dynamic}}$ during forward motion is
proportionally bounded by the magnitude of input $u_{a}$.

For turning motion, every link on the robot goes through a rotation along the
$z_{0}$-axis that can be described by the following rotation matrix:

$R_{yaw}=\begin{bmatrix}\cos{\psi}&-\sin{\psi}&0\\\ \sin{\psi}&\cos{\psi}&0\\\
0&0&1\end{bmatrix}.$ (37)

With the mobile base fixed during the turn, the linear acceleration of each
link’s center of mass can be written as:

$\\!\begin{bmatrix}\ddot{x}_{i}\\\ \ddot{y}_{i}\\\
\ddot{z}_{i}\end{bmatrix}\\!\\!\\!=\underbrace{\\!\\!\\!\begin{bmatrix}-\cos{\psi}\dot{\psi}^{2}-\sin{\psi}u_{\psi}&\sin{\psi}\dot{\psi}^{2}-\cos{\psi}u_{\psi}&0\\\
-\sin{\psi}\dot{\psi}^{2}+\cos{\psi}u_{\psi}&-\cos{\psi}\dot{\psi}^{2}-\sin{\psi}u_{\psi}&0\\\
0&0&0\end{bmatrix}\\!\\!\\!}_{\ddot{R}_{yaw}}\begin{bmatrix}x_{i}\\\ y_{i}\\\
z_{i}\end{bmatrix}\\!\\!\\!.$ (38)

Then, the distance can be written as:

$\displaystyle\left|\left|\boldsymbol{p}_{zmp}^{\text{dynamic}}\\!\\!-\\!\\!\boldsymbol{p}_{zmp}^{\text{static}}\right|\right|_{2}$
(39)
$\displaystyle=\left|\left|\left[x_{zmp}^{\text{dynamic}}-x_{zmp}^{\text{static}},y_{zmp}^{\text{dynamic}}-y_{zmp}^{\text{static}}\right]^{T}\right|\right|_{2}$
$\displaystyle=\left|\left|\left[\frac{\sum_{i}m_{i}\ddot{x}_{i}^{\text{dynamic}}\\!z_{i}}{\sum_{i}m_{i}g_{z}},\frac{\sum_{i}m_{i}\ddot{y}_{i}^{\text{dynamic}}z_{i}}{\sum_{i}m_{i}g_{z}}\right]^{T}\right|\right|_{2}$
$\displaystyle\leq\left|\frac{\sum_{i}m_{i}\ddot{x}_{i}^{\text{dynamic}}\\!z_{i}}{Mg_{z}}\right|+\left|\frac{\sum_{i}m_{i}\ddot{y}_{i}^{\text{dynamic}}\\!z_{i}}{Mg_{z}}\right|$
$\displaystyle=\left|\left[\frac{m_{0}z_{0}}{Mg_{z}},\cdots,\frac{m_{n}z_{n}}{Mg_{z}}\right]\left[\ddot{x}_{0}^{\text{dynamic}},\cdots,\ddot{x}_{n}^{\text{dynamic}}\right]^{T}\right|$
$\displaystyle~{}~{}~{}~{}+\left|\left[\frac{m_{0}z_{0}}{Mg_{z}},\cdots,\frac{m_{n}z_{n}}{Mg_{z}}\right]\left[\ddot{y}_{0}^{\text{dynamic}},\cdots,\ddot{y}_{n}^{\text{dynamic}}\right]^{T}\right|$
$\displaystyle\leq\left|\left|\left[\frac{m_{0}z_{0}}{Mg_{z}},\cdots,\frac{m_{n}z_{n}}{Mg_{z}}\right]\right|\right|_{2}\bigg{(}\left|\left|\left[\ddot{x}_{0}^{\text{dynamic}},\cdots,\ddot{x}_{n}^{\text{dynamic}}\right]\right|\right|_{2}$
$\displaystyle~{}~{}~{}~{}+\left|\left|\left[\ddot{y}_{0}^{\text{dynamic}},\cdots,\ddot{y}_{n}^{\text{dynamic}}\right]\right|\right|_{2}\bigg{)}$
$\displaystyle\leq
2\left|\left|\left[\frac{m_{0}z_{0}}{Mg_{z}},\cdots,\frac{m_{n}z_{n}}{Mg_{z}}\right]\right|\right|_{2}\sum_{i}(|x_{i}|\dot{\psi}^{2}+|x_{i}||u_{\psi}|+$
$\displaystyle|y_{i}|\dot{\psi}^{2}+|y_{i}||u_{\psi}|).$

From inequality (36) and (39), it can be inferred that there exist
trajectories of $u_{a}$ and $u_{\phi}$ such that,
$\forall~{}t\in[t_{0},t_{f}]$, $v>0$, $\dot{\psi}>0$, and
$\left|\left|\boldsymbol{p}_{zmp}^{\text{dynamic}}\\!\\!-\\!\\!\boldsymbol{p}_{zmp}^{\text{static}}\right|\right|_{2}\leq\sigma$
for arbitrarily small $\sigma>0$. Hence, there always exists a trajectory of
$\boldsymbol{u}$ such that $\dot{\tau}>0$ over the unit interval $I$.

∎

## Acknowledgement

This work was supported by the National Sciences and Engineering Research
Council (NSERC) Canadian Robotics Network (NCRN), the McGill Engineering
Doctoral Awards and Summer Undergraduate Research in Engineering (SURE)
programs. The authors would also like to thank Centre de Formation
Professionnelle Mont-Laurier for their help during field work.

## References

  * [1] Q. Huang, K. Tanie, and S. Sugano.  Coordinated motion planning for a mobile manipulator considering stability and manipulation.  The International Journal of Robotics Research, 19(8):732–742, 2000\.
  * [2] T. M. Howard and A. Kelly.  Optimal rough terrain trajectory generation for wheeled mobile robots.  The International Journal of Robotics Research, 26(2):141–166, 2007\.
  * [3] E. G. Papadopoulos and D. A. Rey.  A new measure of tipover stability margin for mobile manipulators.  In Proceedings of IEEE International Conference on Robotics and Automation, pages 3111–3116, Minneapolis, MN, USA, April 1996. IEEE.
  * [4] O. Lindroos, P. La Hera, and C. Haggstrom.  Drivers of advances in mechanized timber harvesting - a selective review of technological innovation.  Croation Journal for Engineering, 38(2):243–258, 2017.
  * [5] K. Chen, M. Kamezaki, T. Katano, T. Kaneko, K. Azuma, T. Ishida, M. Seki, K. Ichiryu, and S. Sugano.  Compound locomotion control system combining crawling and walking for multi-crawler multi-arm robot to adapt unstructured and unknown terrain.  Robomech Journal, 5(2), 2018.
  * [6] D. K. Pai and L.-M. Reissell.  Multiresolution rough terrain motion planning.  IEEE Transactions on Robotics and Automation, 14(1):19–33, 1998\.
  * [7] S.S. Ge and Y.J. Cui.  Dynamic motion planning for mobile robots using potential field method.  Autonomous Robots, 13:207–222, 2002.
  * [8] H. G. Gibson, K. C. Elliott, and S. P. E. Persson.  Side slope stability of articulated-frame logging tractors.  Journal of Terramechanics, 8(2):65–79, 1971.
  * [9] A. Diaz-Calderon and A. Kelly.  On-line stability margin and attitude estimation for dynamic articulating mobile robots.  The International Journal of Robotics Research, 24(10):845–866, 2005.
  * [10] M. Mosadeghzad, D. Naderi, and S. Ganjefar.  Dynamic modeling and stability optimization of a redundant mobile robot using a genetic algorithm.  Robotica, 30:505–514, 2012.
  * [11] N. Bouton, R. Lenain, B. Thuilot, and J. Fauroux.  A rollover indicator based on the prediction of the load transfer in presence of sliding: application to an all terrain vehicle.  In Proceedings of the 2007 IEEE International Conference on Robotics and Automation, pages 1158–1163, Roma, Italy, April 2007. IEEE.
  * [12] N. Bouton, R. Lenain, B. Thuilot, and P. Martinet.  A new device dedicated to autonomous mobile robot dynamic stability: Application to an off-road mobile robot.  In 2010 IEEE International Conference on Robotics and Automation, pages 3813–3818, Anchorage, AK, USA, May 2010. IEEE.
  * [13] D. Denis, B. Thuilot, and R. Lenain.  Online adaptive observer for rollover avoidance of reconfigurable agricultural vehicles.  Computers and Electronics in Agriculture, 126:32–43, 2016.
  * [14] M. Vukobratovic and J. Stepanenko.  On the stability of anthropomorphic sytems.  Mathematical Biosciences, 15:1–37, 1972.
  * [15] K. G. Shin and N. D. McKay.  Minimum-time control of robotic manipulators with geometric path constraints.  IEEE Transactions on Automatic Control, 30(6):531–541, 1985.
  * [16] J. E. Bobrow, S. Dubowsky, and J. S. Gibson.  Time-optimal control of robotic manipulators along specified paths.  The International Journal of Robotics Research, 4(3):3–17, 1985\.
  * [17] H. Seraji.  A unified approach to motion control of mobile manipulators.  International Journal of Robotics Research, 17(2):107–118, 1998\.
  * [18] R. Verschueren, N. van Duijkeren, J. Swevers, and M. Diehl.  Time-optimal motion planning for n-dof robot manipulators using a path-parametric system reformulation.  In 2016 American Control Conference, Boston, MA, USA, July 2016\. IEEE.
  * [19] V. Padois, J.-Y. Fourquet, and P. Chiron.  From robotic arms to mobile manipulation: on coordinated motion schemes.  In 2nd International Innovative Production Machines and Systems Conference, European Union, July 2006.
  * [20] D. Chen, Z. Liu, and G. V. Wichert.  Uncertainty-aware arm-base coordinated grasping strategies for mobile manipulation.  Journal of Intelligent Robotics Systems, 80(1):205–223, 2015.
  * [21] G. B. Avanzini, A. M. Zanchettin, and P. Roco.  Constraint-based model predictive control for holonomic mobile manipulators.  In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1473–1479, Hamburg, Germany, September 2015. IEEE.
  * [22] S. Sugano, Q. Huang, and I. Kato.  Stability criteria in controlling mobile robotic systems.  In Proceedings of the 1993 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 832–838, Yokohama, Japan, July 1993. IEEE.
  * [23] K. M. E. Dine, J. Corrales-Ramon, Y. Mezouar, and J. Fauroux.  A unified mobile manipulator control for online tip-over avoidance based on zmp disturbance observer.  In 2018 IEEE International Conference on Robotics and Biomimetics, pages 1437–1443, Kuala Lumpur, Malaysia, Decemner 2018. IEEE.
  * [24] J. Kim, W. K. Chung adn Y. Youm, and B. H. Lee.  Real-time zmp compensation method using null motion for mobile manipulators.  In 2002 IEEE International Conference on Robotics and Automation, pages 1967–1972, Wangshington, DC, USA, May 2002. IEEE.
  * [25] D. Choi and J. Oh.  Zmp stabilization of rapid mobile manipulator.  In 2012 IEEE International Conference on Robotics and Automation, pages 883–888, Saint Paul, MN, USA, May 2012. IEEE.
  * [26] S. Lee, M. Leibold, M. Buss, and F. C. Park.  Rollover prevention of mobile manipulators using invariance control and recursive analytic zmp gradients.  Advanced Robotics, 26(11):1317–1341, 2012.
  * [27] A. Mohammadi, I. Mareels, and D. Oetomo.  Model predictive motion control of autonomous forklift vehicles with dynamics balance constraint.  In 2016 14th International Conference on Control, Automation, Robotics, and Vision, pages 1–6, Phuket, Thailand, Nov. 2016. IEEE.
  * [28] E. Papadopoulos and S. Sarkar.  The dynamics of an articulated forestry machine and its applications.  In Proceedings of the 1997 IEEE International Conference on Robotics and Automation, pages 323–328, Albuquerque, New Mexico, April 1997\. IEEE.
  * [29] S. Westerberg, I. R. Manchester, U. Mettin, P. L. Hera, and A. Shiriaev.  Virtual environment teleoperation of a hydraulic forestry crane.  In Proceedings of the 2008 IEEE International Conference on Robotics and Automation, pages 4049–4054, Pasadena, CA, USA, May 2008. IEEE.
  * [30] P. L. Hera, U. Mettin, S. Westerberg, and A. S. Shiriaev.  Modeling and control of hydraulic rotary actuators used in forestry cranes.  In 2009 IEEE International Conference on Robotics and Automation, pages 1315–1320, Kobe, Japan, May 2009. IEEE.
  * [31] D. O. Morales, S. Westerberg, P. X. La Hera, L. Freidovich, and A. S. Shiriaev.  Increasing the level of automation in the forestry logging process with crane trajectory planning and control.  Journal of Field Robotics, 31(3):343–363, 2014.
  * [32] D. O. Morales, P. L. Hera, S. Westerberg, L. B. Freidovich, and A. S. Shiriaev.  Path-constrained motion analysis: An algorithm to understand human performance on hydraulic manipulators.  IEEE Transactions on Human-Machine Systems, 45(2):187–199, 2015\.
  * [33] J. Song and I. Sharf.  Time optimal motion planning with zmp stability constraint for timber manipulation.  In 2020 IEEE International Conference on Robotics and Automation (ICRA), pages 4934–4940, Paris, France, May 2020. IEEE.
  * [34] M. A. Patterson and A. V. Rao.  Gpops-ii: A matlab software for solving multiple-phase optimal control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming.  ACM Transactions on Mathematical Software, 41(1):1–37, 2014.
  * [35] A. Reiter, A. M¸ller, and H. Gattringer.  On higher order inverse kinematics methods in time-optimal trajectory planning for kinematically redundant manipulators.  IEEE Transactions on Industrial Informatics, 14(4):1681–1690, 2018\.
  * [36] S. M. LaValle and J. J. Kuffner Jr.  Randomized kinodynamic planning.  The International Journal of Robotics Research, 20(5):378–400, 2001\.