5 Kinematics, Dynamics, and Control of Mobile Robot Manipulators

5 Kinematics, Dynamics, and Control
of Mobile Robot Manipulators
5.1 Kinematics of Wheeled Mobile Manipulators:

Non-Holonomic Constraints
A mobile robot manipulator is a mobile platform hosting a robotic manipulator. The

kinematics of such manipulators involve consideration of several issues that are quite
unique to them. The primary feature is that wheeled mobile platforms are subject to non-
holonomic constraints. These constraints have a significant impact on the coordination
strategy as well as the dynamic interaction between the platform and the manipulator. In
turn, this has a bearing on the control and path planning of mobile robot manipulators.
A key feature of robot manipulators with non-holonomic constraints is that the number
of independent position coordinates exceeds the number of independent velocities that
may be used to describe the velocity kinematics and the dynamics of the manipulator.
Normally, feedback control laws are designed to asymptotically stabilize the equilibrium
position of a mechanical system. However, when non-holonomic constraints are present,
Brockett [1] showed that a time-invariant full state feedback control law is inadequate if
one wishes to asymptotically stabilize the equilibrium position of the mechanical system.
To stabilize systems with non-holonomic constraints, a number of approaches, including
time-varying control laws, piecewise smooth or discontinuous control laws, and com-
binations of these, have been proposed for the stabilization of non-holonomic control
systems to equilibrium points, as outlined in a survey paper by Kolmanovsky and
McClamroch [2], and in Godhavn and Egeland [3], Samson [4], Astolfi [5], Hespanha
[6], and Aguiar and Pascoal [7]. The stability of such systems may be established using a
Lyapunov approach, as outlined by Ye, Michel, and Hou [8].
Every mechanical system is subject to constraints. A standard technique that is
adopted is to solve for some of the coordinates that arise in the constraint equations,
in terms of the rest of the coordinates. Generally, if j constraints are present, one can
solve for j of the coordinates in terms of the remaining coordinates, and eliminate them
from the equations of motion. Some constraint equations, known as Pfaffian constraints,
are constraints on the generalized velocities. However, they cannot be expressed as a
differential of a function of the generalized position coordinates; consequently, the
constraints are said to be non-integrable. Thus, non-holonomic constraints are expressed
in terms of non-integrable functions of the generalized velocities.
A wheeled mobile platform is shown in Figure 5.1, and it is free to move in the
horizontal plane. The mobile platform consists of one front steering wheel and two
206
Downloaded from https://www.cambridge.org/core. University of Sussex Library, on 29 Apr 2019 at 02:04:19, subject to the Cambridge Core terms of use, available at
https://www.cambridge.org/core/terms. https://doi.org/10.1017/9781108525404.006
5.1 Kinematics of Wheeled Mobile Manipulators: Non-Holonomic Constraints 207
d
CM
P0
f
x
2r
Figure 5.1 Typical mobile platform.
independent rear wheels driven by two motors. Since the platform is driven by the two
driving wheels which are to roll without slipping, the velocity constraint is non-
holonomic. A kinematic constraint equation can be written, relating the translation
velocities in the Cartesian coordinates and the rotational angular velocity about an axis
normal to the horizontal plane, for any point on the platform. Thus, for the center of
mass of the platform, the constraint is expressed in terms of the virtual displacements as,
δy cos ϕ δx sin ϕ ¼ d δϕ: (5.1)
In terms of velocities, one has,

_
y_ cos ϕ x_ sin ϕ ¼ d ϕ: (5.2)
In matrix notation,
T
½ sin ϕ cos ϕ d x_ y_ ϕ_ Cq_ ¼ 0: (5.3)
One method of eliminating the constraints is to determine the null space of the
constraint matrix C. The null space of the constraint matrix C can be written by
inspection as,
2 3
cos ϕ sin ϕ
NC ¼ 4 sin ϕ cos ϕ 5: (5.4)
0 1=d
So it follows that,
CNC ¼ 0: (5.5)
If we now introduce the transformation,

q_ ¼ NC v, (5.6)
it follows that the constraints are satisfied. Moreover, it is possible to interpret the
components of the vector v as the forward velocity and the tangential velocity of the
mobile vehicle, respectively.
208 Kinematics, Dynamics, and Control of Mobile Robot Manipulators
Yet if there is a need to establish the complete dynamics of the mobile platform, it is
essential that we write the complete expression for the kinetic energy and generalized
forces driving it. Thus, the kinetic energy is,
1 1
T ¼ m x_ 2 þ y_ 2 þ I ϕ_ 2 : (5.7)
2 2
The Euler-Lagrange equations are,
2 32 3 2 3
m 0 0 x€ Qx
6 76 7 6 Q 7
4 0 m 0 y€ ¼
54 5 4 y 5 CT λ, (5.8)
0 0 I ϕ€ Qϕ
which is expressed in matrix notation as,
q ¼ Q CT λ:
m€ (5.9)
The effect of the constraints on the generalized forces acting on the platform is included
by the term CT λ, where λ is an unknown parameter, known as a Lagrange multiplier.
When the virtual work by the generalized forces is considered,
02 3 1
Qx
B6 7 C
δW ¼ δqT @4 Qy 5 CT λA ¼ Qx Qy Qϕ δq ¼ Qδq, (5.10)
Qϕ
it is observed that the forces of constraint do not contribute to it. Differentiating the
equation defining the coordinate transformation,
q€ ¼ NC v_ þ N_ C v, (5.11)
and projecting the equations of motion into the null space of the constraint matrix, the
equations of motion can be expressed as,
2 3 2 3 2 3
m 0 0 Qx Qx
T6 7 _
T6 7 T6 7
NC 4 0 m 0 5 NC v_ þ N C v ¼ NC 4 Qy 5 NC C λ ¼ NC 4 Qy 5:
T T
(5.12)
0 0 I Qϕ Qϕ
In matrix notation, it is expressed as,

NTC m NC v_ þ N_ C v ¼ NTC Q NTC CT λ ¼ NTC Q: (5.13)
Thus, the unknown Lagrange multiplier has been successfully eliminated and the
equations of motion may now be solved, provided the generalized forces driving and
restraining the platform are known.
This simple example demonstrates the process one must adopt to eliminate the non-
holonomic constraints. In the case of a mobile platform additionally hosting a robotic
manipulator, the interaction between the degrees of freedom of the manipulator and
constraints must be carefully considered.
5.3 Dynamics of Wheeled Mobile Manipulators 209
5.2 Dynamics of Manipulators on a Moving Base
Consider a manipulator with at least two revolute joints on a moving platform. It is

assumed for simplicity that it is a two-link serial manipulator with joint variables θ1 and
θ2 . There are two additional degrees of freedom. In matrix notation, the constraint
equation is modified to,
T
½ sin ϕ cos ϕ d 0 0 x_ y_ ϕ_ θ_ 1 θ_ 2 Cq_ C½ q1 q2 , (5.14)
where the constraint matrix C has the form,
C ¼ ½ C11 0 , q1 ¼ ½ x y ϕ T and q2 ¼ ½ θ1 θ 2 T : (5.15)
The null space of the constraint matrix C can be written by inspection as,
2 3
cos ϕ sin ϕ
N11 0 6 7
NC ¼ , where N11 ¼ 4 sin ϕ cos ϕ 5: (5.16)
0 I22
0 1=d
The transformation of coordinates needed to meet the constraints is,

T
q_ 1 ¼ N11 v, q_ ¼ NC vT qT2 (5.17)
,
where v is the same pair of coordinates as in the preceding section.
The coupled Euler-Lagrange equations for the mobile platform and the manipulator
are,
T
HðqÞ€ q þ Dðqq_ Þq_ þ GðqÞ ¼ QT τT þ Cλ: (5.18)
Eliminating q€1 results in,

" #
v_ v N T
Q
NTC HðqÞNC þ NC Dðqq_ ÞNC þ HðqÞN_ C
T
þ NC GðqÞ ¼
T 11
: (5.19)
q€2 q_ 2 τ
Although one still has functions of the generalized coordinates q in the equations of
motion, they no longer include the unknown Lagrange multiplier, which has been
eliminated.
5.3 Dynamics of Wheeled Mobile Manipulators
The case of a mobile platform that is differentially driven by two motors, each powering
one of the two rear wheels independently, is considered. The case of the rolling contact
between the wheels and the surface of the ground is considered first. Both wheels are
assumed to be of the same radius r w and the wheel separation distance is assumed to be
2b. Assuming pure rolling contact, the velocities of the left and right wheels are,
respectively, given by,
vLw ¼ r w θ_ L , vRw ¼ r w θ_ R: : (5.20)
These are the primary non-holonomic constraints.

