Page 250 - Introduction to Autonomous Mobile Robots

235
Mobile Robot Localization
1. Robot position prediction. The robot’s position at timestep k + 1 is predicted based
on its old location (at timestep ) and its movement due to the control input u k() :
k
(
,
(
p k +( 1 k) = fp ˆ kk) uk()) (5.46)
ˆ
For the differential-drive robot, p ˆ k +( 1 k) = p' is derived in equations (5.6) and (5.7)
respectively.
Knowing the plant and error model, we can also compute the variance Σ k +( 1 k) asso-
p
ciated with this prediction [see equation. (5.9), section 5.2.4]:

(
⋅
T
(
⋅
⋅
⋅
Σ k + 1 k) = ∇ f Σ kk) ∇ f + ∇ f Σ k() ∇ f T (5.47)
p
p
p
p
u
u
u
This allows us to predict the robot’s position and its uncertainty after a movement spec-
ified by the control input u k() . Note that the belief state is assumed to be Gaussian, and so
we can characterize the belief state with just the two parameters p ˆ k +( 1 k) and
(
Σ k + 1 k) .
p
2. Observation. The second step it to obtain sensor measurements Zk +( 1) from the
robot at time k + 1 . In this presentation, we assume that the observation is the result of a
feature extraction process executed on the raw sensor data. Therefore, the observation con-
sists of a set n of single observations z k +( 1) extracted from various sensors. Formally,
0 j
each single observation can represent an extracted feature such as a line or door, or even a
single, raw sensor value.
The parameters of the features are usually specified in the sensor frame and therefore in
a local reference frame of the robot. However, for matching we need to represent the obser-
vations and measurement predictions in the same frame S{} . In our presentation we will
transform the measurement predictions from the global coordinate frame to the sensor
frame S{} . This transformation is specified in the function h i discussed in the next para-
graph.
3. Measurement prediction. We use the predicted robot position p k +( 1 k) and the map
ˆ
M k() to generate multiple predicted feature observations . Each predicted feature has its
z
t
position transformed into the sensor frame:
ˆ
(
(
(
,
z k + 1) = h z p ˆ k + 1 k)) (5.48)
i i t
We can define the measurement prediction as the set containing all n t predicted feature
observations: