description of the Monte-Carlo Simulation method (particle filtering) we used in order to imple- ..... Monte Carlo Samples) ..... [28] S. Koenig and R.G. Simmons.
!"!#! $! %!&
!" # #$% !
& " ' " (
) & * + ,
! + $## !+ $##% $
,& - . $ " ! . * &% , / ,0 + + 0& ' & 1 + / / ,0 * + ! 2 & + 2 & & & *
- * & 3 04 '
, (
+ 0' &
& "
5
" % ' , ,& &* 6 * + ,&
* & , 7 + ' *
& & &%' & 2 ' 8 ,& &% + , *
%
,& '
& "
' & 9 : % 9 " 3 4% 9 7 9 5 %' , ' & ,& % *
9 , 0, 9 5% , 2 9 % ,' 2 , & ,&
& 8 + & 5 * & , ( ' , 0 2 ;' ,& 9 % < ' 9 % 7 9 5 % ' ' ,&
,,
% = & &7 ,& , , ' , 1
& , 2 & " 2 / >' ? ,%' , ( + @ @
, , ' ;& 5% 8
: ; Æ : 9 : :' , : 9 @@ % ' , 9 @ A @ & : ' & : & =6 5 !6 ' ,
$
+ #7 9 7 9 5 9 7 9 5
= &%
9 A 5< =.. % % 9 %< 9 %
% 5 ! " %% 9 ! 9 %%
%
5%
%
>
Single step translation (σtrs=5, σdrf =1)
Single step translation (σtrs=1, σdrf =5)
40 30
50
50
20
20
10
0
0
0
0
−10
−20
−20
−50
−50
−30
−40 0
20
40 60 80 100 100cm distance travelled
0
−40
50 100 150 200 200cm distance travelled
0
20 40 60 80 100cm distance travelled
100
0
50 100 150 200cm distance travelled
200
150
100
100 50
50
100
200
50
100
0
0
0
0
−50
−100
−50
−50
−100 −100
−150 50
100 150 200 250 300 300cm distance travelled
−100
0
100 200 300 400 400cm distance travelled
−200 0
!"
100 200 300cm distance travelled
300
0
200 400 400cm distance travelled
!"
;& $7 2 , 7 % 9 > 9 5 % 9 5 9 > Æ
Æ
9
>%
5 A !
B 2 1 , '
& ' ' &
% ,
,& & , & ,& 2 &<
+ ,& , && & , 6 & = , , , 6 ,& B ! H + &% ;& ,
& * *& ' , ' &' , ' , & & , * + &, . *&
, & ,
I % ' C
Sample Trajectory of an exploring Robot (*: Tracker, o: Odometer, .: Monte Carlo Samples)
Reproducing Thruns results
450
700 σ Trs :3 cm/m σ Rot :2 Deg/360Deg
400
σ Drft :2 Deg/m
600
350
500
300 Y−axis
400
300
250
200
200
150
100 100
0 50
−100 −400
−300
−200
−100
0
100
200
300
400
500
0
100
600
200
300
400
500
600
700
X−axis
!"
!"
;& 7 % G & - ' % . , C#
%' - Æ
, '
, & 2 % *& ' - , 0 & ,
&
+ !%
,& - , &
+ % 0 &
*& 0 , ,& & J & 0 , 2
; , 6 , & * $5%< , & * & & $$% # !
$
"
?
^φ w Observed Robot−Target (Moving)
φ^
θ^ m x m ,y m
ρ Observing Robot−Laser (Stationary) dx= xm− x s dy= ym−ys ρ = dx2 +dy 2
^θ w
x s,ys ^θ s
^θ =atan2(dy,dx)= ^ + ^ θ θs w ^φ =atan2(−dy,−dx)= ^ + ^ φ θm w ^ Tracker Returns: < ρ, θ, φ^>
^ θ
;& 7 , 0 &
& 9 : , & *% & 9 : , &% 0,' 0 : #:
=6 C7 9
9 : 9 #:
A
$ % :
$ % :
C%
, 9 9 0, % , & *% : #: % & 0 9 &% A 5%% & =6 ?7
A : A :% 9 A : A :% A 5% 9
:
$ A : A :
#:
?%
=6 C ? 6 !6 ' 6
,& & ' & ( : #:%' , 2 , ;' I
9 : 0, 0 & =6 I7
A
9 : 9
#:
$ % :
$ % :
9 :
I%
, 9 9 ,& & : =6 H% + =6 I # 2 % % , =6 H 0 & * , , , ,&
% 9
5 $$
5 $$
5 $$
H%
;& >
=6 H : #:% ,&& *
' ,&& , & ; ' & 9 # # # & 9 5## 5## > ' & =6 C 0 : #:
2 9 =6 H 0& & > % = ,
% * , % ) ' 0 * & :% & % & '
7 Æ
9 % % 9 %
5#%
0 * ,&& ' & ;& C'
;& C ,&& ;& C' + ,&& 2 ! ,& & =6 55% 5 % 9 $$
5 $$
H
5 $$
¼
55%
;& >7 0 ,&& &
=6 ? ,& & , & =6 5$% B A 5% 9 : : 9 3 4 9 % & % A 8
% 9
5 $$
5 $$
5
%$& ,& +
0,' 2 K
,&
7
, ,&%7 K 9
5#
;& C7 0 ,&& & ;& >
, ,
B& " 7
9
K
) & %%7
% 9 L %
" 7 . ' , ,& & 1 ,
, & %
9
K
7
' 55
^ θw Observing Robot−Laser (Moving)
Observed Robot−Target (Stationary)
ρ
^ θ
θm x m ,y m
dx= x s − x m dy= y s −y m ρ = dx2 +dy 2
x s,ys
^ =atan2(dy,dx)= ^θ+ θ θw m ^ =atan2(−dy,−dx)= ^φ+ θ φw s ^ ^ Tracker Returns: < ρ, θ, φ >
φ^ w
θs
φ^
;& ?7 F
& , 9 : : #: % 9 : % 0, 0 9
=6 5 =6 C%
A : 9 $ % : #:
$ %
5%
:
, 9 9 % & &% 0 : #: % 0, & & % 9 % & =6 5
A #: A : % 9 A #: A : % A 5% 9
:
$ A #: A :
:
5%
+ & & ,& , =6 H' 55' 5$
%& '
5$
110
Stationary Robot
Stationary Robot
100
Tracker
100
90 80
80
70 60
60
50
Motion
40
40
Motion
30 20
20
10
Moving Robot
Moving Robot 0
20
40
60
80
100
0
120
0
20
!"
40
60
80
100
120
!"
120
120
Stationary Robot 100
100
80
Stationary Robot
80
60
40
Motion
60
Motion
Tracker
40
Motion
20
Motion
20
0
0
Moving Robot
−20
Moving Robot
−20
−40
−40 0
50
100
150
200
0
!"
50
100
150
200
!"
;& I7 % * % & 0 % % & 0
5
;& I
, *
& #'#' #'5## + *& I & 5## '5## $# ;& I 0 0 & ,&% + 5## ' 5## *& I , & , & ,& 0 &% ;
' *& I , & & ,&
5 + / +
" B + , 0& & ' 5C7$C? $IH' 5HH> $ " /0 G ( " 1 & 0 ' 5$%7$>5 $C' + 5HH? " / 0 ; 0& ! ' I%7$5 $>' $### M / 7 , 0& "##$ ' & #I> #H#' . &' !+'' " I 5 5HH > M /' N =' G ;& %& ' ( ) E ./E 5 >CII5 #>I O + D ' B
' "+' 5HHC C M /' N =' G ;&' B " &7 6 ' 5%7$5 $H' + 5HH? ? M / G6 & ;& " * ' 5$C%7ICH II#' 5HHC I . " /1 + , - =, + ' ' 5HH H M ! ' ! 2' ; + * . %& ' 5C7$ ?' 5HHH 5# D0 .& !& G D +
& "##/ ' ' & $?I $?II' 5HH?
5
55 + ! D M D 0 . 1 , ' 5$%75C 5?C' " 5HH> 5$ ; 0
' B /& ' ; ' . )& & ' 1 ! === ' M 5HHH 5 ; 0
' ; ' B /& ' . "
1 0 ##1' " 5HHH 5 ; 0
+ . G $ 1 & & . 2332 ===' " $##$ 5> + ' E ; ' E ( ) ' ' .& P &' . . =&& . ' M $##5 5C (& 0 " M0 ' E ./E7 #>$5>CI?C> ! & ) ' " $### 5? (& 0 ! Q & P 1 , - ' " ' "E' 5HHC === 5I G ;&' M /' N = B 7 . &' )" "=+" H $5' ) " & ' + +' ) " & ' + +' "' ).+' 5HH 5H ;0 ' .& ' ; + ' R R0 ' R +' D 0 ' R 0 E & & 0 & 0 11 & % ' $' & 5># 5>>' 5HH> $# + (
4 " ' ! &' " ' 5H?
$5 ; (2 ' ! "
' " ' ( P
' Q
" & & 1 ' 5' & 5 5I ===' 5HH> $$ EM (' M . ' +;" . E J & 5 ' 5#$%75#? 55' + 5HH $ " +, / 0 ! &
0& ! ' $H5%7$ $I' 5HHI 5>
$ " +, / 0 7 )& , & 0& ,0 6 ! ' 5' & IH H#I' 5HHI $> M ' F
B-0' M +' " & + = & & 1 * 0 1' & $>5I $>$' . ; ' !+' ).+' + $### $C M ' F
B-0' M +' " & + ; 1 * 0 1' & $>5 $>?' . ; ' !+' ).+' + $### $? = D + , * & '7 8 ' I$. %7> >' 5HC# $I . D& ( . ) & & 0 1' & $#5 $#I ===' 5HHC $H D 1 . N . & & & ===' ' ' ' & $IHC $H#' 5HHI # D 1' .& N' .& E & ' E 0 . . & ' $' & 5$5 5$C ===' + 5HHC 5 D 1 .& E & ! & , ' $' & 5$># 5$>? ===' 5HH $ M M G N& ; B " 1 0& & ' ?%7?C I$' M 5HH5 ! G " & & * 0 ' 5$%7H 5#C' 5HH? M . G' & !' G&0 + ,0 6
& & + ' E ; ' EM (' ' ) ' .& P &' M $##5 > ; G = " (
& & & ' 7 H' 5HH? C ;& G = " 0, & $ & ' & $H $?>' 5HHI
5C
? M " ! 0 +, / 0 + 0&
- ! ' & >?$ >?I' 5HHH I " D1 (& 0 & & ' . &' !+' 5HH === H " 0 ' . 5H?H
' 5 + ' E, R0'
# M& B "' !& D& 0' ; N D 2 & - ,
9 ' $' & H# H>' , ' EM' ).+' 5HHH 5 ; E " !& & "##: ' & CI> CH5' M 5HH $ 0 ' (& 0' = & " 1 & ' ' , ' $## === " 0 & ; ' < ' . ! . ' " (
)' " ' S ' ! ' ; $## 7JJ,,, &
J T J J " 0 + * 1
!" # #$' !
& " ' " (
)' I# ) .' " ' SU ' !+E++ N+ $+?' $## > .& (& + /0 / 0 * &7 + * ,0 1 2333 ' & $HI> $HH$' . ; ' ! ' + $$ $I $### === C .& (& + /0 !
1 7 + 0 * 1 & 2333 ' & $H>I $HC>' . ; ' ! ' +
$$ $I $### === ? .& (& + /0 1 6 + 0+1' & 5?H 5II' D
' ' ).+' F C $### .& I E . F 0 1' 5HHH 5?
H . ! F ' >%7>C CI' B 5HIC ># .' " . ' ! = & M ! ( B &' ' ! ' & 5C? 5H .& P &' 5HH# >5 M .
' + / 0' " ' M " ! 0 F- 1 /
! ' & 5#CI 5#?>' 5HHH >$ . ' ; ' B /& ' ; 0
1 - ' 5#57HH 55' $##5 > E0 P ' / ,-' / D + * 1 & ===' ' ' 5' & ? 5$' " $##$ > ( B' ! B1 ' =, 0 D& 0 & & * 99= ' 5' & >H> C#5 ===' 5HH
+ , 0& + , . +5 . +$ ,0 0&' . + , 1
& 0, ' , & 2 - & + 3 4 $#
" '
&% "' '
* &% D * & $#' I' H & ' 6 ' 2 ' ' .' . ! ,0 & & & 2 H' ># + = D * & =D;%' , 6 $' 55
= D ; 5I
0 1 & C' >' ? D 1 ' 6 , & *' 1' & 0 5' #' $H , ' & 1 , , , , F &
& " ! .
5> ,% 0, 2 2 * 6 , , * & ( $$ 0& & & * &
2 & & 5$' 5' $C' >' 5' & &
1 1 , & 0 % >$' & & $> 6 , $ * & V, & 6 $ 0& & - 6 ?' >5
0& & &% & && & '
C' 5C , ,
, & 7 6 , ' & , ' * %' , &' C & / ; J0 ;' '8% ? !& D 5# '8
! D ; " # 2
0 2 ' & & - I & &
, - 1 ,
; & &
& ' & *& 5C * 0 1 6 C + 6
0 0, 5H' ' $5 + 6 , 0
& 1 1' 0 $ 1 & 5H
& .
& * & $' >' C' >' I' 5' & & *& 5' 5? / && , ,
& + ' " 0 & & $I * 0 0, = ' & & "' &
& & &% & 0
/ & ,0
% %'
; ( 6 ( D * $?' I' > ( = D ; =D;%
1& 6 H' ># ; ' ( , Æ 7 , & & * & , & & , , 6 & &
( $$% 9 9 : ' = 0 %
& =6 5>
9
! %
5>%
,' ,
* , 2 !
< ' + ! . !% 2 : + 6 % & = 9 6 5C ( )
*+
$#
9(
) %
5C%
, ( ) % 0, 9 0, ' 0, & ' & 7 9 5 %% ' * & & % 0 =6 5?% & & & &
% 9
%
%
5?%
E % =6 5>%' 0,
! 7 9 5 5%' , 9 5 B , , / & , =6 5I% & , , & ( ! ;
1& & =6 5H
7 9 5 % 9
7 9 5
5% 9
%
7 9 5 5% 7 9 5 5%
%
7 9 5
5I%
5%
5H%
+ ,
2 '
+ & D * 6 ' 0 & $5
' B , & >' ?' #' , ( ;' '
F & , , V
% 8 .
8 , ,0 2 0 & F 2 ' & , ,
,
&
& *
;& H7 " &
! " # $ % &
+ ,
2 ' E & . , , $$
2 0 , ,
B & @* @+
& , & '
, & ,
!
!"
!"
;& 5#7 ,
& 0 % / % + = 0, & && & , 3!4 & ,
;& H'5# % ,
&
0 , 3 ,4 ! C $ % "' ,
,
& 8 ' 0 0 & ;& 5#% 0 ,
0
& ( , $ , $
$
#$ & B 2 & ' '
% 2 Error in Rotation from Odometer (for three different speeds) 0.8
0.6
Error (in degrees)
0.4
0.2
0
−0.2
−0.4
−0.6 −50
−40
−30
−20
−10 0 10 Rotation angle (in degrees)
20
30
40
50
;& 557 = 2 & 2 34 5#' 3 4 >#' 3A4 H#' % ;' , 2 2 & ;& 55'5$ ;& 55% ;& 5$%< J & , & , , &
% 1 " ;& 55'5$
& % + ,
& & .& &' & 3A4 *&% 2 2 , ; 2 , H# ,
,
, , V , , V % , F &* &
; ;& 5 , & = & 2 ' , ; 6 &' Æ
$
Error in Rotation from Intended Angle (for three different speeds) 1.5
1
Error (in degrees)
0.5
0
−0.5
−1
−1.5
−2 −50
−40
−30
−20
−10 0 10 Rotation angle (in degrees)
20
30
40
50
;& 5$7 = 2 & 2 ;& 55%
0
; ,
& + 1 ( , 6 & & 0
! , ;& H% , , + 2 , 2 + , 0 +% , & 9 9 # # # % ;& 5>
5## , 5C> 2 V% * & & O R W
5
2
2 $#' C#' 5##% & 5## & &* , & , '
& , ,0 $>
Histogram of Error (E=1.4055 σ=0.2934) 6
8
5 Number of samples
Number of samples
Histogram of Error (E=0.0882 σ=0.0962) 10
6 4 2
4 3 2 1
0 −0.4
−0.2 0 0.2 Error in degrees (Carpet 1)
0 0.5
0.4
12
10
10
8 6 4 2 0
2
Histogram of Error (E=0.7484 σ=0.1477)
12 Number of samples
Number of samples
Histogram of Error (E=0.5296 σ=0.2143)
1 1.5 Error in degrees (Plastic 1)
8 6 4 2
0
0.5 1 Error in degrees (Carpet 2)
0
1.5
0
0.5 1 Error in degrees (Tile Floor)
1.5
;& 57 = 2 H# % Æ
" # , &
& + 5 &
& & 2 ;& 5C 5$# , & ' , O % & V ;& 5?
;' 1
! '
& & , , & & & ( * & & B & , ' ' 6 " ! % ( 0
& ' ,'
& , $C
Histogram of Error (E=−0.4557 σ=0.1334)
Histogram of Error (E=−1.7205 σ=0.2564)
12
6
10
5
8
4
6
3
4
2
2
1
0 −0.6
−0.4 −0.2 0 Error in degrees (Carpet 1)
0 −2.5
0.2
Histogram of Error (E=−0.8371 σ=0.1042)
−2 −1.5 Error in degrees (Plastic 1)
−1
Histogram of Error (E=−0.9922 σ=0.1654)
10
7 6
8
5 6
4
4
3 2
2 0 −1
1 −0.8 −0.6 −0.4 Error in degrees (Carpet 2)
0 −1.5
−0.2
−1 Error in degrees (Tile Floor)
−0.5
Error distribution from intended angle (−90°)
;& 57 = 2 H# % Æ
& , &' 2 , V B & @* @+ & , & '
, & ; &
, ( , 1 * (
& , (
& , 2 & , & , V
& 3 4
! + , , $55 & , & 6 $# - ./ 012 3 ,
$
$?
Histogram of error in X
Histogram of error in Y
30
25
25
20
20 15 15 10 10 5
5 0 −2
−1 0 1 Error in cm (E=0.0924 σ=0.7906)
0 −2
2
Histogram of error in Θ
−1 0 1 Error in cm (E=0.3422 σ=0.6201)
2
Position of the Robot
30
2
25 1 20 Y
15
0
10 −1 5 0 −1
−2 85
−0.5 0 0.5 1 Error in degrees (E=0.0601 σ=0.3075)
90
95 X
100
105
;& 5>7 = 5## V' 5C>
@ % 9 A @ A C#
$#%
! " & Æ & , , , 0
7 *' & , . ' & & 6 /, * B 2 - & ' & & ' 1 6
,
, ;& 5I% , - '
& ' * ' 4 ( ; & && , 9 A A ' ,
$I
57 " . & O'R % W &% 5## 2
,
;& 5I% ; , & %
9 5 $' 9 9
6 $5
9 : : , : 9 9 A A
9 A 9 A % A % 9 % A % A $ % , % 9 # ) ' % 9 % 9
9 $ 9
$5%
$
"
' 6 , & ' , 7 9 % - 7 ;& 5I
,
=6 $$ ' & &
$H
Histogram of error in X
Histogram of error in Y
7
5 Number of samples
Number of samples
6 5 4 3 2 1 0
0
1 2 3 Error in cm (E=2.4488 σ=0.9003)
4 3 2 1 0
4
0
0.5 1 1.5 Error in cm (E=0.9771 σ=0.4167)
Histogram of error in Θ
Position of the Robot
5
2
4
1
3 0
Y
Number of samples
2
2 −1
1 0 0.5
1 Error in cm (E=1.0867 σ=0.1810)
−2 105
1.5
110
115 X
120
125 Plastic
;& 5C7 = 5$# '
A @ A % A % 9 9 A @ A % A %
A A
$
%$4 ( + 9 A ,
& B 1 ( , , , ( 6 $5% '
E 9 A 7
9
+
, A
9 # 1
9
$% #
Histogram of error in X
Histogram of error in Y
10
8
Number of samples
10
Number of samples
12
8 6 4
6 4 2
2 0 −4
−3 −2 −1 0 Error in cm (E=−1.4318 σ=0.8155)
0 −1
1
−0.5 0 0.5 1 Error in cm (E=0.5329 σ=0.3851) Position of the Robot
8
2
6
1
4
0
Y
Number of samples
Histogram of error in Θ
2
0 −0.5
1.5
−1
−2 105
0 0.5 1 1.5 Error in degrees (E=0.6308 σ=0.2508)
110
115 X
120
125
Carpet
;& 5?7 = 5$# ! '
9 : : , : 9 9
9
%
9 A A % A A %
A A
9
9 9
%
A A A % A
%% 9 # )
9
% %
% 9
9
9
$% Εθ 2 θ i+1
x i+1,y i+1 ∆ρ+Ερ Finishing Position Starting Position Εθ 1 x i,yi
θi
5 ;& 5I7 F
' & ' * -' % & & & 6 $>' , "# 5% , ( , 1 & 6
"# 5% 9 "# 5% "# 5%
@ @ $ @
$>%
$
Average values of the std along the X,Y,θ during translation 5.5
5 STD along the X axis
Standard Deviation
4.5
4 STD along the Y axis 3.5
3 STD of the orientation θ
2.5
0
5
10 15 Number of steps per translation
20
25
;& 5H7 #### & 5## & 2 5## )& , 2 & , & & 2 &
+ & & ;& 5H 5#### & O ' R & O ## ' 2 &
+ & & , ,& 1 % , 0
$
&& & ,
& , & ,& && , ,
1% ,& &&
" (
& , 6 ,& Æ ' * ,&
' E & F &%% # 5 ;
' , && & , & '
,&' 6
'
, < ' ,& & && ' && + & 3 , 4 & 7 BE 9 5 S 9 B%< . 9 9 EA5%< " #$ %& 9 %< ' & % EA5% 9 5< 95< -95< " $ % / . 9-< 9A5
&& * . * 34 & 3- 4 & ,
,
%# ( & ! & &
& ,& " & & &
& & , &
,& & >
, 2 &
1
C