Proceedings of the 18th World Congress

The International Federation of Automatic Control

Milano (Italy) August 28 - September 2, 2011

Kinematic Control of Robots with Noisy

Guidance System
Ciro Natale

Dipartimento di Ingegneria dell’Informazione,

Seconda Università degli Studi di Napoli,
Via Roma 29, 81031 Aversa, Italy (e-mail: [email protected]).

Abstract: The main component of a kinematic control scheme for robots is the inverse
kinematic algorithm. The paper proposes to solve the problem for a general robotic system
by approaching it as a state estimation problem for a nonlinear dynamic system. This point
of view allows to adopt any of the state estimation algorithms available in the literature.
The most attractive reason is that such algorithms can cope with measurement noise and
uncertainties, that always affect the guidance system of an advanced robotic system operating
in an unstructured environment. The choice made here is to use an Extended Kalman Filter and
the convergence analysis of the discrete-time algorithm is carried out in a stochastic framework
to explicitly take into account the presence of noise characterised by any type of probability
distribution. A simulation case study is presented to show the effectiveness of the algorithm.

Keywords: Mobile robots, Autonomous robots, Estimation and filtering, Bayesian methods

1. INTRODUCTION tive sensors is corrupted by noise. In such applications of

kinematic control, differently from the classical kinematic
The classical kinematic control scheme for controlling ad- control of industrial robots, besides the accuracy, another
vanced robotic systems with application in unstructured requirement is of fundamental importance, namely the
environments can be found in many textbooks, e.g. (Sicil- computational burden of the algorithm should allow it to
iano et al., 2008). Basically, the robot is guided by a plan- run in real-time. This observation is the main limitation of
ner whose input consists of measurements from exterocep- the approach based on particle filters of Qin and Carreira-
tive sensors and whose output is the reference trajectory Perpiñán (2008). Also, in every sensor-based (visual or
in the task space. Then the trajectory in the task space is force) servoing, when joint space control is used, inverse
usually converted in references for different variables, that kinematics has to deal with noisy signals and thus the
are, usually, directly controlled by a low-level control loop importance of an inverse kinematic algorithm robust to
at dynamic level. For example, in a robot manipulator, sensor noise, or whose behaviour is predictable in presence
the reference trajectory for the end effector, guided by of noise is desirable. The main difficulty in the study of
an artificial vision system, is converted into a reference algorithmic solutions to the inverse kinematics problem is
trajectory in the joint space by inverting the kinematics. related to the discrete-time nature of the dynamic system
Also in the mobile robotics field, the kinematic control at hand, combined with its strong nonlinearity, deriving
approach has been proposed and successfully adopted for from the kinematics. This paper intends to address the
controlling platoon of autonomous vehicles Antonelli and stability of a discrete time inverse kinematics algorithm
Chiaverini (2006), for control of, e.g., formation, navi- in a stochastic framework, and solving it by resorting
gation or entrapment/escorting. In any case, the basic to the EKF. Stochastic stability of the EKF has been
component of a kinematic control scheme is the inverse originally addressed by Reif et al. (1999). However, these
kinematic algorithm. Inverse kinematics of robotic systems results, even though general, provide only boundedness of
has been addressed and solved in a variety of manners the estimation error, while the present paper ensures Lya-
both for non-redundant and redundant robots. For a com- punov stability. The proposed inverse kinematic algorithm
prehensive review, the reader is referred to (Waldron and is shown to be stable with any type of probability distri-
Schmiedeler, 2008) and references therein. The alternative bution of the noise, not necessarily assumed Gaussian. A
approach proposed in this paper can be considered some- simulation case study presents an application of mobile
how dual with respect to the CLIK (Closed-Loop Inverse robotics, i.e. a platoon of robots escorting a moving target
Kinematics) algorithm, which poses the inverse kinematic tracked by a vision system.
problem as a feedback control problem (Sciavicco and Si-
ciliano, 1986). In fact, it is here posed as a state estimation 2. THE INVERSE KINEMATICS PROBLEM
problem for a nonlinear dynamic system, and the solution
is obtained by resorting to the Extended Kalman Filter Let x ∈ X be the vector of task variables of a robotic
(EKF). This choice is made because in a real setting, the system with X a domain of Rn and let q ∈ Q be the vector
desired task trajectory to be computed on the basis of of the robotic system configuration with Q a domain of
information collected from the environment via exterocep- Rn . For example, in a robotic manipulator x is the pose

978-3-902661-93-7/11/$20.00 © 2011 IFAC 6937 10.3182/20110828-6-IT-1002.02932

18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

