IEEE Robotics & Automation Magazine - December 2013 - 43

Information Fusion for Control
To ensure high-rate, accurate, and drift-free pose estimates for
feedback control, we use two separate extended KFs (EKFs)
to fuse and boost the pose estimate to 100 Hz. The first EKF
combines the 20-Hz pose estimate and the 10-Hz SLAM pose
correction. The information from the pose estimator and
SLAM is not independent, but the major role of this EKF is to
provide smooth compensation for possible delay in the SLAM
estimate introduced by large-scale loop closure and map corrections. We additionally employ a delayed measurement
update scheme to match the timing of the different sources of
information driving the pose estimate.
The second EKF combines the 20-Hz pose estimate from
the first EKF and the 100-Hz IMU data to provide 100-Hz
pose and linear velocity estimates in the world frame. The
final estimation output has an average delay of 0.01 s and
feeds directly into the feedback control loop of the robot for
position and velocity control.
Environment Representation
Given the estimated global pose of the robot, we can transform
laser scans accordingly and produce a dense 3-D environment
representation for planning and obstacle avoidance. However,
the amount of onboard memory is limited in mobile processors like ours. As we are primarily interested in indoor environments, the majority of obstacles take the form of vertical
walls. Therefore, we use a modified multivolume occupancy
grid map similar to [19] to create a compact occupied space
representation by merging contiguous occupied cells into
common vertical regions. For general indoor environments,
the resulting map typically has a memory cost on the same
order as a two-dimensional (2-D) occupancy grid map.
Planning and Control
Since the focus of this article is primarily the mapping and
localization required for autonomous navigation, we note here
only a general overview of our approach. Given the current
pose estimate of the robot and a map of the environment, we
employ incremental sampling-based planning (RRT*) [20],
where the robot is treated as a point-model kinematic system
with orientation (about z W ). We employ proportional-derivative feedback control laws to transform these kinematic inputs
to appropriate dynamic control inputs [21]. Tuning of control
gains is accomplished via optimal control methods (LQR)
based on a linearized system model.
Extensions
In the previous section, we described the system design and
discussed algorithm selection based on performance requirements and processor/sensor constraints. We now detail extensions to these algorithms that permit real-time operation
across multiple floors. Many of these extensions are based on
approximate solutions that are necessary to enable real-time
implementation. Additionally, we discuss the online estimation and compensation of aerodynamic effects due to wind or
propeller backwash in small corridors or near walls.

Environment Assumptions
As the domain of interest is indoor environments and periphery, we assume 2.5-dimensional (2.5-D) environment models
formed by collections of vertical walls and horizontal ground
planes, all assumed to be piecewise constant. Let 6x s, y s, z s@T
be the laser scan endpoints in the body frame. We can project
the laser scans to a horizontal plane by
6x g , y g , z g@T = R i R z 6x s, y s, z s@T.

We eliminate the scans that hit the floor or ceiling, which are
generally not useful for scan matching, by comparing z g with
the redirected laser scans pointing upward and downward.
Although this approach largely simplifies the challenges
of full 3-D scan matching
using only 2-D laser range
finders, the 2.5-D enviMAVs offer a unique
ronment assumption is
violated in cluttered and
capability for mapping and
outdoor environments
with natural structure. In
exploration in complex
our experiments, we see
that partial structure in
environments.
the environment satisfies
this assumption, and the
overall performance is acceptable.
Altitude Measurement
We measure the variation in altitude with scans pointing to the
floor and use this value to approximate the variance used in
the measurement update of the KF. If the variation is too high,
generally due to uneven surfaces or during a floor level change,
we discard the current laser measurement and defer the measurement update until a stable measurement is obtained. An
additional mirror deflects scans vertically upward toward the
ceiling. These measurements provide additional robustness
should there be no stable downward measurements available.
When no upward or downward laser measurements are available, we use an onboard pressure sensor for short-term measurement updates and force the robot to lower its height.
Corrections of accumulated errors caused by frequent floor
transitions are resolved through loop closure.
Incremental, Approximate Loop Closure
Loop closure for large loops is typically computationally
expensive with significant memory requirements. Although
several methods exist for reducing the computational burden
[18], [22], we found that these methods are too demanding
for a 1.6-GHz processor. With the understanding that we are
more concerned with global consistency than absolute accuracy, we propose a mild extension that results in an incremental but approximate approach.
As shown in Figure 3, the general idea of this approach is
as follows: A robot travels from P1 to P1 . Loop closure is
detected between P1 and P5 ; we contract this loop to P1:5 . We
DECEMBER 2013

IEEE ROBOTICS & AUTOMATION MAGAZINE

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2013