Proceedings of the 27th Chinese Control Conference

July 16-18, 2008, Kunming,Yunnan, China


 
刘馨，裴海龙，李坚强

  510640
E-mail: liuxin@scut.edu.cn

 :    !"#$%&'()*+',-./01234
45"56789
:#$%&',  +',

Trajectory Prediction based on Particle Filter Application

in Mobile Robot System
Liu Xin, Pei Hailong, Li Jianqiang
South China University of Technology, Guangzhou, China 510640, P. R. China
E-mail:liuxin@scut.edu.cn

Abstract: Effective prediction on the states of moving objects paves the way for successful motion planning. In this paper,
Particle Filter is used to predict the robot position and velocity and the experimental results of target prediction in robot path
planning are presented to verify its performance. Particle filter can also be combined with the system overall strategies to plan its
motion and improve the system performance.
Key Words: Particle Filter, Mobile robot

RS#$%&'TUV010
1 (Introduction) 1/-.9 !"37K*#$%&'TU
3+', :;"<=>?@ABCD HI23445"J)\789
3EFGHI !J)7K*#$%&L
2
 (General Particle Filter)
I2M)NO+',PQ"9RSLI
PQTU-.01/V013WKX BCZ'>[#$%&'
YZTU[\]^_`a9 LI9TUAB { X k , k ∈ N} 
H
3PQ
"N J)93
>
/PQ"%&)*'
[1]%&3 PQ/PQ" X k = f k ( X k −1 ,Vk −1 )
#1$
) !"%&#EKF$%A& nx nv nx
" fk : R × R → R R >  X k −1 
PQ'()5*+ 
,9-./0
,1423456D3789:;%&  !" {Vk −1 , k ∈ N} >#$%&*+'(A
'<=9 B nx  nv *)>/S*'("9 N >
#$%&'>>.3?3 @ +"9 T>WO
,
ABCDEHI9FM)NG.HI[2]9
3#$%&"TU*+J>KLM@N #$
Z k = hk ( X k , nk ) #2$
OPQR?M@STUVWO9-X%&'%( " hk : R z × R
n nn
→ R nz R> !"
Y Z    [ ' # Bayesian bootstrap $ [3]  \ ]
#condensation$[4]CDEI#Monte Carlo$[5] {nk , k ∈ N} >#$%&*+,'(nz nn *
/^_M@`NI#Metropolis-Hasting$[6]9J*a )>/S*'("9,"K
, /PQ#$%&'3< Z1:k = {Zi , i = 1,..., k } >
KX9
=J)" [7-9]93[10,11]"#$%&/+\ 3HI"-./NU0kG?,
N#Gibbs sampler$>J)*O TUT ZWO
LU0kX9
,-N
U

01 2> 1A$)521*+#posterior density function$
H-9
p( X k | Z1:k )  2 3 a  4 5 ,
p( X 0 | Z0 ) ≡ p( X 0 ) >67859 Z 0 >9
389

Authorized licensed use limited to: Naval Science and Technological Laboratory. Downloaded on March 3, 2009 at 05:16 from IEEE Xplore. Restrictions apply.
 ,45, p( X k | Z1:k ) :RS/;<' Bq( )>
@A*+!"9CD)
(WO
L\9 P
Ns
3 )521*+ p( X k −1 | Z1:k −1 ) 3k-1U06 p( x) ≈ ∑ wi δ ( x − x i )
7=>JPQHIRSChapman-Kolmogorov i =1

H*
LNkU08521*+ #7$
-? i?$M@
p( X k | Z1:k ) = ∫ p( X k | X k −1 ) p( X k −1 | Z1:k −1 )dX k −1
π ( x)
#3$ wi ∝
q( xi ) #8$
-?3
p( X k | X k −1 , Z1:k −1 ) = p( X k | X k −1 ) 
@ZPQHI>ABS*9CD*+ -N#6$"M@PZ
p( X k | X k −1 ) JPQHI#1$/67Q
Vk −1 E
9 p( X 0:i k | Z1:k )
wki ∝
3R?U0k , Z k Z2)
FG #9$
q( X 0:i k | Z1:k )
;<8521*+ 3'(
UN/0CDZ
p( Z k | X k ) p( X k | Z1:k −1 ) p( X 0:i k −1 | Z1:k −1 )  N 2N/0CDZ
p( X k | Z1:k ) =
p( Z k | Z1:k −1 ) #4$ p( X 0:i k | Z1:k ) <N 9^8@A*+Z
2'(7UH