of the end effector and q is the vector of joint positions, Assumptions v),vi) deserve a more thorough discussion.
whereas in a platoon of mobile robots, q is the vector The degree of smoothness of k(q) and ci (q) needed to sat-
of coordinates representing the location of each robot isfy the assumptions can be easily quantified by applying
and x is the vector of suitable task variables depending Lemma 2 in the appendix. Basically, the nonlinear func-
on the mission. Note that the task space X and the tions have to possess second-order derivatives bounded,
configuration space Q are assumed of the same dimension, and in many applications such a requirement is verified.
which generally means that the robot is not redundant. For example, every robot with revolute joints has a direct
The case of redundant robots can be easily carried to kinematics function constituted by polynomial combina-
such a case by adopting the well-known extended jacobian tions of trigonometric functions of the joint variables,
method for redundancy resolution (Baillieul, 1985), where therefore their second-order derivatives are bounded.
an augmenting map is introduced so that the resulting
extended direct kinematics equation can be always written Let xdh ∈ X, being h ∈ Z the discrete-time variable, be
in the form a desired task space position, the objective of any inverse
kinematics algorithm is to find one of the possibly many
k : Q ⊆ Rn → X ⊆ Rn , x = k(q). (1) configurations q dh such that
This function will be hereafter called “direct kinematics xdh = k(q dh ), (3)
function” or “task function” without distinction. It is
well-known that such a method for redundancy resolution which is a system of n nonlinear equations. In order to
guarantees the so-called repeatability property when in- reformulate the inverse kinematics problem as a state
verting the differential kinematics (Shamir and Yomdin, estimation problem, assume that the vector of configura-
1988), even though it suffers from the so called algorith- tion variables q h to be determined from the knowledge
mic singularities. Moreover, it is assumed the absence of of the task variables xh is the state of a discrete-time
both kinematic and algorithmic singularities within the dynamic system whose output is xh and with state space
configuration space Q, i.e. the jacobian representation
∂k(q)
J (q) = ∈ Rn×n (2)
∂q q h+1 = f (q h , wh ) (4)
is invertible ∀ q ∈ Q. This matrix will be hereafter called xh = k(q h ) + v h , (5)
“robot jacobian” or “task jacobian” without distinction. where the state update function f is in general nonlinear
If not differently stated, hereafter the Euclidean norm will and wh and v h are stationary stochastic processes, both
be considered as vector norm and the spectral norm (the assumed uncorrelated with the state variable, with zero
largest singular value) will be considered as matrix norm. mean and covariance matrices W > 0 and V ≥ 0, re-
The smallest and largest singular values of a matrix A will spectively. V is assumed to be positive semi-definite with
be denoted with σ(A) and σ̄(A), respectively. positive norm. Note that V is not necessarily positive def-
Both the direct kinematics function and the robot jacobian inite, as this is very common in many applications, when
are assumed to fulfill the following assumptions: not all the desired task variables come from measurements,
but some of them are assigned by the user, and thus not
i) ∃ δ > 0 : kJ (q)k ≤ δ, ∀ q ∈ Q affected by noise. An example will be presented in the case
ii) ∃ δ ′ > 0 : kJ −1 (q)k ≤ δ ′ , ∀ q ∈ Q study. A key feature of the proposed algorithm is that the
iii) ∃ β > 0 : kJ (q)J T (q)k ≤ β, ∀ q ∈ Q stochastic processes above can have any distribution, while
iv) ∃ β ′ > 0 : σ(J (q)J T (q)) ≥ β ′ , ∀ q ∈ Q classical EKF assumes them to be Gaussian.
v) the function k(q) is smooth enough such that From this point of view, it is straightforward to adopt any
k(q + q̃) = k(q) + J (q)q̃ + r k (q), of the state estimation algorithms available in the liter-
where the reminder r k (q) is such that ature. Of course, the main difficulty is the unavailability
of the state update function f , but this is the classical
∃ νk > 0 : kr k (q)k ≤ νk kq̃k2 , ∀ q̃ : q + q̃ ∈ Q
problem of every target tracking problem Ristic et al.
(2004). In such problems, the usual approach is to assume
vi) for any given matrices X and Y with bounded norm, a simplified model for the state update. Typical choices are
the following matrix function of q the so-called “constant position” and “constant velocity”
−1
models. In this paper the former assumption is considered:

XJ T (q) J (q)XJ T (q) + Y = c1 (q) · · · cn (q)

is smooth enough such that, for i = 1, . . . , n, ∃ ρi > q h+1 = q h + wh (6)

0, νi > 0 such that
∂ci (q) xh = k(q h ) + v h . (7)
ci (q + q̃) = ci (q) + q̃ + r ci (q),
∂q Future developments foresee the use also of the constant
with the reminders r ci (q) such that velocity model or even more complex models. Considering
the desired position in the task space xd (assumed con-
kr ci (q)k ≤ ρi kq̃k2 , ∀ q̃ : q + q̃ ∈ Q stant) as the observed output, this can be corrupted by
and the jacobians such that noise if it is computed by a planner which makes use of
exteroceptive sensors. In this paper EKF will be adopted

∂ci (q)
∂q ≤ νi , ∀ q ∈ Q. to estimate the state q h , and the stability of the algorithm

will be proved. Moreover, xd is assumed belonging to the

6938
18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

