Page 42 - Read Online
P. 42
Ortiz et al. Intell Robot 2021;1(2):131-50 I http://dx.doi.org/10.20517/ir.2021.09 Page 135
useless landmarks. If the new landmark is far from the other landmarks on the map, then the landmark is
added; otherwise, it is ignored. If the distance between the new landmark x +1 = [ +1 , +1 ] and the
others is bigger than min , it should be added into x , i. .,
x +1 = (x ,z ) (14)
It can be transformed into an absolute framework as
x
x +1 = = T(x ,z ) (15)
(x ,z )
ThenonlineartransformationfunctionTalsoappliestotheuncertainties. Weapproximatethetransformation
T by the linearization. P can be expressed as
P P 0
© ª
P = P P 0 ® ® (16)
®
0 0 V
« ¬
where
P = ∇TP ∇T
0 0
I
© ª g g
with ∇T = 0 I 0 ® , ∇g := (x ,z ), ∇g := (x ,z ).
® x z
0
∇g ∇g
« ¬
For the motion part, we use the Ackerman vehicle model [49]
+ −1 −1 cos
© ª © −1 −1 ª
® = + −1 −1 sin ® + w (17)
® −1 −1 ®
−1 tan −1
« ¬ « −1 + −1 ¬
where w is the process noise, is the linear velocity, is the steering angle, is the sample time, and
is the distance between the front and the rear wheels.
At the beginning of map building, the vector ˆx only contains the robot states without landmarks. As explo-
ration increases, the robot detects landmarks and decides if it should add these new landmarks to the state.
! !
x cos( + x )
x +1 = T(x ,z ) , + ,
x sin( + x )
q , ,
2 2 (18)
( − ) + ( − )
© ª
! ®
z = − ® + V
arctan −
®
− )
« ¬
where x, , z, and are defined in Equations (1) and (2),
We exploit the same property in the sliding SLAM. The landmarks with fewer corrections are removed from
the state vector.
, cos(x )
,
ˆ x +1 = ˆx + , sin(x ) + (19)
,
,