p( Z k | Z1:k −1 ) = ∫ p( Z k | X k ) p( X k | Z1:k −1 )dX k q( X 0:k | Z1:k ) = q( X k | X 0:k −1 , Z1:k ) q( X 0:k −1 | Z1:k −1 )

#5$ #10$
i
-?\E*J,HI#2$/67Q
nk   R X 0:k −1 ∼ q( X 0:k −1 | Z1:k −1 )  N N
i
I
CD p( X k | Z k ) 9 X ∼ q( X 0:k | Z1:k ) 9 R p( X 0:k −1 | Z1:k −1 ) 
0:k

3;<[J#4$", Z k )GK485* p( X k | Z1:k ) / p( X k | X k −1 ) NP:ZM@;

+NLM)5*+9#3$/#4$W <Z

ONPOEPQ
LHI)5*+BCRQ p ( Z k | X ki ) p( X ki | X ki −1 )
i i
3-.
HIKX9-.PQ
L)5 w ∝w
k k −1
*+ST>CUY2F9)OVIWX9

q( X ki | X 0:i k −1 , Z1:k ) #11$
HI6YZ[3>\J)?]^%& '>[^8
/_`%&ISaLI^!"% q( X k | X 0:k −1 , Z1:k ) = q( X k | X k −1 , Z k ) @A
&/0_`LI#$%&'/0HI i
*+T X k −1 / Z k N9-NT1AD X k
2.1  (Particle Filter) QF1A7O<=/,9M@
#$%&'>K*ABCDEHIWO SPZ

%&HIK I>JM@T+`N#$ p ( Z k | X ki ) p( X ki | X ki −1 )
OP)5*+2JW
L1PQ
,9Z
wki ∝ wki −1
q( X ki | X ki −1 , Z k ) #12$
i i N
5 6 L I  J { X 0:k , wk }i =s1 P  > ? ) 5 * + Z
)5CD*+/0Z

p( X 0:k | Z1:k )  T +   ,  - ? Ns
i
{ X , i = 1,..., N s )} > M @ # $   
0:k
p ( X k | Z1:k ) ≈ ∑ wki δ ( X k − X ki )
i =1
{wki , i = 1,..., N s )} >M@ X 0:k = { X j , j = 0,..., k } #13$
> / N kU 0       M @ ' ( 7U H
L N s → ∞ U#13$:/14)5CD9
N

∑w
i =1
i
k =19-NkU0*+21/0Z
2.2 (Resampling)
Ns
#$%&'BC>
[%:>WY
p( X 0:k | Z1:k ) ≈ ∑ w δ (X i
k 0:k − X 0:i k )
S !%&)#$M@SZ"9->@
#6$ i =1
ZG#$M@#V$NTUV%&F^_'
M@J: GI

3 CD p( x ) ∝ π ( x ) 
<(#$
[9
)*E;<
L
π ( x ) >( 9 x i ∼ q( x ) i>+"A

390

Authorized licensed use limited to: Naval Science and Technological Laboratory. Downloaded on March 3, 2009 at 05:16 from IEEE Xplore. Restrictions apply.
 / p( X k | Z1:k ) FN)9
*1) 3 (Trajectory Prediction)
#$" N eff GP
!">?T+M TU01/-.)#$
Ns %&'G94KJ)"-.LNMN
N eff =  )7OP'(9
1 + Var ( wk*i ) #14$ #$%&') +',-.32-D
i
p( X | Z1:k )
*i k
9V"(-.JQR-.#x,y$P2
" wk = (L14M@
R67SS,LN-.9x,y*)>2-DT
q( X ki | X ki −1 , Z k )
UPUx, yVTU,9WXMN
,9 N eff 
, Nˆ eff -NN 01/GH>9,YZ>TUa`
1 N-.012Z+',Z[:;Y19
Nˆ eff = Ns ^\1"56<=]?=>