reachable task space X, in the sense that it exists at least error bias related to the noise power and to the filter
a configuration q d such that design parameters, thus very helpful in the design process.
k(q d ) = xd , (8) The presentation of a biased estimator is more a value of
the present paper rather than a limitation, since it means
and it is assumed that q d is an interior point of Q. that by resorting to more sophisticated filtering techniques
the inverse kinematics problem with noisy measurements
Taking into account that the state update equation (6) is can be easily tackled, once it has been formulated as a
already linear with the identity matrix as system matrix state estimation problem. The purpose of this paper is to
and that the jacobian of the nonlinear observation function clearly and rigourously show how the most classical state
in (7) is exactly the robot jacobian in (2), the EKF estimation algorithm for nonlinear systems, the EKF, can
equations are set in the classical three-steps form as be effectively used, with all its limitations, to solve the
• prediction step inverse kinematics problem of a generic robotic system in
a stochastic framework. This will pave the way to the use
q̂h+1|h = q̂h|h , q̂0|0 = qi (9) of more recent and powerful state estimation techniques
to solve the same problem.
P̂ h+1|h = P̂ h|h + W , P̂ 0|0 = O (10)
• Kalman gain computation step
3. CONVERGENCE ANALYSIS
K(q̂h+1|h ) =

−1 To make the convergence analysis more easily readable and
P̂ h+1|h J T (q̂h+1|h ) J(q̂h+1|h )P̂ h+1|h J T (q̂h+1|h ) + V (11) to optimize the computations of the inverse kinematics
• update step algorithm, the following change of notation is introduced

in the filter equations (9)–(13)
q̂h+1|h+1 = q̂h+1|h + K(q̂h+1|h ) xd − k(q̂h+1|h ) + vh (12)

q̂ h|h → q h , P̂ h|h → P h
P̂ h+1|h+1 = I − K(q̂h+1|h )J(q̂h+1|h ) P̂ h+1|h (13)
J (q̂ h+1|h ) = J (q h ) → J h , K(q̂ h+1|h ) = K(q h ) → K h .
where q i is the initial robot configuration, I is the n × n With these new symbols, it is easy to see that (9)–(13) are
identity matrix, q̂ h+1|h and P̂ h+1|h denote the expected equivalent to the discrete-time nonlinear dynamic system
value and covariance matrix estimates, respectively, at with state space equations
time step h + 1 given the observations up to time step
h, and q̂ h+1|h+1 and P̂ h+1|h+1 denote the expected value P h+1 = (I − K h J h )(P h + W ) (15)
and covariance matrix estimates, respectively, at time step q h+1 = q h + K h (xd − k(q h ) + v h ), (16)
h + 1 given the observations up to time step h + 1.
being
In the next section, the convergence of the EKF above will  −1
be proven by showing that the expected value estimate K h = (P h + W )J T
h J h (P h + W )J T
h + V , (17)
q̂ h|h asymptotically converges to one of the possibly many
solutions of (3) with xdh = xd = const. except for a bias and output equation
term, and, in turn, the expected value of the error in the
task space eh = xd − k(q h ), (18)
   
ēh = E xd − k(q̂ h+1|h )) = xd − E k(q̂ h+1|h ) (14) with initial conditions

asymptotically converges to a constant, being E[·] the q0 = qi (19)

expectation operator 1 . It is important to underline that
the estimator will be shown to be biased when only noisy P 0 = O. (20)
measurements are available. This limitation of the EKF is
well-known in the literature (Julier et al., 2000) and it is The expected value of the error in (18) will be shown to
caused by the classical approximation converge to a steady-state value different from zero, but
whose norm decreases for a decreasing norm of the noise
E(k(q)) = k(q̄), covariance matrix. Furthermore, the convergence of the
and the assumption that the estimated state variable algorithm will be proved by proving that the expected
is still Gaussian. The same limiting assumption is also value of the state variable reaches an asymptotically sta-
made in classical EKF convergence results, e.g. (Krener, ble equilibrium. Before stating the theorem ensuring this
2002). Many ways to overcome the problem have been result and whose proof will exploit some useful lemmas in
proposed, e.g. the Unscented Kalman Filter (UKF) and the Appendix, it is necessary to make some preliminary
the Particle Filters (PF). For a thorough review of these considerations. First of all, to take into account the non-
estimation techniques the reader is referred to e.g. Ristic linear dependence of the Kalman gain on the state variable
et al. (2004). One of the novel contributions of this paper q h (see (17)), let g denote the nonlinear part of (16), thus
is that it will not make use of the above assumptions, by defined as follows
completely avoiding to approximate the estimated state
g(q h , v h ) = K h (xd − k(q h ) + v h ), (21)
with a Gaussian random process, although it uses an EKF.
Moreover, it will provide an upper bound estimate of the which is assumed smooth enough to apply Lemma 5. In
1 Hereafter the symbol x̄ will denote the expected value of the virtue of Lemma 5, the expected value of g(q h , v h ) can be
random variable x. computed

6939
18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

