DV

Robot Link Manipulator Control

Chapter 1
Dierential Geometry-based Control
Theory [1]
1.1 Robot-Arm Control
The system under consideration can be dened by the form
x = f(x(t), u(t)) (1.1)
y(t) = h(x(t), u(t)) (1.2)
Consider a friction-less, rigid two-link robot manipulator (or double pendulum) with
control torques u
1
and u
2
applied at the joints. Let the angles made by the arms with
respect to the equilibrium points be
1
and
2
. The dynamics of such a robot-arm may
be obtained via the Euler-Lagrange formalism as follows:-
1. Let = (
1
,
2
) and

= (

1
,

2
). Dening the Lagrangian function as
L(,

) = T(,

) V () (1.3)
where T(,

) is the kinetic energy and V () is the potential energy.
2. For the above conguration with rigid mass-less links, the kinetic energy is com-
puted as the sum of the kinetic energies T
1
and T
2
of the masses m
1
and m
2
. This
1
yields
T
1
(

) =
1
2
m
1
l
1
2

2
1
(1.4)
T
2
(

) =
1
2
(m
2
(l
2
1

2
1
+ l
2
2
(
2
1
+
2
2
) + 2l
1
l
2
(cos
2
)

1
(

1
+

2
))) (1.5)
and similarly the potential energy V is the sum of the potential energies V
1
and
V
2
of the two masses i.e.,
V
1
() = m
1
gl
1
cos
1
(1.6)
V
2
() = m
2
gl
1
cos
1
m
2
gl
2
cos(
1
+
2
) (1.7)
3. Therefore
L(,

) =
1
2
m
1
l
1
2

2
1
+
1
2
(m
2
(l
2
1

2
1
+ l
2
2
(
2
1
+
2
2
) (1.8)
+ 2l
1
l
2
(cos
2
)

1
(

1
+

2
))) + m
1
gl
1
cos
1
+ m
2
gl
1
cos
1
m
2
gl
2
cos(
1
+
2
) (1.9)
4. Now the celebrated Euler-Lagrange equations are
L

i
{
L

i
}
L

i
= u
i
; i = 1, 2 (1.10)
which yields in this case the vector equation
M()

+ C(,

) + K() = u (1.11)
where

= (

1
,

2
), u = (u
1
, u
2
) and
M() =

m
1
l
1
2
+ m
2
l
2
1
+ m
2
l
2
2
+ 2m
2
l
1
l
2
cos
2
m
2
l
2
2
+ m
2
l
1
l
2
cos
2
m
2
l
2
2
+ m
2
l
1
l
2
cos
2
m
2
l
2
2

C(,

) =

m
2
l
1
l
2
(sin
2
)

2
(2

1
+

2
)
m
2
l
1
l
2
(sin
2
)

2
1

K() =

m
1
gl
1
sin
1
+ m
2
gl
1
sin
1
+ m
2
gl
2
sin(
1
+
2
)
m
2
gl
2
sin(
1
+
2
)

5. In Equation 1.11, the term K() represents the gravitational force, C(,

) reects
the centripetal and Coriolis forces.
6. The matrix M() has as determinant m
1
m
2
l
2
1
l
2
2
+ m
2
2
l
2
1
l
2
2
m
2
l
2
1
l
2
2
cos
2

2
which is
positive for all .
2
7. Therefore Equation 1.11 is equivalent to the vector equation

= M()
1
[C(,

) K() + u] (1.12)
8. Equation 1.12 describes the dynamical behavior of a two-link robot manipulator.
It clearly constitutes a non-linear control system with state space as
(
1
,

1
,
2
,

2
) S
1
R S
1
R