∑ ( wki )2 45

\WX45 p( X 0 ) 17*+N X 0i 
#15$ i =1
1
Z7+,
[1A-.@A*+!" M@>%&*+ , i = 1,..., N 9TU45-./0
] )@`NHI9/0)@A*+!"> N
1L1HI9
q( X k | X ki −1 , Z k ) = p ( X k | X ki −1 ) #16$ 
R?N #$YSPQHIN,
#16$L1#12$N Xˆ ki = f ( X ki −1, wki −1 )
i i i #20$
w ∝ w p( Z k | X )
k k −1 k #17$ i
"w k −1 >D3PQ'( pω (ω ) ^_\
3G.FG@A*+!""K* X ki −1 / 9
Z k L234#$V$N w 85> *i
k ;<
-. Z k ;<)R?#$M@
[14]"6N (
L
i
p( Z k | X k ) p( X k | X ) p ( Z k | Xˆ ki ) p( Xˆ k | X ki −1 )
q( X k | X ki −1 , Z k ) opt = k −1
wki = w ki −1
p ( Z k | X ki −1 ) (18$ q( Xˆ k | X ki −1 , Z k ) #21$
i i
-?!"5>@Z7
X k −1  w 8GN k 2UH

,#$S>R q( X | X
i i
, Z k )opt "9\ZG wki
k k −1
w ki −1 = N
9
@`NHIRS:;M@<#$@=3
∑w i
k −1
i =1 #22$
M@#$RQ3>
[93@`NUR

` > ? K * { X : i = 1,..., N }  a = * +
i
 p( X k | Z1:k ) *+/0N "@\ N s a#$N k

i* N
i* j i {w ki : i = 1,..., N }  CD*+ w ki UVk X ki $
N< { X k }i =s1 9-N pr ( X k = X k ) = wk 
i
N9
NN >#$%&*+ wk = 1/ N s 9
@`N

LN "T
! )T+@`NHI[9]9<#$N R 1
N "?@/,\Z`\%&*+ N eff = N
i :l
U ( x ki:l − α xki :l , xki :l + α xki:l ) 9 xk > xk  i?
i
∑ ( wˆ ) i 2
k

9 α E
\N?<-?,>JN " #23$ i =1

3 !"93+',P
N eff /$N σ x2i:l GI

k Q"4[N/; '(/
σ x2i:l 
"9
α ∝ k

N eff
#19$
LN " N eff 3<U\N?A'!"
B;V*=#$9HC σ 2 >D1
 σ 2 )*E;EFGH/\N?AI#J+9

391

Authorized licensed use limited to: Naval Science and Technological Laboratory. Downloaded on March 3, 2009 at 05:16 from IEEE Xplore. Restrictions apply.
 9+',;$Z/63/
"1239

3*a+ #,$%

1  

4  !(Experimental Results)
K*#$%&'H3+',PQ[15,16]
"N )9PQRS.* YH\
R40ms<9%&'9H
3N"4[ )*+`N"Z200
J'(Z0.010.05m9 3*b+ -./012
8)M '(4593\2"
45-.ZPQTUP#0.750.65$m9
NCK Y<*5cm5CZ[301
19

4 #,345%678

2 "#$%&'
\3"/07 !M(+', *
)")]#")99+',0$%
#& <=$3>[( "'( -9
J)3TU])`a2/
PQ42G9\4567H3+'
,(Z"^_J*E
>L
M (Z"")9945647+LI
89\5"GN45/TU `a
Obstacle1R,#0.150.4$m N-Y+ 5*a+ 90#,5%
',A^.`aR#0.30.2$m !-*YH9
3\5(a)" )9+',)72.5sZ
/(b)"# )U 1A3sZ90)IPF