E[g(q h , v h )] = g(q̄ h , v̄ h ) + E[r g (q h , v h )] where the inverse of the Kalman gain computed as in (17)
= K(q̄ h )(xd − k(q̄ h )) + r̄ g , (22) always exists since W is assumed to be positive definite
and P h is positive semi-definite by construction. Interest-
where r̄ g is the expected value of the reminder r g , and the ingly enough, the norm of xd − k(q̃ d ) tends to zero as v
symbol K(q̄ h ) indicates the Kalman gain computed using tends to zero since so it does r̄ g , as already anticipated and
the expect value of the state variable, i.e. as it will be shown in the theorem. In this limit case (v = 0

−1
K(q̄h ) = (P h + W )J T (q̄h ) J(q̄h )(P h + W )J T (q̄h ) + V .(23) and thus r̄ g = 0), (32) certainly admits solutions since,
by assumption, xd belongs to the reachable task space
Still in view of Lemma 5, and due to the assumption of of (1), i.e. (8) holds. Therefore, in view of the smoothness
measurement noise v h uncorrelated with the state variable properties of the left-hand side of (32) and the assumption
q h , r̄ g in (22) is bounded as follows that q d is an interior point of Q, by resorting to the
degree theory (Berger and Berger, 1968), it can be shown
T T
kr̄ g k ≤ 2nνg kcov[( q T
h v h ) ]k= 2nνg kblockdiag{P h , V }k that, if v is small enough, it admits solutions even when
≤ 2nνg (kP h k2 + kV k2 )1/2 , (24) v 6= 0 and thus r̄ g 6= 0. With reference to the nonlinear
dynamic system (15)–(18), the next theorem ensures that
where the symbol cov[x] denotes the covariance matrix the expected value q̄ h of the state variable converges, in the
of the multivariate random variable x, and a standard sense that q̃ d is a locally asymptotically stable equilibrium
property of the spectral norm of block diagonal matrices point of the nonlinear discrete-time system (31). Also, the
has been used (see Fact 9.9.49 in Bernstein (2005)). As theorem ensures that the estimated covariance matrix P h
it will be shown in the next theorem, the norm of this is always bounded and the bound tends to zero as v → 0.
reminder tends to zeros as kV k tends to zero.
Theorem 1. Under the assumptions i),ii),v),vi), if the
Hereafter, the covariance matrix W of the process noise measurement noise covariance matrix has norm v small
will be assumed scalar, namely W = wI, w > 0, being enough and selecting the process noise covariance matrix
I the n × n identity matrix. Considering the point of W in (15) such that
view under which the state update model in (4) has
been introduced, such assumption is not restrictive, since W = wI, with w > vδ ′2 , (34)
this matrix is more a design parameter rather than an
actual model parameter. Moreover, for notation economy, then, the nonlinear dynamic system (31) has a locally
the norm of the covariance matrix V will be hereafter asymptotically stable equilibrium in q̃ d , solution of (32),
indicated with the scalar v. Nevertheless, the matrix can and the covariance matrix estimate in (15) is always
be full to take into account that the used sensors can have bounded, i.e.
different noise characteristics and can also be correlated to kP h k ≤ ψ, ∀h ≥ 0, with ψ = vs(v, w). (35)
each other. With this notation, before stating the theorem,
some useful functions of v and w, under the assumption Proof. For brevity, only a sketch of the proof will be
that w > vδ ′2 , are defined as follows: given. First, the covariance matrix estimate P h is shown
to be always bounded starting from the following chain of
wδ ′2
s(v, w) , , (25) inequalities
w − vδ ′2
p
m(v, w) , 2nνg 1 + s2 (v, w) (26)
−1

 −1
−T −T
−1

kI −K h J h k ≤ J h V J h J h V J h + P h + W
δ′
t(v, w) , v + δ 2 (vs(v, w) + w) ,


(27)
w −1
v J h J h−T

which have the following asymptotic properties ≤  , (36)
σ J −1 −T
h V Jh + P h + W
s(v, w) → δ ′2 , as w → ∞ or as v → 0 (28)
where, the first and second inequalities follow from stan-
p
m(v, w) → 2nνg 1 + δ ′4 , as w → ∞ or as v → 0 (29)
dard properties of matrix norm. Then, applying the prop-
t(v, w) → δ ′ δ 2 , as w → ∞ or as v → 0. (30) erties of the smallest singular value and in view of assump-
The theorem below will ensure that the expected value tions ii) and (34), (36) becomes
q̄ h of the state variable reaches an asymptotically stable
vδ ′2
equilibrium. The dynamic equation of the expected value kI − K h J h k ≤ , ε < 1, ∀ h ≥ 0. (37)
of the estimated state variable can be obtained by taking w
the expected value of both sides of (16) and considering
the definition in (21) Next, considering the update equation (15) of the covari-

ance matrix P h and the inequality (37) shown before, it
q̄ h+1 = q̄ h + K(q̄ h ) xd − k(q̄ h ) + r̄ g . (31) results
This nonlinear system admits an equilibrium point if the kP h+1 k ≤ kI − K h J h k kP h + W k ≤ εkP h k + εw, (38)
nonlinear equation
where, again standard matrix norm properties have been


