Page 241 - Mechatronics for Safety, Security and Dependability in a New Era

Ch46-I044963.fm Page 225 Tuesday, August 1, 2006 3:57 PM
1, 2006
3:57 PM
Page 225
Tuesday, August
Ch46-I044963.fm
225
225
our localization experiments. We approximate the buildings on the map to the polygons and compute
the uncertainties of their poses and dimensions for estimating the uncertainty of robot pose from the
map matching.
inap. tsst "

Zl
i1 S2[
"I 1
u 1
us
tf1
Tl M3 /
l±z M 4
12

(a) (b)
Figure 2: A guide map of our university campus (a) and an example of rough map (b).

LOCALIZATION
The robot matches the extracted planar surfaces from the disparity image to the building walls on the
map using the Mahalanobis distance criterion. Note that the distance is a quantity which is computed
in the disparity space. The disparity space is constructed such that the x-y plane coincides with the
image plane and the disparity axis d is perpendicular to the image plane.
The map matching provides for a correction of the estimated pose of the robot that must be integrated
with odometry information. We use an extended Kalman filter for the estimation of the robot pose
from the result of the map matching and this integration (DeSouza, et al. 2002).

Kalman Filter Framework
Prediction

The state prediction X(k+l|k) and its associated covariance 2x(k+l|k) is determined from odometry
based on the previous state X(k|k) and 2x(k|k). The modeled features in the map, M, get transformed
into the observation frame. The measurement prediction z(k+l) = H(X(k+\ |k), M), where H is the non-
linear measurement model. Error Propagation is done by a first-order approximation which requires
the Jacobian Jx of H with respect to the state prediction X(k+1 |k).

Observation
The parameters of features constitute the vector of observation Z(k+1). Their associated covariance
estimates constitute the observation covariance matrix _R(k+l). Successfully matched observation and
predictions yield the innovations

(1)