Professional Documents
Culture Documents
Gatan GARCIA
Objectives
Understand the general concepts of localization. Understand the interest of hybrid localization algorithms. Grasp the basics of the Kalman filter, which is the most common algorithm in localization problems. Be able to use observability criteria to analyze localization problems.
Course outline
Chapter I Introductory material
Chapter II "bsolute localisation methods Chapter III $elative localisation methods% odometry Chapter I& '#ample of se(uential use of relative and absolute localisations.
Course outline
Chapter & )ybrid localisation method using a Kalman filter Chapter &I *ocalisation and observability Chapter &II +he G,- system
Cha!ter I
Intro uctor" material
ia&ram
3ission
4 ,osition 5, map
4 ,o
sitio n
,erception
onment r i v
Control
'e(initions
Pose of a solid% its position and orientation in 8!, characterized by 9 independent parameters. Localisation% determination of some or all variables of the pose /usually only those necessary to e#ecute a given tas.0. -everal ways to represent the pose. :ot always 9 dof to determine% for e#ample localisation in ;!.
Classes o( localisation
Static localisation% the vehicle is motionless while sensor data necessary for localisation is read and processed. Dynamic localisation% the vehicle moves while the sensor data is read and processed. Quasi-static localisation% the vehicle does move but while sensor data are read and processed, but the motion is negligible. /always chec. the hypothesis0.
Cha!ter II
Absolute localisation metho s
1 /resp. 20 is measured by setting a goniometer above A /resp. B0 and measuring the angular separation between A /resp. B0 and P. )ow to calculate the position of P =
)hin&s to remember
It is often possible to simplify calculations by choosing a convenient local frame. In the case of goniometric systems, it is often useful to use distances as intermediate variables. By linearizing an input>output relation, it is possible to calculate the covariance matri# of the errors on output variables as a function of the covariance matri# of input variables.
-ystem%
Note% the use of distances as @ !educe beacon coordinates in Rm. intermediate variables yields a simpler system @ Calculate mTb. /try with /x,y,0 if @ Calculate 0Tm. you?re not convinced0 @ 1ind b1, b2 and b3.
,roblem% .nowing 1, 1, 2, 2, 3, 3, calculate the position and attitude of the sensor frame RS for .now beacon positions B1 , B2 , B3 .
A !ossible im!lementation
rotation a#is CC! azimuth Beacon /.nown position0 elevation optic a#is encoder
Inter>beacon distances%
/.nown0 (c o n n u e )
BT C i /calculated0 (c a lc u l e )
C i
B
tra6ectory t r a je c t o i r e
O M i
/constant C iT M i ( t r a n s la t io n
OT r s u l taresult0 t f i n a l) M i (/final
+he graph corresponds to a straight line motion between measurement instants t0, ti, tj.
Conse(uences%
-even un.nowns% position < attitude < speed. +he hypotheses must be satisfied during the time needed to detect a sufficient number of beacons to determine the seven un.nowns.
-ow to solve
It is necessary to detect four beacons /one of three beacons seen twice at different locations0 -imilar to previous cases%
Arite beacon coordinates in sensor frame corresponding to time t0 /first beacon detection0. Arite dij2 /there are more than necessary0. -olve in the least s(uares sense. !etermine position and attitude at time t0 plus speed.
Cha!ter III
A relative localisation metho * o ometr"
Beware% there are several forms for the e(uations of odometry /first order e(uivalence0.
Alternate (orms
ey
ex
xest
e % heading error e x % longitudinal error Interpretation in the case of small e% ey % lateral error
Re(erence trajector"
1 0 .8 0 .6 0 .4 x R e f, y R e f (m ) 0 .2 y (m ) 0 -0 .2 -0 .4 -0 .6 -0 .8 -1 -0 .5 0 x (m ) 0 .5 1 0 .8 0 .6 0 .4 xR ef and yR ef
Initial location
0 .2 0 -0 .2 -0 .4 -0 .6 -0 .8 -1 0 1 2 3 t (s ) 4 5 6 7
@ +he real tra6ectory is always the circle% imagine that something forces the robot to follow the circle at a constant speed. @ +he various estimated tra6ectories will be different.
x 10
-3
y /m0
x /m0
0 .0 1 5 0 0 .0 0 5 -5 0 -1 0
ex /m0
2 4 6 8 t /s0 t /s0 0 :o model errors. )igh sampling rate. )igh encoder resolution 0 2 4 6 8
ey /m0
e /B0
t /s0
y /m0
0 -0 .5 -1 0 -0 .5 0 0 .5 1
x /m0
0 .0 2 0 -0 .0 2 -0 .0 4 0 2 4 6 8 -0 .0 6 0 2 4 6 8
-0 .0 5
ex /m0
-0 .1 -0 .1 5 -0 .2
t /s0 t /s0 <CD wheel radius error /right wheel0 is already a severe error E
ey /m0
e /B0
t /s0
y /m0
0 -0 .5 -1
x /m0
0 .0 8 0 .0 6 0 .0 4 0 .0 2 0 0 2 4 6 8
0 .0 3 0 .0 2 0 .0 1 0 -0 .0 1 0 2 4 6 8
ex /m0
ey /m0
e /B0
t /s0
y /m0
0 -0 .5 -1 -0 .5 0 0 .5 1
0 -0 .5 -1 0 x 10
-3
x /m0
0 .0 1
2 1
ex /m0
0 .0 0 5
0 -1
t /s0- 2 0
t /s0
*ow resolution encoders /here CHH dots per revolution0 generate small errors compared to geometrical model errors
ey /m0
e /B0
t /s0
0 .5
0 .5
y /m0
0 -0 .0 0 5 -1 -0 .5 0 0 .5 1 -0 .0 1 0 2 4 6 8
-0 .5
x /m0
0 .0 2 0 .0 1 5
0 .0 1 0 .0 0 5 0 -0 .0 0 5 0 2 4 6 8 -0 .0 1
ex /m0
0 .0 1 0 .0 0 5 0
2 4 6 8 t /s0 t /s0 0 7dometry can do with low sampling rates /here FH )z0. "nd calculations are simple.
ey /m0
e /B0
t /s0
0 .0 0 5
/rd2s0
11
"ctual speed 'stimated speed /CHH dots0 'stimated speed /CHHH dots0
0 0 .1 0 .2 0 .3 0 .4 0 .5 0 .6 0 .7 0 .8 0 .9 1
10
t /s0 " higher resolution /CHHH dots in green, CHH in red0 reduces noise on speed estimate by numerical difference. Useful if you use speed estimation in your control for e#ample.
6
11
"ctual speed 'stimated speed /CHHH )z0 'stimated speed /CHH )z0
/rd2s0
10
t /s0 " higher sampling rate /CHHH )z in red, CHH )z in green0 increases noise on speed estimate by numerical difference.
0 0 .0 1 0 .0 2 0 .0 3 0 .0 4 0 .0 5 0 .0 6 0 .0 7 0 .0 8 0 .0 9 0 .1
calculated position
C;H
120
#B
B3
7dometry o d o m K trie
s e u le
NF LH LF CHH # / m 0 x /m0 +he e#periment was conducted on a lawn on uneven, wet terrain. 'rror C.CF m for a ;9 m path /appro#. G.G D0 with non $ero initial error#
LH9 0
# B1
C
MF
75
80
NH
attitu e !arameters
h o r i z o n t a l plane p la n e )orizontal le ft>h a n d e d o r t h ohanded gonal *eft trih e d ro n orthogonal
trihedron
dc dv dv d" z y
# P
H
d c % g r a d ie n t d v % o r th o g o n a l c r o s s "fa ll
1rame
,endulum
ro llin g p la n e b e tw e e n tim e in s ta n ts i a n d i+ 1
a
7 zi
R n s
&
z i< C
&
v
y i< C
inclinometer measurements
#
i
yi
'rinci le i P # i< C 'ncoders give $ %i .nown 0TSi .nown 0R& .nown rolling plane & in this plane, odometric displacement using a ;! model pro6ection onto R0 gives 'x, 'y, '(, ') and hence %i<1.
i< C
'lementary rotation%
is always defined
Cha!ter IA
9.am!le o( se:uential use o( relative an absolute localisations
;"stem
escri!tion
ym
*inear detector
e M
xm
,ermanent magnet
@ 3agnetic bloc.s are inserted in the ground at .nown locations. @ +he robot is e(uipped with a linear detector which measures the position of the bloc. wrt the center of the detector, when crossed.
0rinci!le o( o!eration
+he principle of operation is similar to the /old0 sailor method. Between two passages over magnetic stations, the robot uses its wheel encoders for relative localisation. Ahen it crosses a magnetic station, it calculates its posture relative to the station /and hence its absolute posture0 and forgets its previous relative localisation. !istance between stations depends on odometry performance.
Rm/t20
*ocalisation result%
Localisation results
1 5 0 -5 -1 0 -1 0 .1 -0 .5 0 0 .5 1 -1 5 0 2 4 6 8
y /m0
0 -0 .5 -1
x /m0
0 .0 5
ex /m0
-0 .0 5 -0 .1 0 2 4 6 8 -0 .1 5 0 2 4 6 8
-0 .0 5 -0 .1
ey /m0
t /s0
0 .0 5
e /B0
t /s0
0 .5
%urther results
Cha!ter A
-"bri localisation metho usin& a 6alman (ilter
)he robot an
B2 y0 ym y +l 2
its sensors
xm B1
+r
B3
x0
+he sensor is a rotating sensor. It detects only one beacon at a time, and not fre(uently /it is slow0. Its motion between two measurements cannot be neglected.
4hat
o we want to 1now5
Ae want to determine the vehicule position and heading ,ITx, y, U+. Ae shall be satisfied with .nowing , at discrete time instants% ,kITxk, yk, kUt with tk<1>tkIT. Ae suppose that ,0 is .nown with a certain precision% Vust start motionless and use the method seen in chapter II.
4hen no beacon is
Use odometry%
etecte
+hese e(uations are the standard form of an evolution model. , is called the state vector.
+ypically, k is characterised by its covariance matri# -. But what to do with - = )hat we want% reflect the fact that our confidence in , decreases with successive applications of odometry e(uations without e#ternal measurements.
)he sensor
3easurement% j
etects a beacon
+he measurement depends on the actual position and heading of the vehicule, in a way we can write e#plicitly%
Ahat to do with j = Ahat information does it bear = If actual and estimated positions were e(ual what would we see = Ae would measure%
W=
@ )ow a small vehicle displacement influences the measurement around the current estimated position% /m#10
B y x
Overall al&orithm
)hat ha ens at time t k *1
++ 'rediction hase, 22 $ead sensors and calculate input% 22 Calculate linear appro#. of system% If an e#ternal measurement . has been obtained during last period then ++ Consider ./ corresponds to time 22 tk<1. ,erform estimation hase, 22 Calculate gain%
++ Update state%
Coherence test
Consider the sensor which detects light beacons. Ahat happens if it detects the reflection of a beacon on a window= It is possible to test the coherence of a measurement by calculating the associated 3ahalanobis distance%
If d S threshold, the measurement is coherent and can be used in the estimation phase. :ote that d depends on the beacon /via 20. Calculating the values of d for each beacon allows to identify which one yielded the measurement% no need for signed beacons
9.!erimental setu!
- I$ ' 3
" vant
H .C m
H .; N F m
C .C F H m
Because the sensor is not above point M, the e(uations used are slightly different from those presented before.
9.!erimental setu!
cam Kra
sen sor A h ite ro p e
C a sto r w h e e l
cam era
* in e a r c a m e r a to tr a c . th e w h ite ro p e
@ +he robot trac.s a white rope tightened between .now points. @ +he points have been precisely localised by surveyors with sub>centimeter accuracy.
;ensor characteristics
H.HLB in azimuth H.HCB in elevation /not used in ;! localisation0 1or a beacon at GH m, it corresponds to 9.F cm error in x>y and N mm in (. -low sensor% C revolution in F s.
9.!erimental results
C,ath h e m in
0 .0 8 0 .0 6 0 .0 4 0 .0 2
120
#B
<8syy curve
/m 0
y /m 0 1 0 5
>8syy curve
t /s0
100 95 90
'#act path
B
filtre ; !
B2B #
;
75
80
85
90
95
100
# /m 0
@ +he /estimated0 is e#tracted from the covariance matri#. @ +he error signal should always be in the <2> 8 interval. @ +he increase of at the end of the path reflects a less favorable beacon>vehicle configuration. @ B2 is sometimes missed at the end% things get worse...
9.!erimental results
18000
0 .1
0 .0 8
16000
14000
0 .0 6 0 .0 4 0 .0 2
12000
10000
20
40
60
80
: u m b e r o f m e a s u re m e n ts o n B C
8000
6000
9.!erimental results
0 .8 0 .6 0 .4 0 .2 8 6 4 2
/d eg 0
0 -0 .2 -0 .4 -0 .6 -0 .8 0 10 0 200 300 400 500
/d eg 0
0 -2 -4 -6 -8 0 10 0 200 300 400 500
t /s0
t /s0
)eading error
9.!erimental results
0 ,0 3 0 ,0 1 /m 0 -0 ,0 1 -0 ,0 3 96 9 1 .5 87 8 2 .5 ' s tim a te d P 78 7 3 .5 69
r o ll a n g le
O errors are repeatable over three tests along the same path
9.!erimental results
f ir s t r e v o lu t io n 0 .2
/m 0
; n d r e v o lu t io n
K a lm a n f ilt e r
o d o m e try
B3 120 115
0 .1 0 -0 .1 -0 .2 -0 .3 -0 .4
y /m 0
110 105 100 95
90
B1 80 90
B2 100
# /m 0
t /s0
800
+rac.ed path
General e:uations
1undamental% +he output vector . must be a function of the state vector only. -ny arameter in the e. ression of 3 which is not from , must be a known constant.
9.ercise
xm y0 ym y d1 x B1/H,H0 d2 B2/d,H0 -tate % Tx,y,U+ Arite the observation e(uation. Calculate matri# C of the Kalman filter x0
M
;ame e.ercise
xm y0 ym y d1 x B1/H,H0
M
d2 B2/d,H0
x0
Cha!ter AI
Localisation an observabilit"
Cotion o( observabilit"
7bservability% possibility to reconstruct the state x .nowing the inputs 4 and the outputs y of the system. Xuestions%
Is state estimation always possible= "re there situations when the estimator diverges=
:ote%
+he system is generically observable if the set of points for which this condition is not satisfied is a zero>measure set /or null set0.
Lie
erivative
Let g be a differentiable real function, and f a real vector function. The Lie derivative of g with respect to f is the vector field:
Ahere
of real>valued function # with respect to state vector ,. f the matrix ! has full ran" #n$ then the system is observable.
; B1
det/510IH on the circle /20 which passes through the three /non aligned0 beacons. 'lsewhere, observability is granted. 7n the circle, we do not .now yet.
If v/ IH then +hen
and
)ence if the translational speed is zero, det/50 I det/510 and ra1k/50 S 8. -till, this does not prove that the system is not observable, because the rank condition is a sufficient condition but is not a necessary condition.
;imulation
300
400
500
600
+he estimator does not converge% it cannot drive the initial error to zero.
d e g re e
) e a d in g ' rro rs
3 00 400 500 600
C o n s ta n t s p e e d I H .; m 2s
t in s
"fter a transient phase, the error curves with and without initial error are 4 the same 5. ,recision is not as good as outside the circle. +his e#ample illustrates the influence of the input on observability, for a non linear system.
"s p S n, observation cannot be obtained using the azimuth angles /outputs0 alone. +he input /speed and rotation speed0 will have to play a part...
If v I H%
+he sufficient condition will never be met if the vehicule does not move. ,retty logical, though. But what if it moves=
)wo beacons an
movin& robot
+he analysis of the ran. of 7 becomes comple# as higher derivatives need to be used. By studying the determinants of the various s(uare sub>matrices of 5 with a formal calculation software, we found three potentially dangerous vehicle motions%
*1 *3 B1
*2
B2
;imulations on D D
1 .5
d e g re e
108 106
104 102
e s tim a te d p a th
0 .5
W ith I n itia l E r r o r
100 98
re a l p a th beacon
81 8 1 .1 8 1 .2 8 1 .3 8 1 .4 8 1 .5 8 1 .6 8 1 .7
96 94
-0 .5
) e a d in g ' rro rs
0 100 200 300 400 500
t in s
600
92 8 0 .9
Ahen the vehicle moves along *1, the observer does not converge. Remark% our observer converges for other straight line paths through B1, other than *1. 7ther researchers tested a least> s(uare observer which did not converge% observability does not guarantee that a particular observer will converge...
;ome conclusions
7bservability analysis allows to find dangerous situations which are sometimes not easy to suspect intuitively. 7bservability is not a 4 binary 5 phenomenon. Ahen the vehicle comes close to non observable situations, the precision degrades and numerical problems can appear. -o much for zero>measure setsE