392

Authorized licensed use limited to: Naval Science and Technological Laboratory. Downloaded on March 3, 2009 at 05:16 from IEEE Xplore. Restrictions apply.
 dionr on Aemrpace and Electmnii: Systems, 1997, 33

338-343.
[4] ZHOU S, KMEGER V, CHELLAPPA R. Face recognition
from video: a condensation approach [J]. Fifth IEEE
lntrrnarionul Conference on Automatic Fuce and Cesrure
Recognition, 2002
212-217.
[5] GILKS W R , Bemini C~. Fallowing a moving target - monte
earlo inference for dynamic bayesian models [J]. Journal of
Royal Statistical Society, 2001, 63:127-146.
[6] AYKMYD R. Bayesian estimation for hamogencous and
ihomogeneous gaussian random gelds [J]. IEEE Tranractions
on Pattern Analysis and Machine Intelligence, 1998,
20:533-539.
[7] LOY G, FLETCHER L , N. Apostoloff, et al. An adaptive
fusion architecture for target tracking [J]. Proc. 5th IEEE Int.
Conf. Autom. Face and Gesture Recog., 2002:248-253.
5*b+ :0#,5% [8] KWOLEK B.Person following and mobile camera localiza-
tion using particle filters[J]. Proc. 4th Int. Workshop Robot
Motion and Control, 2004
265-270.
5 ;(Conclusion) [9] LIU J, PRAHLAD V. Improved particle filter in sensor
fusion for tracking random moving object [J]. IMTC2004
Instrumentation and Measurement Technology Conference,
3 !"K*#$%&HI3 +' 2004.
,PQ"N7J)93/"9 [10] HUE C, -P L C.J, PEREZ P. Sequential monte carlo methods
+',9F'.4Z5^`a/Z/\ for multiple target tracking and data fusion[J]. IEEE
;OUV/9V2PQ*+LI: Transactions on Signal Processing, 2002, 50:309-325.
 49N693)67"'>[ [11] HUE C, -P L C J, PEREZ P. Tracking multiple objects with
particle filtering [J]. IEEE Transactions on Aerospace and
/FGLI3PQ"P[9 Electronic System, 2002, 38:791-812.
[12] CARPENTER J, CLIFFORD P, FEARNHEAD P.Improved
<=>?(References) particle filter for nonlinear problems[J]. Proc. Inst. Elect.
Eng., Radar, Sonar, Navig., 1999.
[1] TAN K H, LEWIS M A. Virtual structures for [13] LIU J S, CHEN R.Sequential monte carlo methods for dy-
high-precision cooperative mobile robotic control [J]. Intel- namical systems [J]. J. Amer. Statist. Assoc., 1998, 93:
ligent Robots and Systems '96, IROS 96, Proceedings of the 1032-1044.
1996 IEEE/RSJ International Conference on, [14] DOUCET A. On sequential monte carlo methods for bayes-
1996,1:132-139. ian filtering [R]. Available as Technical Report.
[2] ARULAMPALAM M, MASKELL S, GORDON N., et al. A [15] CUED/F-INFENG/TR. 310, Cambridge University De-
tutorial on particle filters for online nonlinear/non-gaussian partment of Engineering, 1998.
bayesian tracking [J]. IEEE Trans. Signal Process.,
[16] ALAN K. Mackworth. On seeing robots [R]. Technical
2002,50(2):174-188.
Report TR-93-05, 1993.
[3] BEADLE E , UJURIC P. A fast-weighted bayesian bootstrap
[17] KIM J H, KIM D H, KIM Y J, et al. Soccer Robotics
filter far nonlinear model state estimation [J]. IEEE Tmnso-
(Springer Tracts in Advanced Robotics) [M], 2004.

393

Authorized licensed use limited to: Naval Science and Technological Laboratory. Downloaded on March 3, 2009 at 05:16 from IEEE Xplore. Restrictions apply.