The Cartesian velocity components at point P0 are, respectively, given by,
1 rw _
x_ 0 ¼ ðvLw þ vRw Þ cos ϕ ¼ θ L þ θ_ R cos ϕ, (5.21)
2 2
1 rw _
y_ 0 ¼ ðvLw þ vRw Þ sin ϕ ¼ θ L þ θ_ R sin ϕ: (5.22)
2 2
The angular rotation rate of the mobile platform is,
1 rw _
ϕ_ ¼ ðvRw vLw Þ ¼ θ R θ_ L : (5.23)
b 2b
The Cartesian velocity components at the center of mass of the platform are, respect-
ively, given by,
rw _ rw _
x_ C ¼ x_ 0 dϕ_ sin ϕ ¼ θ L þ θ_ R cos ϕ d θ R θ_ L sin ϕ, (5.24)
2 2b
rw _ rw _
y_ C ¼ y_ 0 þ dϕ_ cos ϕ ¼ θ L þ θ_ R sin ϕ þ d θ R θ_ L cos ϕ: (5.25)
2 2b
It is assumed that one of the manipulator base joint axes intersects the horizontal plane
of the mobile at the point B and that this point is located at distance L, ahead of the
center of mass of the platform along the longitudinal axis of symmetry. The Cartesian
velocity components at point B on the platform are, respectively, given by,
rw _ rw _
x_ B ¼ x_ 0 ðd þ LÞϕ_ sin ϕ ¼ θ L þ θ_ R cos ϕ ðd þ LÞ θ R θ_ L sin ϕ, (5.26)
2 2b
rw _ rw _
y_ B ¼ y_ 0 þ ðd þ LÞϕ_ cos ϕ ¼ θ L þ θ_ R sin ϕ þ ðd þ LÞ θ R θ_ L cos ϕ: (5.27)
2 2b
Moreover,
z_ B ¼ 0: (5.28)
These relations can expressed in matrix form as,

T
vB ¼ JP θ_ L θ_ R : (5.29)
Assume that the transformation relating the manipulator base joint axes to the inertial
Cartesian frame at point B is given by TIB . Then a velocity vector in terms of the base
frame located at the manipulator’s base joint may be transformed to the inertial frame. It
follows that the end effector inertial velocity components are given in terms of the joint
angular velocities Θ_ by,
_
vE ¼ vB þ TIB JmV Θ: (5.30)
Thus we have,
2 3
θ_ L
vB JP 0 6_ 7
¼ 4 θ R 5: (5.31)
vE JP TIB JmV
Θ_
In addition the relation,

2 3
θ_ L
rw _ rw 6 7
ϕ_ ¼ θ R θ_ L ¼ ½ 1 1 0 4 θ_ R 5, (5.32)
2b 2b
Θ_
is also required. In these equations, any rows with all zero entries as well as the
corresponding velocities on the left-hand side are deleted. This relation may be com-
pactly expressed as,
2 3 2 3
θ_ L θ_ L
6 7 6 7
X_ ¼ J4 θ_ R 5 and q_ ¼ NC 4 θ_ R 5 NC v, (5.33)
Θ_ Θ_
T T
where X_ ¼ ½ xB yB xE yE zE T , q ¼ xC yC ϕ Θ_ and v ¼ θ_ L θ_ R Θ_ .
Thus,
2 3
θ_ L
6 7
X_ ¼ J4 θ_ R 5 ¼ Jv and v ¼ J1 X, _ (5.34)
Θ_
where we have tacitly assumed that the matrix J is square and invertible. This is related
to the issue of manipulability.
The constraint matrix C satisfies,
CNC ¼ 0: (5.35)
The coupled Euler-Lagrange equations for the mobile platform and the manipulator are,
T
HðqÞ€q þ Dðqq_ Þq_ þ GðqÞ ¼ QT τT þ Cλ: (5.36)
€ the following equation of motion is obtained,

Eliminating q,

T Q
NTC HðqÞNC v_ þ NTC _
Dðqq_ ÞNC þ HðqÞN C v þ NC GðqÞ ¼ NC
T
: (5.37)
τ
5.3.1 Manipulability
Often the desired motion is specified in inertial coordinates and relative to the end
effector’s position coordinates. For this reason, we may eliminate the coordinates v and
express the equations in terms of the inertial coordinates X, where the relationship
between v and X_ is given by, v ¼ J1 X. _ Thus, in order that the transformation of
coordinates can be applied, it is essential that the Jacobian J is invertible. If J is not
square, it is interpreted as the generalized inverse and given by,
1
Jþ ¼ JT J JT : (5.38)
In this case, the generalized inverse is feasible only if the matrix JT J is invertible. Thus,
a measure of the manipulability, introduced by Yoshikawa [9] formobile manipulators,
is defined as the determinant of the matrix JT J; i.e. m ¼ det JT J . Another measure of
the manipulability is defined as the ratio of the smallest to the largest singular values of
the matrix JT J. Thus, w2 ¼ σ min =σ max is another measure of manipulability; σ min and
σ max are the minimum and maximum singular values of J. Bayle, Fourquet, and Renaud
[10] proposed yet another measure of manipulability, which is given by,
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi
w5 ¼ 1 ðσ min =σ max Þ2 : (5.39)
Nait-Chabane, Hoppenot, and Colle [11] proposed yet another measure of

manipulability, which is in fact also a function of the direction in the workspace in
which the manipulator seeks to function.
5.3.2 Tip Over and Dynamic Stability Issues

To understand the basic issues related to tip over and dynamic stability, a typical
example of a two-link manipulator on a moving platform is considered. The motion
of the platform is restricted to one-dimension, so the non-holonomic constraints are
implicitly satisfied. The platform carrying the manipulator is illustrated in Figure 5.2.
X2
Y2
Tip mass
M2
Y1 X1
L2 q2
X0
Xw
d0 M1
L1
q1
Zw Y0
Figure 5.2 Typical mobile platform carrying a two-link manipulator.
It is proposed to obtain the total kinetic and potential energies of the arm in terms of
the total mass, moment of inertia, and mass-moment components,
M Total ¼ m1 þ m2 þ M 1 þ M 2 þ M cart , (5.40)
I 11 ¼ m1 L21cm þ M 1 L21 þ ðm2 þ M 2 ÞL21 þ m1 k 21 , I 22 ¼ m2 L22cm þ M 2 L22 þ m2 k22 ,

Γ1 ¼ ðm1 L1cm þ m2 L1 þ M 1 L1 þ M 2 L1 Þ, Γ2 ¼ ðm2 L2cm þ M 2 L2 Þ: (5.41)
In these expressions, M 2 is the tip mass and mi , Li , Licm , and ki are, respectively, the ith
link mass, the ith link length, the ith link position of the center of mass with reference to
the ith joint, and the ith link radius of gyration about an axis parallel to the joint axis at
its center of mass.
The position coordinates of the centers of mass of link 1 and link 2, the position
coordinates of the centers of mass of the lumped masses M 1 and M 2 , and that of the cart
are, respectively, given by,
x1, cm ¼ L1cm cos θ1 , z1, cm ¼ d0 þ L1cm sin θ1 , (5.42)
x2, cm ¼ L1 cos θ1 þ L2cm sin ð90 θ1 þ θ2 Þ, (5.43)
z2cm ¼ d0 þ L1 sin θ1 þ L2cm cos ð90 θ1 þ θ2 Þ, x3, cm ¼ L1 cos θ1 , (5.44)
z3, cm ¼ d0 þ L1 sin θ1 , x4, cm ¼ L1 cos θ1 þ L2 sin ð90 θ1 þ θ2 Þ, (5.45)
z4cm ¼ d0 þ L1 sin θ1 þ L2 cos ð90 θ1 þ θ2 Þ, x5, cm ¼ 0, (5.46)
and,
z5, cm ¼ d 0 : (5.47)
The velocity components of the centers of mass of link 1 and link 2, the velocity
components of the centers of mass of the lumped masses M 1 and M 2 , and that of the cart
x_ 1, cm ¼ L1cm θ_ 1 sin θ1 , z_ 1, cm ¼ d_ 0 þ L1cm θ_ 1 cos θ1 , (5.48)