K(q̄ h ) xd − k(q̄ h ) + r̄ g = 0 (32)
applied. In virtue of (37), it is ε ∈ (0, 1) and, by applying
admits solutions. Under this assumption, call q̃ d one of Lemma 6, it results
such solutions, then it is εw
xd − k(q̃ d ) = −K −1 (q̃ d )r̄ g , (33) kP h k ≤ ψ = , ∀ h ≥ 0, (39)
1−ε

6940
18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

where the bound can be expressed as a function of the and where assumption vi) has been considered. Therefore,
design parameter w and the norm v of the noise covariance the Kalman gain can be rewritten as
matrix K(q̄ h ) = K(q̃ d ) + M (q̃ h ) + R(q̃ h ), (50)
wvδ ′2
ψ= = vs(v, w). (40)
w − vδ ′2 where

The boundedness of the covariance matrix estimate implies R(q̃ h ) = ( r p1 · · · r pn ) , (51)

that also the reminder r̄ g in (24) is bounded, i.e.   !
∂p1  ∂pn 
kr̄ g k ≤ 2nνg (ψ 2 + v 2 )1/2 = vm(v, w). (41) M (q̃ h ) = q̃ · · · q̃ (52)
∂q q̃ h ∂q q̃ h
d d

In a similar way, an upper bound of the Kalman gain is which both have bounded norm in virtue of Lemma 3 and
obtained as Lemma 4, respectively, i.e.
(ψ + w)δ δδ ′2
kK h k ≤ = , µ, ∀ h ≥ 0, (42) n
w/δ ′2 1−ε kM (q̃ h )k ≤ νkq̃ h k, ν=n
X
νj (53)
j=1
where, using the definition of ε in (37), the bound can be
n
expressed as a function of v and w as √ X
kR(q̃ h )k ≤ ρkq̃ h k2 , ρ= n ρj . (54)
wδ ′2 δ j=1
µ= = δs(v, w), (43)
w − vδ ′2 It can be then shown that the dynamics in the new state
′2 variable can be written as
which tends to δδ as v → 0. Also, the inverse of the
Kalman gain is shown bounded, i.e. q̃ h+1 = Ãq̃ h + n(q̃ h ), (55)
  where
kK −1 T −T
h k = J h (P h + W )J h + V J h (P h + W )
−1


′ Ã = I − K(q̃ d )J (q̃ d ) (56)
  δ
≤ kJ h kkP h + W kkJ T hk + v ≤
σ(P h + W )



n(q̃ h ) = M (q̃ h ) + R(q̃ h ) xd − k(q̃ d ) − J (q̃ d )q̃ h − r k
(ψ + w)δ 2 + v δ ′


≤ = t(v, w), ∀ h ≥ 0. (44) − K(q̃ d )r k . (57)
w
In virtue of (37), the norm of the matrix à is less than
This implies also that the difference xd − k(q̃ h ) in (33) is one, therefore all its eigenvalues have magnitude lower
bounded, in fact than one as ensured by Fact. 5.10.14 in Bernstein (2005).
kxd − k(q̃ d )k ≤ kK −1 (q̃ d )kkr̄ g k ≤ vt(v, w)m(v, w), (45) Furthermore, the nonlinear part of the dynamics can be
shown bounded by fixing a ball of radius φ around the
where the bounds in (41) and (44) have been exploited. origin, i.e. kq̃ h k ≤ φ, as
As usual when invoking a Lyapunov argument, instead of kn(q̃ h )k ≤ ν̃kq̃ h k, (58)
studying the stability of the equilibrium point q̃ d of the
system (31), it is more convenient to study the stability being
of the origin, which is equilibrium point of the system
obtained with the change of variable ν̃ = δνk s(v, w)φ + νvt(v, w)m(v, w) + νδφ + ννk φ2 +
q̃ h = q̄ h − q̃ d ⇔ q̄ h = q̃ d + q̃ h . (46) + ρφvt(v, w)m(v, w) + ρδφ2 + ρνk φ3 . (59)
It is easy to see that ν̃ can be arbitrary small by selecting
Before computing the dynamics in the new state variable, φ and v small enough, as it is assumed in the theorem
it is convenient to write the function k(q̄ h ) as hypothesis. The asymptotic stability of the origin can be
k(q̄ h ) = k(q̃ d + q̃ h ) = k(q̃ d ) + J (q̃ d )q̃ h + r k , (47) proved by taking the Lyapunov function candidate
Vh = q̃ T
h q̃ h
with kr k k ≤ νk kq̃ h k2 and where assumption v) has been
considered. Also the Kalman gain K(q̄ h ) in (23) can be and recalling that kÃk ≤ ε < 1, hence à has all its
rewritten as eigenvalues with magnitude less than one. 2


K(q̄ h ) = K(q̃ d + q̃ h ) = p1 (q̃ d + q̃ h ) · · · pn (q̃ d + q̃ h ) ,(48)
4. CASE STUDY
being
The case study presented in this section is close to an ac-
∂pj 

