HAL Id: hal-00845193
https://hal.archives-ouvertes.fr/hal-00845193
Submitted on 16 Jul 2013
HAL is a multi-disciplinary open access archive for the deposit and dissemination of sci- entific research documents, whether they are pub- lished or not. The documents may come from teaching and research institutions in France or
L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires
Interval analysis for guaranteed and robust nonlinear estimation in robotics
Eric Walter, Luc Jaulin, Michel Kieffer
To cite this version:
Eric Walter, Luc Jaulin, Michel Kieffer. Interval analysis for guaranteed and robust nonlinear es- timation in robotics. Nonlinear Analysis: Theory, Methods and Applications, Elsevier, 2001, 47, pp.191-202. �hal-00845193�
nonlinear estimation in robotis
Eri Walter a
, Lu Jaulin
and Mihel Kieer a
a
Laboratoire desSignaux et Systemes, CNRS{SUPELEC{UPS
91192 Gif-sur-Yvette,Frane
LISA, Universite d'Angers, 2 BdLavoisier, 49045 Angers,Frane
Abstrat
One ofthe hallenges of intervalanalysisis to explore and bridge thegap between
trivialillustrativeexamplesforwhihitisnotreallyneededandatualompliated
appliationsforwhihitisstillpowerless. Twoexamplesof appliationspertaining
to this gap are presented in this paper. The rst one orresponds to the forward
kinematiproblem fora Stewart-Gough platform, a benhmark for numerial and
symbolial omputations. All real solutions are isolated in a guaranteed manner.
The seond example is relative to the loalization and traking of a vehile in a
partiallyknownenvironmentfrom distanemeasurementsprovidedbysonars.The
unavoidable presene of outliers is taken into aount, whih makes the method
atuallyappliable.Noneoftheseproblemsanbesolvedsatisfatorilybytheusual
loalnumerialmethodsbasedoniterativerenements,andtheadvantagesprovided
byanapproah basedon intervalanalysis areevidened.
Key words: intervalanalysis,outliers,robotis, robustestimation, state
estimation,Stewart-Goughplatform
1 Introdution
Intervalanalysis (IA)makesitpossibletoobtainnumerialsolutionsonom-
puterstosuhbasiproblemsassolvingsetsofnonlinearequationsorinequal-
ities or minimizing nononvex ost funtions. These numerial solutions are
provided under the form of sets guaranteed to ontain all atual solutions of
the initial mathematial problem. This is a onsiderable advantage over the
usual numerial methods that deliver a point estimate obtained by iterative
renement of some initialguess, withoutany guarantee of exhaustivity.
lems of pratial interest turn out to be too omplex to be handled. One of
the hallenges of IA is thus to explore and bridge the gap between trivial il-
lustrative examples for whih it is not really needed and atual ompliated
appliations forwhih itis stillpowerless.
The purpose ofthis paper istopresent two nontrivialbut workable examples
taken fromtheeldofrobotis.Neitherofthemanbesolvedsatisfatorilyby
the usual loal numerial methods based on iterative renements. Given the
spaeavailable,theirpresentation willbeskethy, but referenes areprovided
formoreinformation.The rstappliation,onsideredinSetion2,isalassi-
alproblemofparallelrobotis,whihhas beomeabenhmarkfornumerial
and symbolialomputations.The seondappliation,desribed inSetion3,
istheloalizationandtrakingofamobilerobotfromdistane measurements
provided by sonars.
2 Forward kinemati problem for a Stewart-Gough platform
Fig.1.Stewart-Goughplatform
A Stewart-Gough platform (SGP) onsists of abase and amobile plate,on-
netedby sixlimbswith variablelengths(Figure1).Byatingonthe lengths
of theselimbs,one an modify the positionof the mobileplate relativetothe
base. This devie is an example of a parallel robot, as opposed to an artiu-
latedarm wheretheeetors attahed totheartiulationsat inseries.SGPs
areused inightsimulators,aswellasinmanyotherappliationswherefore
andpreisionarerequired.Whatisknownastheforward(ordiret)kinemati
the mobile plate relativetothe base given
the positions a(i) (i= 1; :::;6) of the onnetions between the limbsand
base,denedinaframeR
0
attahedtothebasebythenumbersa 0
1 (i), a
0
2 (i)
and a 0
3 (i);
the positions b(i) (i= 1; :::;6) of the onnetions between the limbsand
mobile plate, dened in a frame R
1
attahed to the mobile plate by the
numbers b 1
1 (i), b
1
2
(i)and b 1
3 (i);
and the lengthsy
i
of the limbs.
The ongurationof the platform is speied by the vetor
x=( 0
1
; 0
2
; 0
3
; ;;) T
; (1)
where 0
1 ,
0
2 and
0
3
are theoordinates oftheoriginofthe frameofthe mobile
plate inR
0
, and where , and are the Euler angles of the transformation
from R
1 to R
0
. The model omputing the vetor y
m
of the lengths of the
limbsas a funtionof the onguration xan bewritten asin Table 1.
The forward kinematiprobleman nowbe formulated as that of omputing
all x's suh that y
m
(x) = y, where the numerial value of y is known. This
problemhasgeneratedalotofheatamongmathematiians,mehaniiansand
omputeralgebraists. Itisknown that thereare atmost40omplexsolutions
in the most general ase, but of ourse only the real solutionsare of interest.
Hansen's algorithmfor sets of nonlinear equations [1℄ an beused to solve it
[2℄ and [3℄. This involves a guaranteed numerial searh in a six-dimensional
box of onguration spae, whih is hosen large enough to enlose all so-
lutions. Among the advantages of the IA approah, one may note that the
problem is easily treated on a personal omputer even in the most general
asewherethebase andmobileplatearenonplanar,thatalltherealsolutions
areobtained(and onlythem),thatthetrigonometrifuntionsare handledas
suhwithouthavingtoperformanoverparametrizationtomaketheequations
polynomial and that the numerial results are provided with a reliable eval-
uation of their auray (eah onguration vetor onsistent with the data
is isolated in a very smallbox guaranteed to ontain it). Last and not least,
IA allowsunertainty inmeasurementsand geometriparameters tobetaken
into aount.
ThisproblemevidenestheapabilityofIAtosolveompliatedsetsofnonlin-
earequationsinanexhaustiveandguaranteedmanner.IAisalreadyompeti-
tivewithmethodsbasedonomputedalgebra,overwhihithastheadvantage
ofprovidingaguaranteedevaluationofthenumerialaurayofthesolutions
thatitdelivers. Muhremainstobedone,however, tospeed upomputations
Model omputingthelengths ofthelimbsasfuntionsof theongurationvetor
input:
0
1
; 0
2
; 0
3
; ;;
r
11
:=os os sin ossin;
r
12
:= os sin sin osos;
r
13
:=sin sin;
r
21
:=sin os+os ossin;
r
22
:= sin sin+os osos;
r
23
:= os sin;
r
31
:=sinsin;
r
32
:=sinos;
r
33
:=os;
fori:=1 to 6
b 0
1
(i):=
0
1 +r
11 b
1
1 (i)+r
12 b
1
2 (i)+r
13 b
1
3 (i);
b 0
2
(i):=
0
2 +r
21 b
1
1 (i)+r
22 b
1
2 (i)+r
23 b
1
3 (i);
b 0
3
(i):=
0
3 +r
31 b
1
1 (i)+r
32 b
1
2 (i)+r
33 b
1
3 (i);
y
m (i):=
p
(a 0
1 (i) b
0
1 (i))
2
+(a 0
2 (i) b
0
2 (i))
2
+(a 0
3 (i) b
0
3 (i))
2
;
end for
output: y
m
(i); i=1;:::;6.
that take up toa quarter of anhour onpresent day personal omputers.The
next setion will illustratethe apability of IA to deal with ompliated sets
of nonlinear inequations.
3 Loalization and traking of a vehile
Theautonomousloalizationofavehileinapartiallyknownenvironmentisa
keyissue inmobilerobotis.Theproblemonsideredinthissetionistheesti-
mationofthepositionandorientationofavehilefromdistanemeasurements
provided by a belt of on-board sonars. The simpler ase where the vehile is
immobileis treated rst, beforeextending the methodology to aommodate
motion.
The three-dimensionalvetor xtobe estimated omprises the positionof the
vehile in the room, speied by the Cartesian oordinates x
1
and x
2
of the
middle of the axis between the front wheels in the world frame (in meters),
and the angle of the rotation aligning the axes of a frame attahed to the
robotwith thoseof the world frame(inradians).The n
s
sonarsof the vehile
deliveravetoryofn
s
distanes tolandmarksofitsenvironmentindiretions
thatarespeiedinthe robotframe.Toestimatexfromy,oneneedsamodel
y
m
(x), desribing howthe distane measurements are expeted todepend on
the ongurationvetor x, and amap of theenvironment.The mapavailable
to the robot onsists of a olletion of line segments at known positions in
theworld frame,whihrepresent thelandmarks(walls,pillars,pieesoffurni-
ture...). Our (admittedly fairly simplisti)measurement model assumes that
thewavesemittedbythe sonarspropagateinsideones,andthat thedistane
reported by a given sonar orresponds to that to the losest linesegment at
leastpartly loated inthe emissionone.Forany given pointongurationx,
it isthen possible to ompute the n
s
expeted distanes y
m
(x), whih should
maththen
s
atualdistanesy.Sinedimxdimy,theequationy
m
(x)=y
usuallyhas nosolutionfor x,beauseofthe unertainty inthe measurements
and of the approximate nature of the model. It is therefore desirable to nd
allvalues ofxthat are onsistentwith the distane measurementsgiven their
unertainty. Basedon laboratory measurements,it is possible toharaterize
the unertainty of the distane y
i
provided by the i-th sonar by using an in-
terval [y
i
℄ instead of a single numerial value. The vetor y is then replaed
by an interval vetor (or box) [y℄; and we are looking for the set S of all
ongurations that are onsistent with the map and distane measurements
S=fx2[x
0
℄ jy
m
(x) 2[y℄g; (2)
where [x
0
℄ is a searh box in onguration spae, hosen large enough to be
guaranteed to ontain allongurations of interest.
TheSIVIAalgorithm(forsetinverterviaintervalanalysis[4℄,[5℄) anbeused
topartition[x
0
℄intothree setsof nonoverlapping boxes (subpavings), namely
Sonsisting of those that have been proved tobelong to S;Sonsisting of
those boxes for whih nothing has been proved yet and a set of boxes that
have been proved not to belong to Sand an thus be disarded. As a result,
Sisbraketed between inner and outer approximations:
SSS=S[S: (3)
there is no onguration onsistent with all measurements. If x
is the (un-
known) atual onguration of the robot, then y
m (x
) 2= [y℄: This is due to
the presene in y of outliers, i.e., of distanes that do not satisfy either our
model or our bounds on the measurement errors or both. There are many
reasons for the presene of suh outliers, besides the already mentioned sim-
plisti nature of the model. The map may be partly outdated, a sensor may
befaulty,peoplemay haveintereptedbeamswith theirlothes,orthere may
be multiple reetions... The point is that unless the presene of outliers is
taken into aount, the loalization proedure remains an aademi exerise
without potential for appliation. The strategy that we have eleted onsists
of aepting that q out of the n
s
distane measurementsmay beoutliers,and
ofharaterizingthe setS q
ofallxin[x
0
℄thatareonsistentwithn
s
qofthe
distane measurements (see [6℄, [7℄). It is important to understand that this
an be done by SIVIA without speifying whih of the n
s
distane measure-
mentsare outliers,soombinatorialexplosion isavoided. A possible poliy is
tostart assumingthat there is nooutlier (q=0) and toinrement q until S q
beomesnonempty.Assomeoutliersmaygoundeteted,itissafertoinrease
qbeyondthisminimalvalue,butthisinreases thesizeofS q
;soaompromise
must bestruk between robustness and auray of the loalization.
Fig.2.Map of theenvironment of therobot
Example 1 Figure 2 presents the map of the environment in whih the n
s
=
24 sensors of the robot have produed the emission diagram of Figure 3. If
there were nooutliers, there wouldbe a linesegmentof the map atleastpartly
between eah of the pairs of ars of irles thatmaterialize the unertainty in
thedistaneanddiretionofmeasurementsforanygivensonar.Itisneessary
to assume that there are at least q =3 outliers to obtain a nonempty set for
theestimated onguration. Figure 4represents S 3
asomputed bySIVIAand
its two-dimensional projetions, and Figure 5 shows two ongurations that
are onsistent with all measurements but three. As an be seen, there are two
types of radially dierentsolutions,and thisisdue to aloalsymmetry in the
map. Let us stressthat the fat that the solution is notunique is not a defet
of the method. One should instead be thankful that the ambiguity in the data
has been revealed.
Fig. 4.Solutionset inongurationspae and its 2Dprojetions
3.2 Traking
Assume now that the vehile is in (slow) motion. By exat disretization of
the kinemati equations, a nonlinear disrete-time state-spae model an be
obtained as
x
k+1
=f
k (x
k
;u
k
;v
k
); (4)
where x
k
is the onguration of the vehile (now a funtion of time); u
k is a
knowntwo-dimensionalontrolvetor,onstantbetweentimeskandk+1;and
v
k
is an unknown state perturbation vetor that aounts for the unertain
desription of reality by this model. Assume further that a vetor y
k of n
s
distane measurements is obtained at time k, whih will be modeled by the
observation equation
y
k
=y
m (x
k )+w
k
; (5)
m k
is the measurement noise. The problem to be treated is then to estimate x
k
inreal time fromthe informationavailableup totime k, i.e.,
I
k
= n
[x
0
℄;fu
i
;y
i
;[v
i
℄;[w
i
℄g k
i=0 o
; (6)
where [v
i
℄ and [w
i
℄ are known boxes respetively assumed toontain v
i and
w
i .
As in Kalman ltering, the proedure for state estimation alternates a pre-
dition phase, during whih an outer approximation S
k +
of the set S
k + of
all x
k+1
that are onsistent with I
k
is built, and a orretion phase, during
whih I
k+1
, whih inludes the new data vetor y
k+1
, is taken into aount
to update S
k+
into S
k +1
. The atual state x
k+1
is not hanged by this op-
eration, so the orretion algorithm boils down to the algorithm for stati
loalization, with [y℄replaed by [y
k+1
℄=y
k+1 [w
k+1
℄and [x
0
℄ replaed by
S
k+
. The same strategy an be used for protetion against outliers, with q
replaed by q
k+1
. The fat that S
k +
is usually muh smaller than [x
0
℄ speeds
uptheproess.Duringpredition,S
k+
isomputedasanouterapproximation
of the set f
k (S
k
;u
k
;[v
k
℄) by using an inlusion funtion assoiated to f
k . As
the pessimism of this inlusion funtion dereases with the widths of its box
arguments, [v
k
℄ and the boxes of S
k
are split into smaller subboxes before
omputingtheir ontributions toS
k+
. Theresultingimageboxesoverlap,and
a nal transformation is performed to make S
k +
a subpaving. The resulting
state estimatorisa bounded-error nonlinear ounterpart toKalmanltering,
whih hasnoequivalenttothebestof ourknowledge.Formore detail,see [8℄,
[9℄.
Example 2 Figures 6and7illustratethetrakingofarobotstartingfromthe
situation desribedin Example 1. Their right-hand sides show the projetions
onto the (x
1
;x
2
) plane of the solution sets from k =0 to the value indiated.
Their left-handsides show,in ontinuouslines,the emissiondiagramsof on-
gurations belonging to the solution set for the value of k indiated. Up to
k =7, there are two radially dierent types of ongurations thatare onsis-
tentwiththedata.Oneofthemiseliminatedbythedataolletedatk=8,see
the emission diagram in dashed lines at the bottom of Figure 6. The presene
of outliers does not prelude aurate traking.
Autonomous robot loalization and traking are well amenable to solution
via IA beause the number of parameters or state variables to be estimated
is small. The results obtained are global, and no onguration ompatible
with prior informationand measurementsan be missed.They areextremely
robust, and the estimator used an even handle a majority of outliers. The
present omputing times allow real time implementation for slowly moving
vehiles, but there is ample room for improvement of the methodology, for
the physis of the problemand aommodating other types of sensors.
Theseproblems,aswellasothertypialrobotiproblemssuhaspathplaning,
an thus serve as benhmarks for further studies of the global guaranteed
methods for nonlinear analysis provided by IA.
Aknowledgment: The authors thank INTAS for its support under grant
RFBR-97-10782.
Referenes
[1℄E.R.Hansen,GlobalOptimizationusingIntervalAnalysis,MarelDekker,New
York, 1992.
[2℄O. Didrit, Analyse par intervalles pour l'automatique; resolution globale et
garantie de problemes non lineaires en robotique et ommande robuste, These
dedotorat, Universite Paris-Sud, Orsay(juin 1997).
[3℄O. Didrit, M. Petitot, E. Walter, Guaranteed solution of diret kinemati
problems for general ongurations of parallel manipulators, IEEE Trans. on
Robotisand Automation14 (2)(1998) 259{266.
[4℄L.Jaulin,E.Walter, Guaranteednonlinearparameterestimationfrombounded-
error data via interval analysis, Math. and Comput. in Simulation 35 (1993)
1923{1937.
[5℄L. Jaulin,E. Walter, Set inversion via interval analysis for nonlinear bounded-
errorestimation,Automatia 29 (4)(1993) 1053{1064.
[6℄M.Kieer,L.Jaulin,E.Walter,D.Meizel,Robustautonomousrobotloalization
usingintervalanalysis,ReliableComputing6 (3)(2000) 337{362.
[7℄M. Kieer, L. Jaulin, E. Walter, D. Meizel, Nonlinear identiation based on
unreliablepriorsanddata, withappliationto robot loalization,in:A.Garulli,
A. Tesi, A. Viino (Eds.), Robustness in Identiation and Control, Springer,
London,1999,pp.190{203, LNCIS245.
[8℄M. Kieer, Estimation ensemblistepar analyse par intervalles,appliation a la
loalisationd'unvehiule,PhDdissertation,UniversiteParis-Sud,Orsay(1999).
[9℄M.Kieer,L.Jaulin,E.Walter,Guaranteedreursivenonlinearstateestimation
usingintervalanalysis,in:Pro.37thIEEEConfereneonDeisionandControl,
Tampa,Deember16-18, 1998,pp. 3966{3971.
theemission diagram isnotompletely represented; bottom:the emissiondiagram
indashedlines orrespondsto a ongurationthatisno longeronsistent withthe
data