x_ 2, cm ¼ L1 θ_ 1 sin θ1 þ L2cm θ_ 1 þ θ_ 2 cos ð90 θ1 þ θ2 Þ, (5.49)

z_ 2cm ¼ d_ 0 þ L1 θ_ 1 cos θ1 L2cm θ_ 1 þ θ_ 2 sin ð90 θ1 þ θ2 Þ, (5.50)
x_ 3, cm ¼ L1 θ_ 1 sin θ1 , z_ 3, cm ¼ d_ 0 þ L1 θ_ 1 cos θ1 , (5.51)

x_ 4, cm ¼ L1 θ_ 1 sin θ1 þ L2 θ_ 1 þ θ_ 2 cos ð90 θ1 þ θ2 Þ, (5.52)

z_ 4cm ¼ d_ 0 þ L1 θ_ 1 cos θ1 L2 θ_ 1 þ θ_ 2 sin ð90 θ1 þ θ2 Þ, (5.53)
x_ 5, cm ¼ 0, (5.54)
and,
z_ 5, cm ¼ d_ 0 : (5.55)
The total potential energy is given by,

V ¼ ðm1 L1cm þ m2 L1 þ M 1 L1 þ M 2 L1 Þg cos θ1 þ ðm2 L2cm þ M 2 L2 Þg sin ð90 θ1 þ θ2 Þ:
(5.56)
It is expressed as,
V Γ1 g cos θ1 þ Γ2 g sin ð90 θ1 þ θ2 Þ: (5.57)
The cart does not contribute to the potential energy.

The total translational and rotational kinetic energy may be shown to be,
1 1 1 2
T ¼ M Total d_ 20 þ I 11 θ_ 21 þ I 22 θ_ 1 θ_ 2 þ Γ1 d_ 0 θ_ 1 cos θ1
2 2 2

_
þ Γ2 θ 1 θ 2 d 0 cos ðθ1 θ2 Þ þ L1 θ_ 1 cos ðθ2 Þ :
_ _ (5.58)
The Euler-Lagrange equations may be shown to be,

I 11 θ€1 þ I 22 θ€1 θ€2 þ Γ2 d€0 cos ðθ1 θ2 Þ þ L1 2θ€1 θ€2 cos ðθ2 Þ

þ Γ1 d€0 cos θ1 Γ1 g sin θ1 Γ2 L1 2θ_ 1 θ_ 2 θ_ 2 sin ðθ2 Þ
Γ2 g cos ð90 þ θ2 θ1 Þ ¼ τ 1 , (5.59)

I 22 θ€2 θ€1 Γ2 d€0 cos ðθ1 θ2 Þ þ L1 θ€1 cos ðθ2 Þ
þ gΓ2 cos ð90 þ θ2 θ1 Þ þ Γ2 L1 θ_ 21 sin ðθ2 Þ ¼ τ 2 , (5.60)

M Total d€0 þ Γ1 θ€1 cos θ1 þ Γ2 θ€1 θ€2 cos ðθ1 θ2 Þ
2
Γ1 θ_ 21 sin θ1 Γ2 θ_ 1 θ_ 2 sin ðθ1 θ2 Þ ¼ f : (5.61)
Examining the second equation for the outer link, the torques driving the link apart from
the control torque are due to the d’Alembert forces generated by the translation
acceleration of the cart, the tangential acceleration of the base link, the acceleration
due to gravity, and the centrifugal force due to the rotation rate of the base link.
However, none of these forces would contribute to the driving torque on the second
link if the center of mass of the second link is such that,
Γ2 ¼ 0; (5.62)
i.e. the joint is located at the center of mass of the second link and its tip mass. If this is
indeed the case, the second equation reduces to,

I 22 θ€2 θ€1 ¼ τ 2 : (5.63)
By a similar argument, if the base joint is also located at the center of mass of both links,
i.e. if,
5.4 Dynamic Control for Path Tracking by Wheeled Mobile Manipulators 215
Γ1 ¼ ðm1 L1cm þ m2 L1 þ M 1 L1 þ M 2 L1 Þ ¼ 0, (5.64)
the other two equations reduce to,

I 11 θ€1 þ I 22 θ€1 θ€2 ¼ τ 1 , (5.65)
and,
M Total d€0 ¼ f : (5.66)
In this case, the manipulator is said to be mass balanced and is referred to as a balanced
manipulator. The simplicity of the equations of motion attests to the benefits of
balancing the manipulator. There is no possibility of any static or tip over instability.
Otherwise, due to the d’Alembert forces generated by the translation acceleration of the
cart acting in the horizontal direction, by the tangential acceleration of the base link
acting normal to it, the acceleration due to gravity acting vertically down, and the
centrifugal force due to rotation rate of the base link acting radially to it, there is a real
danger that the manipulator would tip over and fall to the ground. This is the problem of
tip over instability.
A number of stability measures and criteria have been developed by various research-
ers to establish the stability of a mobile manipulator and predict the occurrence of tip
over. These measures and criteria include the classical Zero-Moment Point, the Force-
Angle stability measure, and the Moment-Height Stability measure. The Zero Moment
Point, or ZMP, is a point on the ground where the sum of all the forces and moments
acting on the robot platform can be replaced by a single force. It was originally defined
for bipedal robots, so as to be able to assess their stability while they walk (see, for
example, Vepa [12]), and it has been adapted to mobile manipulators by several
researchers. A different approach to stability analysis was proposed by Papadopoulos
and Rey [13], in which they referred to the Force-Angle stability measure (FA). The FA
algorithm measures the angle of the total applied force on the center of mass of the
entire mobile platform with reference to a support polygon, which is derived from
the points of contact the mobile platform makes with the ground at any instant of time.
The angle is considered to be positive when the force is acting within the support polygon
and negative outside it. Tip over instability occurs when the angle is zero or negative.
Moosavian and Alipour [14] proposed the Moment-Height Stability (MHS) measure,
which also accounts for the manipulator’s own inertia about each axis of the support
polygon relative to the gravitation force acting at the manipulator’s center of gravity.
5.4 Dynamic Control for Path Tracking by Wheeled Mobile Manipulators
Consider a typical two-link manipulator mounted on the mobile platform shown in

Figure 5.2, via a capstan with its axis of rotation normal to the base of the mobile
platform and pointing up. The inertial position coordinates of the centers of mass of the
link 1 and link 2, the position coordinates of the centers of mass of the lumped masses
M 1 and M 2 , and that of the cart are, respectively, given by,
x1, cm ¼ L1cm cos θ1 , (5.67)
z1, cm ¼ d0z þ L cos ϕ þ L1cm sin θ1 cos ðϕ þ ϕ1 Þ, (5.68)
y1, cm ¼ d0y þ L sin ϕ þ L1cm sin θ1 sin ðϕ þ ϕ1 Þ, (5.69)
x2, cm ¼ L1 cos θ1 þ L2cm sin ð90 θ1 þ θ2 Þ, (5.70)
z2cm ¼ d0z þ L cos ϕ þ ðL1 sin θ1 þ L2cm cos ð90 θ1 þ θ2 ÞÞ cos ðϕ þ ϕ1 Þ, (5.71)
y2cm ¼ d 0y þ L sin ϕ þ ðL1 sin θ1 þ L2cm cos ð90 θ1 þ θ2 ÞÞ sin ðϕ þ ϕ1 Þ, (5.72)
x3, cm ¼ L1 cos θ1 , (5.73)
z3, cm ¼ d 0z þ L cos ϕ þ L1 sin θ1 cos ðϕ þ ϕ1 Þ, (5.74)
y3, cm ¼ d0y þ L sin ϕ þ L1 sin θ1 sin ðϕ þ ϕ1 Þ, (5.75)
x4, cm ¼ L1 cos θ1 þ L2 sin ð90 θ1 þ θ2 Þ, (5.76)