pj (q̃ d + q̃ h ) = pj (q̃ d ) +  q̃ + r pj (49) tual application involving sensor-guided robots, and thus
∂q q̃ h the desired position in the task space is actually affected by
d
noise. Consider a platoon of three mobile robots moving on
with a plane, whose positions with respect to a fixed reference

∂p 

frame can be represented by the vector q obtained by
2 j stacking the Cartesian coordinates (q2i−1 , q2i ), i = 1, 2, 3
kr pj k ≤ ρj kq̃ h k , ≤ νj , j = 1, . . . , n

∂q q̃ of each robot, namely q = ( q1 q2 q3 q4 q5 q6 )T .
d

6941
18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

The objective of the mission is the entrapment/escorting 3.5

of a moving target whose position (xd1 , xd2 ) is measured by h = 1000
a vision (or GPS) system, thus the centroid of the platoon 3
has to follow the target, while the robots have to maintain
a fixed distance among each other. Following the approach
2.5
in Antonelli and Chiaverini (2006), the platoon of robots
can be controlled by defining the vector of task variables

[m]
x = ( x1 x2 x3 x4 x5 x6 )T , whose first two entries 2
are the coordinates of the platoon centroid, the next three
entries are proportional to the squared distances from one 1.5 target
robot to another, and the last entry is aimed at fixing the robot 1
orientation of the robots formation. Therefore, the task 1 robot 2
function x = k(q) is defined as follows robot 3
centroid
1/3(q1 + q3 + q5 )
   
x1 0.5
 x2   1/3(q2 + q4 + q6 ) 0 0.5 1 1.5 2 2.5 3
   1/2 (q − q )2 + (q − q )2

 [m]
 x3   1 3 2 4

x  =  2 2 , (60) 3.5
 4   1/2 (q1 − q5 ) + (q2 − q6 )

 x5   1/2 (q − q )2 + (q − q )2  h = 2000
3 5 4 6
x6 q1 − q3 3

whose jacobian, not reported for brevity, has rank 6 as soon 2.5
as the robots are at a positive distance and q2 6= q4 and its
norm is bounded as long as the robots are constrained to
move in a finite region of the plane. Moreover, the Hessian [m] 2

matrices of the task function and of each jacobian column

are constant, hence assumptions i),ii),v),vi) are verified. 1.5 target
robot 1
The mission can be fulfilled by assigning a desired value 1 robot 2
to each task variable, i.e. robot 3

T centroid
xd = xd1 xd2 1/2L2 1/2L2 1/2L2 0 , (61) 0.5
0 0.5 1 1.5 2 2.5 3 3.5
[m]
where L is the desired distance among the robots. Ob-
viously, only the first two task variables are affected by 3.5
measurement noise, which is assumed to have√a uniform h = 3000
distribution with symmetric bounds equal to ± 3 times a 3
3 cm standard deviation in both directions, thus the noise
covariance matrix is 2.5
V = diag{9 · 10−4 , 9 · 10−4 , 0, 0, 0, 0}. (62)
[m]

2
The vision system provides the target position (with a
frequency of 30 Hz), which is assumed to follow a path
1.5
described by the following functions of the time variable target
robot 1
1 robot 2
xd1h = 1 + 2T /tf h (63) robot 3
xd2h = 1 + 0.2 sin(2T π/tf h) + 1.8T /tf h, (64) centroid
0.5
being T = 1/30 s the sampling time and tf = 100 s 0.5 1 1.5 2 2.5 3 3.5
[m]
the total duration of the target motion. The platoon
configuration at the initial time instant is far more than
Fig. 1. Mobile robots and target paths at h = 1000,
50 cm from the desired platoon configuration, and the
h = 2000, h = 3000, with w = 8.2 · 10−6 .
desired distance among the robots has been fixed at L =
40 cm. The configuration q of the three robots needed to Baillieul, J. (1985). Kinematic programming alterna-
accomplish the entrapment/escorting mission is computed tives for redundant manipulators. In Proc. IEEE In-
according to the algorithm (15)–(18), using the parameter ternational Conference on Robotics and Automations,
value w = 8.2 · 10−6 . The results are reported in Fig. 1, (ICRA’85), 722–728. St. Louis, MI.
where the paths of the robots are clearly much less noisy Berger, M. and Berger, M. (1968). Perspectives in Non-
than the desired path and the expected value of the task linearity. W. A. Benjamin, New York, NY.
space error is very small. Bernstein, D. (2005). Matrix Mathematics. Princeton
University Press, Woodstock, UK, 1st edition.
REFERENCES Julier, S., Uhlmann, J., and Durrant-Whyte, H. (2000). A
Antonelli, G. and Chiaverini, S. (2006). Kinematic control new approach for the nonlinear transformation of means
of platoons of autonomous vehicles. IEEE Trans. Robot., and covariances in linear filters. IEEE Trans. Autom.
22(6), 1285–1292. Control, 45(3), 477–482.

6942
18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