= TS
1
TS
1
(1.13)
1.1.1 Cartesian Form of Two-Link Manipulator
Often the purpose of controlling a robot arm is that of using the end-eector for doing
some prescribed task. Hence the Cartesian co-ordinates of the end-point will be a more
interesting output rather than the angles
1
and
2
.
Denoting the Cartesian co-ordinates of the end-point as y
1
and y
2
, the output func-
tions can be given by
y
1
= l
1
sin
1
+ l
2
sin(
1
+
2
) (1.14)
y
2
= l
1
cos
1
+ l
2
cos(
1
+
2
(1.15)
Equation 1.15 is called the direct kinematics of the robot arm.
Problem
How to determine the angles
1
and
2
when the end-position (y
1
, y
2
) is given (possibly
as a function of time). This is the so called inverse kinematics problem for the robotic
arm.
Computing the Jacobian of the right hand side of Equation 1.15 gives
J(
1
,
2
) =

l
1
cos
1
+ l
2
cos(
1
+
2
) l
2
cos(
1
+
2
)
l
1
sin
1
+ l
2
sin(
1
+
2
) l
2
sin(
1
+
2
)

(1.16)
Thus
detJ(
1
,
2
) = l
1
l
2
sin
1
cos(
1
+
2
) l
1
l
2
sin
1
cos(
1
+
2
) (1.17)
= l
1
l
2
sin
2
(1.18)
3
Hence for any point (
1
,
2
) with
2
= k, k R, we see that the rank of J(
1
,
2
) is
2 and hence inverse function theorem can be applied to get
1
and
2
as a non-linear
function of (y
1
, y
2
).
1.2 Spacecraft Attitude Control
The equations describing the attitude control of a spacecraft are basically those of a
rotating rigid body with extra terms giving the eect of the control torques. Therefore
the equations can be separated into
1. Kinematic equations relating the angular position with the angular velocity and
2. Dynamic equations describing the evolution of angular velocity (or equivalently
angular momentum)
1.2.1 Kinematic Equations
The kinematic equations can be represented as follows:-
The angular position is described by a rotation matrix . transforms an inertially
xed set of orthonormal axes, say, e
1
, e
2
, e
3
into a set of orthonormal axes r
1
, r
2
, r
3
(with
the same orientation as e
1
, e
2
, e
3
), which are xed in the spacecraft and have as origin
the center of mass of the spacecraft i.e., e
i
= r
i
, i = 1, 2, 3.
The evolution of may now be expressed as

(t) = (t)S((t)) (1.19)

where (t) is the angular velocity of the spacecraft at time t with respect to the axis in
the spacecraft and S() is a 3 3 matrix dened by
S() =

0
3

2

3
0
1

2

1
0

(1.20)
with = (
1
,
2
,
3
).
An alternative (local) description of Equation 1.20 is obtained as follows:-
4
The angular position may be described locally by three angles , , which represent
consecutive clockwise rotations about the axes r
1
, r
2
and r
3
respectively. Setting r
i
to
be the standard i-th basis vector in R
3
, we obtain the kinematic equations as follows:-

0
0

1 0 0
0 cos sin
0 sin cos

(1.21)
+

1 0 0
0 cos sin
0 sin cos

cos 0 sin
0 1 0
sin 0 cos

0
0

(1.22)
Therefore

1 sintan costan
0 cos sin
0 sin(cos)
1
cos(cos)
1

(1.23)
Clearly this description is only locally valid in the region

2
< <

2
. But it shows
that Equation 1.20 evolves on a three-dimensional space which in fact is the Lie group
SO(3) of 3 3 real orthogonal matrices with determinant 1.
1.2.2 Dynamic Equations
The dynamic equations depend on how the spacecraft is controlled. Here we consider
that the spacecraft is controlled by gas jet actuators.
Let J be the inertia matrix of the spacecraft, h, the angular momentum of the space-
craft with respect to the inertial axes e
1
, e
2
, e
3
and b
1
, b
2
, b
3
, ..., b
m
the axes about which
the corresponding control torque of magnitude ||b
i
||u
i
is applied by means of opposing
pairs of gas-jets. Here ||.|| denotes the standard Euclidean norm on R
3
.
Using a momentum balance about the center of mass one obtains the dynamic equa-
tions for the controlled spacecraft as

R = R S() (1.24)
J = S()J +
m

i=1
B
i
u
i
(1.25)
5
The spacecraft attitude control model Equation 1.25 shows that the dynamics are
typically non-linear for two reasons:-
1. The state space of Equation 1.25 equals the Cartesian product SO(3) R
3
with
determinant 1.
2. The non-linear terms
i
r
jk
appear where R = (r
jk
with j, k = 1, 2, 3.
Both phenomena are essential in a further analysis of the controlled aircraft.
It is easily seen from the model of Equation 1.20 that (R, , u) = (I
3
, 0, 0) forms an
equilibrium for the system given by Equation 1.25.
Linearising the dynamics 1.25 around (I
3
, 0, 0) yields

R = 0 (1.26)
J

=
m

i=1
b
i
u
i
(1.27)
Thus the linearised model 1.27 does not reveal any of the essential features of the original
model 1.25, like for instance stability, controllability, etc.
Hence for a better understanding of the controlled spacecraft one has to develop a
non-linear analysis rather than studying the linearisation of such a model.
1.3 Flexible Link Control
6
References
[1] H. Nijmeijer and A. van der Schaft, Nonlinear Dynamical Control Systems. New
York: Springer-Verlag, 1990.
7