z4cm ¼ d0z þ L cos ϕ þ ðL1 sin θ1 þ L2 cos ð90 θ1 þ θ2 ÞÞ cos ðϕ þ ϕ1 Þ, (5.77)
y4cm ¼ d0y þ L sin ϕ þ ðL1 sin θ1 þ L2 cos ð90 θ1 þ θ2 ÞÞ sin ðϕ þ ϕ1 Þ, (5.78)
x5, cm ¼ 0, (5.79)
and
z5, cm ¼ d0z þ L cos ϕ, y5, cm ¼ d 0y þ L sin ϕ, (5.80)
where d0z and d0y are position coordinates of the mobile platform in the horizontal
plane, ϕ is the orientation angle of the platform to the horizontal, and ϕ1 is the rotation
angle of the plane of operation of the outer links of the manipulator with respect to the
forward axis of the platform.
The velocity components of the centers of mass of the link 1 and link 2, the velocity
components of the centers of mass of the lumped masses M 1 and M 2 and that of the cart
x_ 1, cm ¼ θ_ 1 L1cm sin θ1 , (5.81)

_ sin ϕ þ L1cm θ_ 1 cos θ1 cos ðϕ þ ϕ Þ L1cm ϕ_ þ ϕ_ sin θ1 sin ðϕ þ ϕ Þ,
z_1, cm ¼ d_ 0z ϕL 1 1 1
(5.82)

_ cos ϕ þ L1cm θ_ 1 cos θ1 sin ðϕ þ ϕ Þ þ L1cm ϕ_ þ ϕ_ sin θ1 cos ðϕ þ ϕ Þ,
y_ 1, cm ¼ d_ 0y þ ϕL 1 1 1
(5.83)

x_ 2, cm ¼ θ_ 1 L1 sin θ1 θ_ 1 θ_ 2 L2cm sin ðθ1 θ2 Þ, (5.84)

_ sin ϕ þ θ_ 1 L1 cos θ1 þ θ_ 1 θ_ 2 L2cm cos ðθ1 θ2 Þ cos ðϕ þ ϕ Þ
z_2cm ¼ d_ 0z ϕL
1
ϕ_ þ ϕ_ 1 ðL1 sin θ1 þ L2cm sin ðθ1 θ2 ÞÞ sin ðϕ þ ϕ1 Þ, (5.85)

_ cos ϕ þ θ_ 1 L1 cos θ1 þ θ_ 1 θ_ 2 L2cm cos ðθ1 θ2 Þ sin ðϕ þ ϕ1 Þ
y_ 2cm ¼ d_ 0y þ ϕL

þ ϕ_ þ ϕ_ 1 ðL1 sin θ1 þ L2cm sin ðθ1 θ2 ÞÞ cos ðϕ þ ϕ1 Þ, (5.86)
x_ 3, cm ¼ θ_ 1 L1 sin θ1 , (5.87)

_ sin ϕ þ L1 θ_ 1 cos θ cos ðϕ þ ϕ Þ L1 ϕ_ þ ϕ_ sin θ1 sin ðϕ þ ϕ Þ, (5.88)
z_3, cm ¼ d_ 0z ϕL 1 1 1

_ cos ϕ þ L1 θ_ 1 cos θ sin ðϕ þ ϕ Þ þ L1 ϕ_ þ ϕ_ sin θ1 cos ðϕ þ ϕ Þ, (5.89)
y_ 3, cm ¼ d_ 0y þ ϕL 1 1 1

x_ 4, cm ¼ θ_ 1 L1 sin θ1 θ_ 1 θ_ 2 L2 sin ðθ1 θ2 Þ, (5.90)

_ sin ϕ þ θ_ 1 L1 cos θ1 þ θ_ 1 θ_ 2 L2 cos ðθ1 θ2 Þ cos ðϕ þ ϕ Þ
z_4cm ¼ d_ 0z ϕL
1
ϕ_ þ ϕ_ 1 ðL1 sin θ1 þ L2 sin ðθ1 θ2 ÞÞ sin ðϕ þ ϕ1 Þ, (5.91)

_ cos ϕ þ θ_ 1 L1 cos θ1 þ θ_ 1 θ_ 2 L2 cos ðθ1 θ2 Þ sin ðϕ þ ϕ Þ
y_ 4cm ¼ d_ 0y þ ϕL 1

_ _
þ ϕ þ ϕ ðL1 sin θ1 þ L2 sin ðθ1 θ2 ÞÞ cos ðϕ þ ϕ Þ, (5.92)
1 1
_ sin ϕ, y_ 5, cm ¼ d_ 0y þ ϕL
x_ 5, cm ¼ 0 and z_5, cm ¼ d_ 0z ϕL _ cos ϕ: (5.93)
The null space of the constraint is defined as,
2 3 2 3
d0y 2 3 cos ϕ sin ϕ 0 0 0
6 d 0z 7 v 6 sin ϕ
6 7 6 vT 7
F
6 cos ϕ 0 0 077
d d6 ϕ 7 6 7 6 0
7 ¼ NC 6 ϕ_ þ ϕ_ 1 7, NC ¼ 6 1=d 0 0 07
q 6 7: (5.94)
dt dt 6
6 ϕ þ ϕ 1
7
7
6
4 θ_ 1 5
7 6 0
6 0 1 0 077
4 θ1 5 4 0 0 0 1 05
θ_ 1 θ_ 2
θ1 θ2 0 0 0 0 1
Observe that NC is of the form,

NC11 033
NC ¼ : (5.95)
032 I33
Thus the platform’s horizontal plane velocity coordinates and the end effectors relative
velocity components, relative to the platform, are given by,
2 3
2 3 d0y 2 3
y5, cm 6 d 0z 7 vF
6 z5, cm 7 6 7 6 vT 7
d d6 6
7
7 d d6
6 ϕ 7 7
6
6 _ _ 7
7
x 6 x4, cm 7 ¼ J dt q J dt 6 ϕ þ ϕ1 7 ¼ JNC 6 ϕ þ ϕ 1 7,
dt dt 4
y4, cm y5, cm 5 6 7 4 θ_ 1 5
4 θ1 5
z4, cm z5, cm θ_ 1 θ_ 2
θ1 θ2
2 3
1 0 L cos ϕ 0 0 0
6 0 1 L sin ϕ 0 0 0 7
6 7
J¼6 6 0 0 0 0 L 1 sin θ 1 L2 sin ð θ 1 θ 2 Þ 7,
7
40 0 0 yϕ1 L1 cos θ1 sin ðϕ þ ϕ1 Þ L2 cos sðθ1 θ2 Þ sin ðϕ þ ϕ1 Þ 5
0 0 0 zϕ1 L1 cos θ1 cos ðϕ þ ϕ1 Þ L2 cos ðθ1 θ2 Þ cos ðϕ þ ϕ1 Þ
(5.96)
where,
yϕ1 ¼ ðL1 sin θ1 þ L2 sin ðθ1 θ2 ÞÞ cos ðϕ þ ϕ1 Þ, (5.97)
zϕ1 ¼ ðL1 sin θ1 þ L2 sin ðθ1 θ2 ÞÞ sin ðϕ þ ϕ1 Þ: (5.98)
Moreover, J is of the form,

Jp 023
J¼ : (5.99)
033 Jm
It can now be confirmed that the Jacobian-based transformation JNC is non-singular

and we can proceed to find the Euler-Lagrange equations. To this end let,
M Total ¼ m1 þ m2 þ M 1 þ M 2 þ M cart , m11 ¼ m1 þ M 1 , m22 ¼ m2 þ M 2 , I 11 ¼ m1 L21cm
þM 1 L21 þ ðm2 þ M 2 ÞL21 þ m1 k21 , I 22 ¼ m2 L22cm þ M 2 L22 þ m2 k22 , Γ11 ¼ m1 L1cm þ M 1 L1 ,
Γ2 ¼ m2 L2cm þ M 2 L2 , Γ1 ¼ Γ11 þ m22 L1 . The total translational and rotational kinetic
energy may then be shown to be,
1 2
1 1 1 1
T ¼ M Total d_ 0y þ d_ 20z þ I p þ M cart L2 ϕ_ 2 þ I m ðθ1 ; θ12 Þϕ_ 2t þ I 11 θ_ 1 þ I 22 θ_ 12
2 2
2 2 2 2 2