√ √
Krener, A.J. (2002). The convergence of the extended kr f (x) ≤ mkr f (x)k∞ = m/2 max |x̃T H(fi (ξ))x̃| ≤
kalman filter. In A. Rantzer and C.I. Byrnes (eds.), i
√ √
Directions in Mathematical Systems Theory and Opti- ≤ m/2 max kH(fi (ξ))kkx̃k ≤ m/2 max νi kx̃k2 ,νf kx̃k2
2
i i
mization, 173–182. Springer Verlag, Berlin.
Lakshmikantham, V. and Trigiante, D. (2002). Theory of
Difference Equations. Marcel Dekker, New York, NY. In the following two lemmas, the symbol ei will denote the
Lang, S. (2006). Calculus of Several Variables. Springer, unit vector with all zero entries but the i-th entry equal
New York, NY, 3rd edition. to one.
Qin, C. and Carreira-Perpiñán, M. (2008). Trajectory
Lemma 3. Given n vector functions r j : q ∈ Q ⊆
inverse kinematics by nonlinear, nongaussian tracking.
Rn → r j (q) ∈ Rm , j = 1, . . . , n smooth enough to satisfy
In Proc. IEEE International Conference on Acoustics,
hypothesis of Lemma 2 such that
Speech and Signal Processing, (ICASSP’08), 2057–2060.
Reif, K., Günter, S., Yaz, E., and Unbehauen, R. (1999). ∃ ρj > 0 : kr j (q))k ≤ ρj kqk2 , ∀q ∈ Q, j = 1, . . . n, (A.4)
Stochastic stability of the discrte-time extended kalman
filter. IEEE Trans. on Automatic Control, 44(4), 714– then, the m × n matrix R(q) = ( r 1 (q) · · · r n (q) ) is
728. such that
n
Ristic, B., Arulampalam, S., and Gordon, N. (2004). √ X
Beyond the Kalman Filter. Artech House, London, UK. kR(q)k ≤ ρkqk2 , ∀q ∈ Q with ρ = m ρj . (A.5)
j=1
Sciavicco, L. and Siciliano, B. (1986). Coordinate trans-
formation: A solution algorithm for one class of robots. Proof. By definition of infinity norm and Euclidean norm,
IEEE Trans. Syst., Man, Cybern., 16, 550–559. each column r j (q) of R(q), in view of (A.4), is such that
Shamir, T. and Yomdin, Y. (1988). Repeatability of
redundant manipulators: Mathematical solution of the kr j (q)k∞= max |eT 2
i r j (q)| ≤ kr j (q)k ≤ ρj kqk ,∀q ∈ Q(A.6)
i
problem. IEEE Trans. on Automatic Control, 33(11),
1004–1009. and for all j = 1, . . . , n. Again by definition and in view
Siciliano, B., Villani, L., Oriolo, G., and Sciavicco, L. of (A.6), the infinity norm of the matrix R(q) is such that
(2008). Robotics. Springer, New York, NY.
Waldron, K. and Schmiedeler, J. (2008). Kinematics. In n
X n
X
B. Siciliano and O. Khatib (eds.), Springer Handbook of kR(q)k∞ = max |eT
i r j (q)| ≤ max kr j (q)k∞ ≤
i i
Robotics, chapter 1, 9–33. Springer, Heidelberg. j=1 j=1
n
X
Appendix A. SOME USEFUL LEMMAS ≤ ρj kqk2 . (A.7)
j=1
The first lemma establishes the degree of smoothness
The claim immediately follows from the well-known prop-
required to some of the nonlinear functions involved in
erty of matrix norms
the paper, in particular those defined in (1),(21) and √
assumption vi). kR(q)k ≤ m kR(q)k∞ .
Lemma 2. Given a vector function defined in a domain Lemma 4. Given n real matrices of dimensions n × n Aj ,
D ⊆ Rn , f : x ∈ D → f (x) ∈ Rm with the Hessian j = 1, . . . , n such that
matrices H(fi ) of all its components fi (x), i = 1, . . . , m ∃ νj > 0 : kAj k ≤ νj , j = 1, . . . , n (A.8)
norm-bounded uniformly in D, i.e.
∃ νi > 0 : kH(fi (x))k ≤ νi , ∀ x ∈ D, i = 1, . . . , m(A.1) and a real vector x = ( x1 xn )T , then
...
Xn
then, ∀ x̃ ∈ D : x + x̃ ∈ D and the whole line from x to k( A1 x . . . An x )k ≤ νkxk, with ν = n νj . (A.9)
x̃ belongs to D, it is j=1
∂f (x)
f (x + x̃) = f (x) + x̃ + r f (x) (A.2) Proof. Recalling the definition of the infinity norm of a
∂x
matrix, it is
and the reminder r f (x) is such that Xn

∃ νf > 0 : kr f (x)k ≤ νf kx̃k2 . (A.3) kAj k∞ = max |eT

i Aj ek |, (A.10)
i
k=1
Proof. The proof is a direct consequence of Taylor’s the-
orem for functions of several variables Lang (2006) written hence, the infinity norm of the matrix ( A1 x . . . An x )
with a second order reminder. This theorem ensures that is such that
it always exists a point ξ on the line connecting x and x̃, n
X
if this entirely belongs to the domain D, such that k( A1 x . . . An x )k∞ = max |eT
i Aj x| =
 T  i