þM Total Lϕ_ d_ 0y cos ϕ d_ 0z sin ϕ þ Γ1 θ_ 1 cos θ1 d_ 0y sin ϕt þ d_ 0z cos ϕt

þΓ2 θ_ 12 d_ 0y sin ϕt þ d_ 0z cos ϕt cos θ12 þ L1 θ_ 1 cos θ2

_ θ_ 1 Γ1 cos θ1 þ θ_ 12 Γ2 cos θ12 sin ϕ þ Lϕ_ ϕ_ ðΓ1 sin θ1 þ Γ2 sin θ12 Þ cos ϕ
þϕL 1 t 1

_ _ _
þϕ t ðΓ1 sin θ1 þ Γ2 sin θ12 Þ d 0y cos ϕt d 0z sin ϕt : (5.100)
In the above expression, θ12 ¼ θ1 θ2 , ϕt ¼ ϕ þ ϕ1 , I p is the mass moment of inertia of

the platform about an axis normal to the platform and about its center of mass, and I m is
the moment of inertia of the manipulator about an axis normal to the platform at the
capstan, which is given by,
1
I m ðθ1 ; θ12 Þ ¼ m1 k21y þ L21cm sin 2 θ1

2
1 2
þ m2 k 2y sin θ12 þ ðL1 sin θ1 þ L2cm sin θ12 Þ2

2 (5.101)
2
1 1
þ M 1 L21 sin 2 θ1 þ M 2 ðL1 sin θ1 þ L2 sin θ12 Þ2 :
2 2
In the previous equation, kiy are the radii of gyration of the ith link at its center of mass,
with respect to an axis that is mutually perpendicular to both the joint axis and the
direction of the vector pointing to the next joint.
The total potential energy is given by,
V ¼ ðm1 L1cm þ m2 L1 þ M 1 L1 þ M 2 L1 Þg cos θ1 þ ðm2 L2cm þ M 2 L2 Þg cos θ12 : (5.102)
It is expressed as,
V Γ1 g cos θ1 þ Γ2 g cos θ12 : (5.103)
The platform does not contribute to the potential energy. It follows that the nonzero
derivatives of the potential energy function are,
∂
V Γ1 g sin θ1 ,
∂θ1 (5.104)
∂
V Γ2 g sin θ12 :
∂θ12
Let,
I pL ¼ I p þ M cart L2 ; H ¼ ½ H1 H2 , cðÞ ¼ cos ðÞ, sðÞ ¼ sin ðÞ, (5.105)
where
2 3
M Total 0 M Total Lcϕ
6 0 M Total M Total Lsϕ 7
6 7
6 M Lcϕ M Lsϕ I 7
6
H1 ¼ 6 Total Total pL 7
7
6 ðΓ1 sθ1 þ Γ2 sθ12 Þcϕt ðΓ1 sθ1 þ Γ2 sθ12 Þsϕt LðΓ1 sθ1 þ Γ2 sθ12 Þcϕ1 7
4 Γ1 cθ1 sϕt Γ1 cθ1 cϕt Γ1 cθ1 sϕ1 5
2 Γ2 cθ12 sϕt Γ2 cθ12 cϕt 3 Γ2 cθ12 sϕ1
ðΓ1 sθ1 þ Γ2 sθ12 Þcϕt Γ1 cθ1 sϕt Γ2 cθ12 sϕt
6 ðΓ1 sθ1 þ Γ2 sθ12 Þsϕt Γ1 cθ1 cϕt Γ2 cθ12 cϕt 7
6 7
6 LðΓ1 sθ1 þ Γ2 sθ12 Þcϕ1 Γ1 cθ1 sϕ1 Γ2 cθ12 sϕ1 7
H2 ¼ 6
6
7:
7
6 I m ðθ1 ; θ12 Þ 0 0 7
4 0 I 11 Γ2 L1 cθ2 5
0 Γ2 L1 cθ2 I 22
(5.106)
Furthermore,

H11 H12
H ½ H1 H2 , (5.107)
H21 H22
where H11 and H22 are square matrices.

Also, let the generalized coordinates, the gradient of the potential function, and the
generalized forces be defined, respectively, as,
2 3 2 3 2 3
d 0y 0 Fy
6 d0z 7 6 0 7 6 Fz 7
6 7 6 7 6 7
6 ϕ 7 ∂ 6 0 7 6N 7
q6 6 7 6
V ¼ g6 7 gG, Q 6 7
ϕ 7 ,
∂q 0 7 6 N t 7: (5.108)
6 t 7 6 7 6 7
4 θ1 5 4 Γ1 sθ1 5 4 τ1 5
θ12 Γ2 sθ12 τ 12
The kinetic energy and its partial derivatives may be expressed as,
1 ∂ ∂ 1 ∂H d ∂ ∂H
T ¼ q_ T Hq,
_ _
T ¼ Hq, T ¼ q_ T _
q, T ¼ Hq€ þ q_ T _
q: (5.109)
2 ∂q_ ∂q 2 ∂q dt ∂q_ ∂q
Thus the Euler-Lagrange equations are,
d ∂ ∂ ∂ ∂H 1 ∂H
T T þ V ¼ Hq€ þ qT
_ q_ qT
_ q_ þ gG ¼ Q λCT , (5.110)
dt ∂q_ ∂q ∂q ∂q 2 ∂q
where C is the non-holonomic constraint matrix.
The Euler-Lagrange equations simplify to,
d ∂ ∂ ∂ 1 ∂H
T T þ V ¼ Hq€ þ q_ T q_ þ gG ¼ Q λCT : (5.111)
dt ∂q_ ∂q ∂q 2 ∂q
Hence,
1 ∂H
Hq€ þ q_ T q_ þ gG ¼ Q λCT : (5.112)
2 ∂q
Making the coordinate transformation,
T
q_ ¼ NC vF vT ϕ_ þ ϕ_ 1 θ_ 1 θ_ 1 θ_ 2 NC v, (5.113)
and multiplying by NTC gives,

1 ∂H
NTC HNC v_ þ NTC HN_ C v þ NTC vT NTC NC v þ NTC gG ¼ NTC Q: (5.114)
2 ∂q
Making the similarity transformation, x_ ¼ JNC v, with,

Jp 023 NC11 033 JN 023
JNC ¼ ¼ p C11 , (5.115)
033 Jm 032 I33 032 Jm
so,
1
v ¼ ðJNC Þ1 x,
_ vT ¼ x_ T NTC JT : (5.116)
It follows that,
Hx x€ þ BVx_ þ BgG ¼ BQ τx , (5.117)
where,

Hx ¼ JNC NTC HNC ðJNC Þ1 , B ¼ JNC NTC ,
1 1 1 ∂H
V ¼ HNC J_ N_ C þ HN_ C ðJNC Þ1 þ x_ T NTC JT NTC NC ðJNC Þ1 :
2 ∂q
Also,

NTC11 023 H11 H12 NC11 033
NTC HNC ¼ , (5.118)
033 I33 H21 H22 032 I33
so,

NTC11 H11 NC11 NTC11 H12 J N NT 023
NTC HNC ¼ and B ¼ p C11 C11 : (5.119)
H21 NC11 H22 032 Jm
Consequently, one could express Hx and BVas,

Hx11 Hx12 Vb11 Vb12
Hx , BV : (5.120)
Hx21 Hx22 Vb21 Vb22
The structure of these equations suggests that we could split the state vector x into two
components, x1 and x2 , such that,
2 3
x4, cm
x1 y5, cm
x¼ , x1 ¼ , x2 ¼ 4 y4, cm y5, cm 5: (5.121)
x2 z5, cm
z4, cm z5, cm
As a result, the equations of motion in the x domain can be expressed in terms of two
equations for x1 and x2 . It is now feasible to design a control law to implement a desired
motion plan.
Having established the equations of motion of a mobile manipulator in an inertial
frame with no further constraints, the controller synthesis for tracking a desired motion
plan is now considered. The coordinated control of a wheeled mobile manipulator has
been considered in some detail by Yamamoto [15]. The control is assumed to be
generated from the demanded acceleration, the demanded rate vector, the demanded
position vector, and three feedbacks, (i) the gravity-gradient potential function V, (ii) a
dissipation function similar to the Rayleigh dissipation function, and (iii) an artificial
potential field. To a certain extent, such a methodology employs potentials that are
similar in principle to the potentials encountered in the derivation of the Euler-Lagrange
equations in the preceding section and would preserve the conservative nature of the
robot dynamics, thus ensuring that the closed loop system is inherently stable.
The control torques are assumed to be,
dD dΦ
τx ¼ Hx x€d þ BVx_ d þ BgG , (5.122)
d x_ dx
where x_ d is the demanded rate vector, D is a dissipation function, and Φ is an artificial
potential function. With an appropriate choice of D and Φ, the control law may be
interpreted as a proportional-derivative control law.
The equations of motion reduce to,
dD dΦ
Hx ðx€ x€d Þ þ BVðx_ x_ d Þ þ þ ¼ 0: (5.123)
dx_ dx
If D and Φ are, respectively, assumed to be,
1 1
D ¼ ðx_ x_ d ÞT Kd ðx_ x_ d Þ and Φ ¼ ðx xd ÞT Kp ðx xd Þ, (5.124)
2 2
with Kp being positive definite, it follows that,
Hx ðx€ x€d Þ þ ðKd þ BVÞðx_ x_ d Þ þ Kp ðx xd Þ ¼ 0: (5.125)
If we define the error e as, e ¼ x xd , the error vector between the current position
vector and the demanded position vector, the error satisfies the equation,
Hx e€ þ ðKd þ BVÞe_ þ Kp e ¼ 0: (5.126)
To ensure stability, consider the Lyapunov function and its time derivative,
1 T
V¼ e_ Hx e_ þ eKp e , (5.127)
2

V_ ¼ e_ T Hx e€ þ Kp e ¼ e_ T ðKd þ BVÞe:
_ (5.128)
Thus, to guarantee stability, Kd þ BV must be adequately positive definite.
5.5 Decoupled Control of the Mobile Platform and Manipulator
In general, a mobile manipulator cannot follow or track a desired trajectory unless the
mobile platform is in motion, so the desired position of the end effector is within the
local workspace of the manipulator. Moreover, the definition of the useful workspace
of the manipulator is based on the manipulability of the manipulator within the
workspace. Thus, it is assumed that there are no singular configurations of the
manipulator within the local workspace. Moreover, the position of the end effector
in three dimensions requires three coordinates. Thus, a manipulator with three or
more links is generally required. However, when there are more than three links,
there is a certain level of redundancy, which must be effectively managed. One
approach to doing this is to decouple the manipulator and the mobile platform
dynamics. Such a decoupled control approach is feasible, as both the transformations,
NC and JNC , are decoupled. Thus, one may split the state vector x into two
components, x1 and x2 , and design independent controllers for positioning the mobile
platform and the manipulator end-effector within the workspace. The mobile is
positioned so the actual position of the end effector is closest to its desired position
(for example, vertically below or just behind in the case of the manipulator and
platform discussed in the preceding section!)
To illustrate the decoupling approach, consider the equations of motion given by
Equation (5.117) in Section 5.4, which are,
Hx x€ þ BVx_ þ BgG ¼ BQ τx : (5.129)
They may be expressed as,

Hx11 Hx12 x€1 Vb11 Vb12 x_ 1 0 Q
þ þg ¼B 1 ,
Hx21 Hx22 x€2 Vb21 Vb22 x_ 2 Jm G 1 Q2
2 3
0
J N NT 023 4 5 Q1
B ¼ p C11 C11 , G1 ¼ Γ1 sθ1 and ¼ Q: (5.130)
032 Jm Q2
Γ2 sθ12
The first step toward decoupling is to define auxiliary control inputs Up and Um
such that,
Up ¼ BQ1 Vb12 x_ 2 Hx12 x€2 , Um ¼ BQ2 Vb21 x_ 1 Hx21 x€1 : (5.131)
Thus, the decoupled equations of motion are,
5.6 Motion Planning for Mobile Manipulators 223
Hx11 x€1 þ Vb11 x_ 1 ¼ Up , (5.132)
Hx22 x€2 þ Vb22 x_ 2 þ gJm G1 ¼ Um : (5.133)
5.6 Motion Planning for Mobile Manipulators
The control problem discussed in Section 5.5 has a few pitfalls, particularly when there
are significant disturbances, and consequently the tracking error will not tend to zero
asymptotically, although it may still be small. In general, one can identify three types of
control problems: (i) maintain a particular equilibrium configuration with adequate
stability margins; (ii) tracking a reference cart’s position and velocity where the
reference position and velocity vectors satisfy the same constraints as the actual cart’s
position and velocity vectors; (iii) a general path-following problem, where the end
effector follows a general trajectory within its global workspace. It is possible in
principle to solve all of the control problems in a unified framework. Thus, it is possible
to adopt one of several alternative approaches that are used to control a typical fixed-
base manipulator (see, for example, Vepa [16]).
Given that the objective of motion planning is to define a feasible path that is smooth and
continuous, to reach a destination point without colliding with any obstacles, the generation
of a motion plan for robots and mobile manipulators in unstructured, unknown, dynamic
environments can be a complex task involving a variety of sensors and a number of
algorithms for localization, navigation, and obstacle avoidance. Sensory information from
a variety of sensors, including stereo cameras and laser, infrared, or ultrasonic ranging
sensors such as a Red-Green-Blue depth (RGBD) sensors, which use a combination of a
color camera, an infrared sensor for depth estimation, and a microphone array for sound
measurements, must be used to collect basic data prior to implementing an algorithm for
localization, navigation, and obstacle avoidance. Other complex sensor systems include
laser scanners, Charge Coupled Device (CCD) cameras, Global Positioning System (GPS)
sensors, and inertial measuring units. Typically, a robotic mobile manipulator must not only
avoid dynamic obstacles autonomously at all times but must also deal with the emergence
of new obstacles such as the sudden entry of an object, which requires the mobile planner to
dynamically and reactively re-plan the motion paths. Obstacle avoidance may be imple-
mented by first constructing the configuration space. Given a map of the obstacles on a two-
dimensional space, one can define the actual workspace by identifying the most traversable
domains. Once the workspace is identified, the configuration space is obtained from the
workspace by a process known as image morphology dilation. The configuration space is
obtained by eroding the edges around the free workspace. This step involves the creation of
a morphology dilation of the robot footprint. Dilation is a basic operator in creating a new
morphology of a binary image. The effect of applying the operator on a binary image is to
enlarge the boundaries by specific amounts. Thus, areas of boundary pixels are grown in
size while holes within those regions are made smaller.
To plan a path, two metrics are constructed over a grid in the configuration space; one
representing the distance from the nearest obstacle, while the second is the weighted
distance to the goal position. An artificial potential function Φ is constructed by

subtracting the second metric from the first. One always moves forward such that the
gradient of the artificial potential function Φ is always maximum, thus ensuring that one
is always avoiding an obstacle while also moving toward the intended goal position.
The ability to re-plan the motion requires that localization, navigation, and obstacle
avoidance must be implemented continuously and in real time. Thus, there is a need not
only to estimate the current position of the mobile platform and its workspace but also
the intended future motion of the platform and manipulator arms. This requires estab-
lishing a dynamic model or map of the environment, which in turn is used to define a
dynamic reference model for the future motion. The model of the environment is
established by several rounds of filtering and de-noising, followed by terrain mapping
and decomposition, and obstacle detection to build a three-dimensional occupancy grid
as well as a semantic representation of the known objects. The task of terrain mapping,
which is performed extensively, is to establish whether the nature of the terrain is urban,
natural with vegetation, desert, or canyons, to distinguish between a traversable terrain
and a non-traversable terrain and to determine a feasible local plane path for the
platform to track, from the characteristics of the traversable terrain most suited for the
platform to follow. Thus, the search for a feasible motion plan is limited by the robot’s
sensory capabilities and its own kinematic constraints. When several feasible paths are
available, the choice is made based on an optimal criterion.
An imaging sensor produces a two-dimensional projection of a three-dimensional
space. This is known as the image space. The occupancy grid is initially generated in the
image space and then projected on to the ground plane to generate a Cartesian map
relative to the ground plane. A sparsely populated occupancy grid is a desirable feature
of the domain in which the mobile platform can move forward. An algorithm such as the
A* algorithm (see, for example, Vepa [12]) or a related algorithm is then used to find an
optimal path through the occupancy grid between the platform’s current position and the
goal. Thus, we have succinctly described the entire process of motion planning, although
it must be said that several algorithms must be implemented in real time to successfully
implement a feasible motion-planning scheme. Such a qualitative description of the
process is generally also applicable to a manipulator platform operating in space.
5.7 Non-Holonomic Space Manipulators
The maneuvers and dynamic interactions between a space robotic manipulator on a