x̃ H(f1 (ξ))x̃ j=1
T
 x̃ H(f2 (ξ))x̃ 
 
n  n n
n X
∂f (x) 1  X  TX
 X
f (x + x̃) = f (x) + x̃ +  .. . = max ei Aj ek xk  ≤ max

|eT
i Aj ek ||xk |
∂x 2 .  i   i
j=1 k=1 j=1 k=1
x̃T H(fm (ξ))x̃ n X
X n n
X
Denoted the last term of the right-hand side with the ≤ max |eT
i Aj ek kxk∞ ≤ kAj k∞ kxk∞ ≤
i
symbol r f , this can be easily upper bounded as j=1 k=1 j=1

6943
18th IFAC World Congress (IFAC'11)
Milano (Italy) August 28 - September 2, 2011

n n
" #
√ X √ X

∂g 
≤ n kAj kkxk∞ ≤ n νj kxk∞ , (A.11) ȳ = E[g(x0 + x)] = E g(x0 ) + x + r g (x) =
j=1 j=1
∂x x0

where assumption (A.8) has been exploited. The claim ∂g 
immediately follows from the well-known properties of = g(x0 ) + E[x] + E[r g (x)] =
∂x x0
matrix and vector norms
= g(x0 ) + E[r g (x)] (A.18)
√
k( A1 x ... An x )k ≤ n k( A1 x ... An x )k∞ and by definition of expected value it is
kxk∞ ≤ kxk. Z Z



kE[r g (x)]k = · · · r g (α)fX (α)dα1 . . . dαn

≤
The following lemma is the basis for proving the conver-
gence of the proposed inverse kinematics algorithm.
Z Z
≤ · · · kr g (α)kfX (α)dα1 . . . dαn ≤
Lemma 5. Given a vector function defined in a domain
D ⊆ Rn , g : z ∈ D → g(z) ∈ Rn smooth enough to Z Z
satisfy hypothesis of Lemma 2, thus ≤ νg · · · kαk2 fX (α)dα1 . . . dαn , (A.19)
∂g(z)
g(z + z̃) = g(z) + z̃ + r g (z), where the property of the expression of g recalled in the
∂z statement of the Lemma has been exploited along with a
where r g is such that standard property of integrals. Now, by applying the same
∃ νg ≥ 0 : kr g (z)k ≤ νg kz̃k2 , ∀ z ∈ D, change of variables as in (A.15) from which immediately
and given a zero mean vector x of n random variables, follows that kαk2 = kβk2 , in view of (A.17), the inequality
characterised by any type of joint probability density in (A.19) is
function fX (x), with a positive semi-definite covariance Z Z
matrix X, then the expected value of the random variable kE[r g (x)]k ≤ νg · · · kβk2 fX (U T β)dβ1 . . . dβn =
y = g(x0 + x), being x0 any deterministic constant 2 , is
n Z Z
ȳ = E[g(x0 + x)] = g(x0 ) + E[r g (x)] X
= νg · · · βi2 fX (U T β)dβ1 . . . dβn
with kE[r g (x)]k ≤ nνg kXk.
i=1
n
Proof. By definition, the covariance matrix of the multi- X
variate randomZ variable x is = νg σi ≤ νg nσ1 = νg nkXk, (A.20)
i=1
Z
X = · · · ααT fX (α)dα1 . . . dαn (A.12) having chosen as matrix norm the largest singular value.

and, in view of its symmetry and positive semi-definiteness, The Lemma below, whose proof is straightforward, is a
its singular value decomposition can be written in the form spacial case of classical results on recurrence inequalities
that can be found in (Lakshmikantham and Trigiante,
X = U T ΣU , (A.13)
2002).
with U orthonormal matrix and Σ = diag{σ1 , . . . , σn } = Lemma 6. Let bh be a non-negative sequence satisfying
U XU T , therefore from (A.12) it follows that bh+1 ≤ αbh + c, (A.21)
Z Z
Σ = U · · · ααT fX (α)dα1 . . . dαn U T = where α and c are non-negative real numbers. If b0 ≤ a0 ,
being a0 the initial condition of the dynamic system
Z Z
ah+1 = αah + c, (A.22)
= · · · U ααT U T fX (α)dα1 . . . dαn . (A.14)
By applying the change of variable then
U α = β ⇔ α = U T β, (A.15) bh ≤ ah , ∀ h ≥ 0. (A.23)

whose jacobian U has determinant equal to one, the

diagonal matrix above can be written as
Z Z
Σ = · · · ββ T fX (U T β)dβ1 . . . dβn , (A.16)

and thus each entry on its diagonal (singular values of the

original covariance matrix) is
Z Z
σi = · · · βi2 fX (U T β)dβ1 . . . dβn , i = 1, . . . , n. (A.17)

Now, the claim of the lemma can be easily proved. In fact,

the expected value of y is
2 Of course, this implies that x0 is the expected value of the random
variable x + x0 .

6944