spacecraft while it captures an orbiting object is a function of several factors, including
the proximity of the two and their behavior before and after docking. Thus, it involves
four phases, the approach phase, freely floating, docking, and post-docking. While both
the approach phase when the spacecraft is in flight and the freely floating phase involve
dynamic coupling between the satellite and the manipulator, they both require motion
planning so as to be able to complete the docking or capture of the orbiting object.
Of the translational and angular momentum conservation equations, only the former
can be integrated, while the latter are non-integrable or non-holonomic constraints.
5.7 Non-Holonomic Space Manipulators 225
Furthermore, while the total translational momentum of a spacecraft in orbit or the

orbital moment of momentum is generally conserved, it is possible that in the absence of
any applied torques, the total angular momentum is also conserved.
In Section 4.9 the equations of motion of a satellite were derived based on the
principles of the conservation of moment of momentum or angular momentum. The
moment of momentum of the satellite and the manipulator it is carrying was evaluated in
a body-fixed coordinate system. In general, the total moment of momentum of a multi-
body system about its center of mass in body-fixed coordinates may be expressed as,
X X
HB ¼ HBi ¼ IBi ωBi : (5.134)
i i
The expression may be transformed to an inertial or space-fixed frame with its origin at
the center of mass of the multibody system and consequently the moment of momentum
in the space-fixed frame is,
X X
HI ¼ TIB HB ¼ TIB HBi ¼ TIB IBi ωBi : (5.135)
i i
As a result of the conservation of the moment of momentum in the space-fixed frame,

X X X
HI ¼ TIB HB ¼ TIB HBi ¼ TIB IBi ωBi ¼ TIB0 IBi0 ωBi0 , (5.136)
i i i
where TIB the transformation from the body to the inertial frame, IiB the moment of
inertia matrix of the ith body, and ωBi the angular velocity vector in the body-fixed
frame of the ith body are evaluated at time t ¼ 0, on the right-hand side of Equation
(5.136). Thus, the total moment of momentum of the multibody system about its center
of mass in body-fixed coordinates is given by,
X
HB ¼ TBI TIB0 IBi0 ωBi0 : (5.137)
i
It follows that the left-hand side of Equation (5.137) is proportional to a linear

combination of the column vectors of the inverse of TIB or TBI , the transformation
from the inertial frame to the body frame. However, when the multibody system has
zero initial angular momentum at time t ¼ 0, so ωBi0 ¼ 0 for all i, the total moment of
momentum of the multibody system about its center of mass in body-fixed coordinates
HB is zero. This principle was first used for space robotic manipulators by Papadopou-
los and Dubowsky [17] for the case of zero initial angular momentum and by Nanos and
Papadopoulos [18]. Hence, from Section 4.9 it follows that the equations of motion of
the main body of the satellite are given by,
!
d X N XN
ICM1 ω þ ICMi ω þ ti ez θ_ i þ ω ICM1 ω þ ω þ ti ez θ_ i ICMi ω þ ti ez θ_ i
dt i¼2 i¼2

XN
¼ τ sat þ Μsm þ R
p s0 R p se Fi : (5.138)
i¼1
Assuming the total external moment acting on the main satellite body is zero, so,

XN
τ sat þ Μsm þ R
p s0 R p se Fi ¼ 0, (5.139)
i¼1
it can be seen that the total moment of momentum of the multibody system about its
center of mass in body-fixed coordinates HB is zero, and consequently,
X
N
ICM1 ω þ ICMi ω þ ti ez θ_ i ¼ 0: (5.140)
i¼2
Hence,
!
X
N X
N
ICMi ω þ ICMi ti ez θ_ i ¼ 0, (5.141)
i¼1 i¼2
which can be solved for ω. Thus ω is given by,

!1
XN X N
ω¼ ICMi ICMi ti ez θ_ i : (5.142)
i¼1 i¼2
Note that in the case when the initial angular momentum is nonzero,
X
HB ¼ TBI TIB0 IBi0 ωBi0 ¼ TBI qj HI 0 : (5.143)
i
In this equation, HI 0 is a vector constant. It follows that HB is not zero and the right-
hand side is a function of the attitude quaternion components of the main body of the
satellite, qj , j ¼ 1, 2, 3, 4. In this case, the equation for ω includes additional terms that
are functions of the attitude quaternion components of the main body of the satellite.
Thus,
!1 ( )
XN XN
ω¼ ICMi ICMi ti ez θ_ i TBI q HI : (5.144)
j 0
i¼1 i¼2
From Section 4.9 it follows that the equations of motion of the manipulator arms are
given by,
T T T T
d ∂T tran ∂T tran X ∂ e t ∂ e z tk
ez ti ICMi ω_ þ ti ez θ€i þ
T T
þ θ_ k z i
ICMi ω
dt ∂θ_ i ∂θi ∂θ k ∂θi
X ∂ðti ez Þ 1 ∂ðtk ez Þ
k
þ eTz tTi ICMi θ_ k θ_ i

∂θk 2 ∂θi

X ∂ eTz tTi
k
1 ∂ eTz tTk
þ θ_ k ICMi ti ez θ_ i
∂θk 2 ∂θi
k
X !
N
¼ ez ti τ i þ ez ti Μi þ p i0 p icm Fi þ p i0 p ie
T T T T R R R R
Fi ,
i¼iþ1
(5.145)
References 227
with
!!
d ∂T tran ∂T tran X € ∂2 T tran _ _ ∂ ∂2 T tran 1 ∂ ∂2 T tran
¼ θk þ θ jθ k ,
dt ∂θ_ i ∂θi k ∂θ_ k ∂θ_ i ∂θk ∂θ_ j ∂θ_ i 2 ∂θi ∂θ_ j ∂θ_ k
and
! !
∂2 T tran 1 X
N X
N
¼ mi vi, j mi vi, k :
_ _
∂θ k ∂θ j mtotal i¼1 i¼1
From these equations one may eliminate the satellite angular velocity vector ω and
obtain the equations of motion of the manipulator in terms of its joint angles only. In the
case of nonzero initial angular momentum, there are additional terms in the manipulator
equations, which are functions of the attitude quaternion components of the main body
of the satellite as well as the joint angles and the joint angle rates. These terms that
appear in the manipulator equations may be cancelled by applying initial decoupling
control torques to the joints of the manipulator, provided the attitude quaternion
components are available for feedback; i.e. they can either be measured or estimated.
Thus, the manipulator’s dynamics may be transformed such that they are completely
decoupled from the main satellite body’s attitude dynamics, as long as the condition of
the conservation of the initial angular momentum at time t ¼ 0 is satisfied. Moreover,
all of the methods of controller synthesis that can be applied to fixed-base manipulators
may now be applied to the manipulator on board the satellite.
References
[1] Brockett, R. W. (1983) Asymptotic stability and feedback stabilization. In Differential

Geometric Control Theory, R. W. Brockett, R. S. Millmann, H. J. Sussmann, eds., Boston:
Birkhauser, 181–191.
[2] Kolmanovsky, I. and McClamroch, N. H. (1995) Developments in nonholonomic control
problems. IEEE Control Systems Magazine, 15: 20–36.
[3] Godhavn, J. M. and Egeland, O. (1997) A Lyapunov approach to exponential stabilization of
nonholonomic systems in power form. IEEE Transactions on Automatic Control, 42(7):
1028–1032.
[4] Samson, C. (1995) Control of chained systems: Application to path following and time-
varying point-stabilization of mobile robots. IEEE Transactions On Automatic Control,
40(1): 64–77.
[5] Astolfi, A. (1999) Exponential stabilization of a wheeled mobile robot via discontinuous
control. Journal of Dynamic Systems Measurement and Control, 121: 121–126.
[6] Hespanha, J. P. (1996) Stabilization of nonholonomic integrators via logic-based switching,
in Proceedings of the 13th World Congress of IFAC., E., San Francisco, CA. 467–472.
[7] Aguiar, A. P. and Pascoal, A. (2000) Stabilization of the Extended nonholonomic double
integrator via logic-based hybrid control, Proceedings of the SYROCO’00 – 6th IFAC
Symposium on Robot Control, Vienna, Austria.
[8] Ye, H., Michel, A. N., and Hou, L. (1998) Stability theory for hybrid dynamical systems.
IEEE Transactions on Automatic Control, 43(4): 461–474.
[9] Yoshikawa, T. (1985) Manipulability of robotic mechanisms. The International Journal of
Robotics Research, 4: 3–9.
[10] Bayle, B., Fourquet, J. Y., and Renaud, M. (2001) Manipulability analysis for mobile
manipulators, in ICRA’2001, Seoul, South Korea, 1251–1256.
[11] Nait-Chabane, K., Hoppenot, P., and Colle, E. (2007) Directional manipulability for motion
coordination of an assistive mobile arm, ICINCO, May 2007, Angers, France, hal00341319.
[12] Vepa, R. (2009) Biomimetic Robotics: Mechanisms and Control, New York: Cambridge
University Press.
[13] Papadopoulos, E. and Rey, D. (2000) The force-angle measure of tip-over stability margin
for mobile manipulators. Vehicle System Dynamics, 33: 29–48.
[14] Moosavian, S. A. and Alipour, K. (2006) Moment-height tip-over measure for stability
analysis of mobile robotic systems, in Proceedings of the IEEE/RSJ International Confer-
ence on Intelligent Robots and Systems, October 2006, 5546–5551.
[15] Yamamoto, Y. (1994) Coordinated control of a mobile manipulator, University of Pennsyl-
vania Department of Computer and Information Science Technical Report No. MS-CIS-
94–12, March 1994.
[16] Vepa, R. (2016) Nonlinear Control of Robots and Unmanned Aerial Vehicles: An Integrated
Approach, Boca Raton, FL: CRC Press.
[17] Papadopoulos, E. and Dubowsky, S. (1991) Coordinated manipulator/spacecraft motion
control for space robotic systems, in IEEE International Conference on Robotics and
Automation (Sacramento, CA), 1696–1701.
[18] Nanos, K. and Papadopoulos, E. G. (2017) On the dynamics and control of free-floating
space manipulator systems in the presence of angular momentum. Frontiers in Robotics and
AI, 4: 26. doi: 10.3389/frobt.2017.00026.

5 Kinematics, Dynamics, and Control of Mobile Robot Manipulators

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

5 Kinematics, Dynamics, and Control of Mobile Robot Manipulators

Uploaded by

Copyright:

Available Formats

5 Kinematics, Dynamics, and Control

of Mobile Robot Manipulators

5.1 Kinematics of Wheeled Mobile Manipulators:

A mobile robot manipulator is a mobile platform hosting a robotic manipulator. The

Figure 5.1 Typical mobile platform.

In terms of velocities, one has,

If we now introduce the transformation,

which is expressed in matrix notation as,

In matrix notation, it is expressed as,

5.2 Dynamics of Manipulators on a Moving Base

Consider a manipulator with at least two revolute joints on a moving platform. It is

where the constraint matrix C has the form,

C ¼ ½ C11 0 , q1 ¼ ½ x y ϕ T and q2 ¼ ½ θ1 θ 2 T : (5.15)

The transformation of coordinates needed to meet the constraints is,

Eliminating q€1 results in,

5.3 Dynamics of Wheeled Mobile Manipulators

vLw ¼ r w θ_ L , vRw ¼ r w θ_ R: : (5.20)

These are the primary non-holonomic constraints.

The angular rotation rate of the mobile platform is,

These relations can expressed in matrix form as,

In addition the relation,

€ the following equation of motion is obtained,

Nait-Chabane, Hoppenot, and Colle [11] proposed yet another measure of

5.3.2 Tip Over and Dynamic Stability Issues

Figure 5.2 Typical mobile platform carrying a two-link manipulator.

M Total ¼ m1 þ m2 þ M 1 þ M 2 þ M cart , (5.40)

I 11 ¼ m1 L21cm þ M 1 L21 þ ðm2 þ M 2 ÞL21 þ m1 k 21 , I 22 ¼ m2 L22cm þ M 2 L22 þ m2 k22 ,

x1, cm ¼ L1cm cos θ1 , z1, cm ¼ d0 þ L1cm sin θ1 , (5.42)

x2, cm ¼ L1 cos θ1 þ L2cm sin ð90 θ1 þ θ2 Þ, (5.43)

z2cm ¼ d0 þ L1 sin θ1 þ L2cm cos ð90 θ1 þ θ2 Þ, x3, cm ¼ L1 cos θ1 , (5.44)

z3, cm ¼ d0 þ L1 sin θ1 , x4, cm ¼ L1 cos θ1 þ L2 sin ð90 θ1 þ θ2 Þ, (5.45)

z4cm ¼ d0 þ L1 sin θ1 þ L2 cos ð90 θ1 þ θ2 Þ, x5, cm ¼ 0, (5.46)

x_ 1, cm ¼ L1cm θ_ 1 sin θ1 , z_ 1, cm ¼ d_ 0 þ L1cm θ_ 1 cos θ1 , (5.48)

x_ 3, cm ¼ L1 θ_ 1 sin θ1 , z_ 3, cm ¼ d_ 0 þ L1 θ_ 1 cos θ1 , (5.51)

The total potential energy is given by,

The cart does not contribute to the potential energy.

The Euler-Lagrange equations may be shown to be,

þ gΓ2 cos ð90 þ θ2 θ1 Þ þ Γ2 L1 θ_ 21 sin ðθ2 Þ ¼ τ 2 , (5.60)

Γ1 ¼ ðm1 L1cm þ m2 L1 þ M 1 L1 þ M 2 L1 Þ ¼ 0, (5.64)

the other two equations reduce to,

M Total d€0 ¼ f : (5.66)

5.4 Dynamic Control for Path Tracking by Wheeled Mobile Manipulators

Consider a typical two-link manipulator mounted on the mobile platform shown in

x1, cm ¼ L1cm cos θ1 , (5.67)

z1, cm ¼ d0z þ L cos ϕ þ L1cm sin θ1 cos ðϕ þ ϕ1 Þ, (5.68)

y1, cm ¼ d0y þ L sin ϕ þ L1cm sin θ1 sin ðϕ þ ϕ1 Þ, (5.69)

x2, cm ¼ L1 cos θ1 þ L2cm sin ð90 θ1 þ θ2 Þ, (5.70)

x4, cm ¼ L1 cos θ1 þ L2 sin ð90 θ1 þ θ2 Þ, (5.76)

x_ 1, cm ¼ θ_ 1 L1cm sin θ1 , (5.81)

x_ 3, cm ¼ θ_ 1 L1 sin θ1 , (5.87)

zϕ1 ¼ ðL1 sin θ1 þ L2 sin ðθ1 θ2 ÞÞ sin ðϕ þ ϕ1 Þ: (5.98)

Moreover, J is of the form,

It can now be conﬁrmed that the Jacobian-based transformation JNC is non-singular

In the above expression, θ12 ¼ θ1 θ2 , ϕt ¼ ϕ þ ϕ1 , I p is the mass moment of inertia of

I m ðθ1 ; θ12 Þ ¼ m1 k21y þ L21cm sin 2 θ1

þ m2 k 2y sin θ12 þ ðL1 sin θ1 þ L2cm sin θ12 Þ2

I pL ¼ I p þ M cart L2 ; H ¼ ½ H1 H2 , cðÞ ¼ cos ðÞ, sðÞ ¼ sin ðÞ, (5.105)

where H11 and H22 are square matrices.

and multiplying by NTC gives,

Consequently, one could express Hx and BVas,

Thus, to guarantee stability, Kd þ BV must be adequately positive deﬁnite.

5.5 Decoupled Control of the Mobile Platform and Manipulator

Hx x€ þ BVx_ þ BgG ¼ BQ τx : (5.129)