1
Path Following using Visual Odometry for a Mars Rover
in High-Slip Environments
1
Daniel M. Helmick, Yang Cheng,
Daniel S. Clouse, and Larry H. Matthies
Jet Propulsion Laboratory
4800 Oak Grove Drive
Pasadena, CA 91109
818-354-3226
firstname.lastname@jpl.nasa.gov
Stergios I. Roumeliotis
University of Minnesota
Dept. of Computer Science and Engineering4-192 EE/CS,
200 Union Street SE
Minneapolis, MN 55455
612-626-7507
stergios@cs.umn.edu
1
0-7803-8155-6/04/$17.00© 2004 IEEE
Abstract— An architecture for autonomous operation of
Mars rovers in high slip environments has been designed,
implemented, and tested. This architecture is composed of
several key technologies that enable the rover to accurately
follow a designated path, compensate for slippage, and reach
intended goals independent of the terrain over which it is
traversing (within the mechanical constraints of the mobility
system). These technologies include: visual odometry, full
vehicle kinematics, a Kalman filter, and a slip
compensation/path follower. Visual odometry tracks
distinctive scene features in stereo imagery to estimate rover
motion between successively acquired stereo image pairs
using a maximum likelihood motion estimation algorithm.
The full vehicle kinematics for a rocker-bogie suspension
system estimates motion, with a no-slip assumption, by
measuring wheel rates, and rocker, bogie, and steering
angles. The Kalman filter merges data from an Inertial
Measurement Unit (IMU) and visual odometry. This
merged estimate is then compared to the kinematic estimate
to determine (taking into account estimate uncertainties) if
and how much slippage has occurred. If no statistically
significant slippage has occurred then the kinematic estimate
is used to complement the Kalman filter estimate. If
slippage has occurred then a slip vector is calculated by
differencing the current Kalman filter estimate from the
kinematic estimate. This slip vector is then used, in
conjunction with the inverse kinematics, to determine the
necessary wheel velocities and steering angles to
compensate for slip and follow the desired path.
T
ABLE OF CONTENTS
..............................................................................
1. I
NTRODUCTION.............................................. 1
2. V
ISUAL ODOMETRY ALGORITHMS............... 2
3. K
INEMATIC ALGORITHMS ............................ 4
4. K
ALMAN FILTER ........................................... 7
5. S
LIP COMPENSATION/PATH FOLLOWING .. 11
6. R
ESULTS....................................................... 12
7. C
ONCLUSIONS.............................................. 16
R
EFERENCES.................................................... 16
1. INTRODUCTION
There is a very strong scientific rationale for Mars rover
exploration on slopes, to access channels, layered terrain,
and putative shorelines and fluid seeps, in pursuit of
evidence for fossil or extant life and in order to understand
the geologic and climatic history of the planet. Precision
landing capabilities anticipated for 2009 and beyond will
bring such terrain within practical reach of rover missions
Figure 1: Rocky 8 on a Sandy Slope
2
for the first time. MOLA results [1] show Valles Marineris
slopes over 600 meter baselines of typically less than 5
degrees on lower canyon walls to 28 degrees on upper walls,
while slopes elsewhere on Mars are within this upper bound.
MOLA measurements of inner slopes of craters with seeps is
typically from 15 to over 21 degrees for baselines of 330
meters, though typical fresh-looking Mars craters have
slopes 2 to 2.5 times lower [2]. In principle, such slopes are
within the mobility envelope of rovers. However, such
slopes are likely to have abundant loose material, which
could cause significant wheel slippage and sinkage in
addition to the usual obstacles posed by rocks and tip-over
hazards. Relatively little research has been done on rover
navigation on slopes or in the presence of significant wheel
slippage or sinkage, particularly for transverse slippage
when traveling across a slope. Therefore, there is little rover
navigation experience or autonomous navigation capability
for what is likely to be important terrain for future rover
missions.
This paper describes the design, implementation, and
experimental results of an integrated system to enable
navigation of Mars rovers in high slip environments. This
system enables the rover to accurately follow a designated
path, compensate for slippage, and reach intended goals
independent of the terrain over which it is traversing (within
the mechanical constraints of the mobility system). The
architecture is comprised of several key components that
were developed and refined for this task and are described in
detail below. These components include: visual odometry,
full vehicle kinematics, a Kalman filter, and a slip
compensation/path following algorithm. A high-level
functional block diagram of the system can be seen in Figure
2. Visual odometry is an algorithm that uses stereo imagery
to estimate rover motion independent of terrain properties.
The full vehicle kinematics uses position sensor inputs from
the joints and wheels of the rocker-bogie mobility system
(see Figure 4) to estimate rover motion. The Kalman filter
merges estimates from visual odometry and the onboard
IMU to estimate rover motion at very high rates. Both the
IMU estimate and the visual odometry estimate are
independent of the vehicle’s interaction with its
environment. The motion estimate from the Kalman filter is
then compared with the motion estimate from the vehicle
kinematics, which is highly dependent upon the vehicle’s
interaction with its environment. This comparison allows
for the statistical analysis of the difference between the two
estimates. Accounting for estimate uncertainties, if the
vehicle kinematic motion estimate can contribute to the
Kalman filter motion estimate, then no statistically
significant slippage has occurred. If, however, slippage has
occurred, then the kinematic estimate and the Kalman filter
estimate are differenced, resulting in a rover ‘slip vector.’
This slip vector is then used in combination with a path
following algorithm to calculate rover velocity commands
that follow a path while compensating for slip.
The individual components of the system as well as that of a
simplified integrated system has been tested onboard a
rover. Two independent tests were performed using Rocky
8 (see Figure 4), a Mars rover research platform. In the first
test, visual odometry was tested onboard the rover in the
JPL Mars Yard over two 25m traverses. Results from this
visual odometry test are very encouraging. Under normal
conditions, wheel odometry accuracy is not better than 10%
of distance traveled and, in higher slip environments, it can
be significantly worse. Results from our tests show that we
can achieve greater than 2.5% accuracy, regardless of the
mechanical soil characteristics. The second test was a field
test that used the slip compensation architecture described
above, minus the Kalman filter. This test involved traverses
of over 50 meter on sandy slopes.
Results from the Mars Yard test and the field test are
presented in Section 6. Testing of the fully integrated
system, including the Kalman filter and the continuous slip
compensation, is planned for the near future.
2. VISUAL ODOMETRY ALGORITHMS
Mobile robot long distance navigation on a distant planetary
body requires an accurate method for position estimation in
an unknown or poorly known environment. Techniques for
position estimation by stereo sequences have been shown to
be a very reliable and accurate method. Visual odometry, or
ego-motion estimation, was originally developed by
Matthies [3]. Following this work, some minor variations
and modifications were suggested for improving its
robustness and accuracy [4][5]. However, the key idea of
this method remains the same: to determine the change in
Visual Odometry
IMU
Kalman
Filter
Mahalanobis
Comparator
Inverse
Kinematics
Forward
Kinematics
Slip Compensation/
Path Following
Controller
Mahalanobis
Comparator
Desired
Path
=
p
z
y
x
x
kin
φ
[]
T
KF
rpzyxx
φ
=
=
y
x
x
path
[]
T
des
yxx
φ
=
Ψ
6...1
6...1
θ
=
r
p
z
y
x
x
IMU
φ
=
r
p
z
y
x
x
VO
φ
[]
T
6121
6121
qq
=
θθρρβ
ΨΨρρβ
…
…
stereo pair
=
p
z
y
x
x
slip
φ
Figure 2: Path Following/Slip Compensation Architecture
3
position and attitude for two or more pairs of stereo images
by using maximum likelihood estimation. The basic steps of
this method are described below.
Feature Selection
Features that can be easily matched between a stereo pair
and tracked between image steps are selected. The Forstner
interest operator [6] is applied to the left image of the first
stereo pair. The pixels with lower interest values are better
features. In order to ensure that features are evenly
distributed across the image scene, a minimum distance
between any two features is enforced. In order to reduce the
volume of data that needs to be sorted, the image scene is
divided into grids, with a grid size significantly smaller than
the minimum distance between features. Only the pixel with
the lowest interest value in each grid is selected as a
potential feature. Then, all potential features are sorted in
descending order. The top N pixels meeting the minimum
distance constraint are selected as features.
Feature Gap Analysis and Covariance Computation
The 3D positions of the selected features are determined by
stereo matching. Under perfect conditions, the rays of the
same feature from the left and right images should intersect
in space. However, due to image noise and matching error,
they do not always intersect. The gap (the shortest distance
between the two rays) indicates the goodness of the stereo
matching. Features with large gaps are eliminated from
further processing.
Additionally, the error model is a function of the gap. This
effect is incorporated in the covariance matrix computation.
Assuming the stereo cameras are located at C
1
(X
1
, Y
1
, Z
1
)
and C
2
(X
2
, Y
2
, Z
2
) (see Figure 3), r
1
and r
2
are two unit rays
from the same feature in both images. Because of noise, r
1
and r
2
do not always intersect in space. The stereo point is
taken to be the midway between the closest points of the two
rays.
Assuming the closest points between the two rays are P
1
and
P
2
, thus, we have
2222
1111
mrCP
mrCP
+=
+=
(1)
where m
1
and m
2
are the length of P
1
C
1
and P
2
C
2
.
Therefore, we have
0)()(
0)()(
2112212212
1112212112
=⋅−+−=−
=⋅−+−=−
rmrmrCCrPP
rmrmrCCrPP
(2)
Then we have
21212
2
21
2121
1
)(
)(1
))((
Brmrrm
rr
rrrBBr
m −⋅=
⋅−
⋅⋅−
=
(3)
0.2/)(
21
PPP += (4)
where
12
CCB −= and m
1
and m
2
are functions of feature
locations on both images. Taking the partial derivatives
results in
22
21
22
2
2
'
2
'
1
'
1
]G1[
)]FG][(G)rB(rB[2
]G1[
]G1][F)rB(G)rB(rB[
m
−
⋅⋅⋅−⋅
+
−
−⋅⋅−⋅⋅−⋅
=
(5)
'
21
'
1
'
2
rBmFmGm ⋅−⋅+⋅= (6)
0.2/)(
'
222
'
2
'
111
'
1
'
mrmrmrmrP +++= (7)
where
)rrrr(F
'
212
'
1
⋅+⋅= and )rr(G
21
⋅= .
The covariance of P is
t
r
l
p
PP '
0
0
'
Σ
Σ
=Σ
(8)
where P’ is the Jacobian matrix, or the matrix of first partial
derivatives of P with respect to C
1
and C
2
.
C
1
C
2
m
1
m
2
p
1
p
2
r
1
r
2
Figure 3: Feature Gap
4
Feature Tracking
After the rover moves some distance, a second pair of stereo
images is acquired. The features selected from the previous
image are then projected into the second pair using the
knowledge of the approximated motion provided by the
onboard wheel odometry. Then a correlation-based search
and tracking based on an affine template precisely determine
these features’ 2D positions in the second image pair. The
affine template tracking aims to remove the tracking error
caused by large roll and scale change between images. In
this case, the relationship between two images within the
template is expressed as an affine transform
feydxycbyaxx ++=++=
112112
(9)
Where [a, b, c, d, e, f] are the unknown coefficients of the
affine transform that can be determined by an iterative
method by minimizing a merit function [7]
min)]y,x(I)y,x(I[M
2
222111
=−=
∑
(10)
where I
j
(x, y) specifies the pixel value at position (x, y) in
image j. Stereo matching is then performed on these tracked
features on the second pair to determine their new 3D
positions. If the initial motion guess is accurate, the
difference between the two estimated 3D positions should be
within the error ellipse. However, when the initial motion
guess is off, the difference between the two estimated
positions reflects the error of the initial motion guess and
can be used to determine the change of rover position.
Motion Estimation
Motion estimation is done in two steps. Coarse motion is
first estimated with Schonemann motion estimation, and
then a more accurate motion -estimate is determined by
maximum likelihood motion estimation.
Schonemann motion estimation [8] uses singular value
decomposition (SVD) with an orthogonal constraint to
estimate a rotation matrix and a translation that transforms
the feature positions in I
1
to those found in I
2
. The
Schonemann method is simple and fast, however, it is highly
unstable when large errors are involved. In order to
overcome this problem, the least-median-of-squares method
[9] is applied. In this method, a subset of features is
randomly selected. Then each feature from the previous
frame is projected to the current frame, and the distance
error between that projection and the position of the
corresponding feature in I
2
is calculated. The total count of
features under a given error tolerance is calculated. This
procedure is repeated multiple times. The motion with the
largest number of agreeable features is chosen as the best
motion.
The best motion estimation found using the above procedure
is refined using maximum likelihood motion estimation.
Maximum likelihood motion estimation takes account of the
3D position differences and associated error models in order
to estimate motion. Let Q
pj
and Q
cj
be the observed feature
positions before and after a robot motion. Then we have
jpjcj
eTRQQ ++= (11)
where R and T are the rotation and translation of the robot
and e
i
is the combined errors in the observed positions of the
j
th
features. In this estimation, the 3 axis rotations (Θ) and
translation (T) are directly determined by minimizing the
summation in the exponents
∑
= min
jj
T
j
rWr , where
TRQQr
pjcjj
−−= and W
j
is the inverse covariance
matrix of e
j
. The minimization of the nonlinear problem is
done by linearization and iterations [3]. Two nice properties
of maximum-likelihood estimation make the algorithm
powerful. First, it estimates the 3 axis rotations (Θ) directly
so that it eliminates the error caused by rotation matrix
estimation (which occurs with least-squares estimation).
Secondly, it incorporates error models in the estimation,
which greatly improves the accuracy.
3. KINEMATIC ALGORITHMS
Full rover kinematic algorithms were developed to fill two
roles in the architecture shown in Figure 2. The first role is
the forward kinematics of the vehicle, which estimates rover
motion given the wheel rates, and rocker, bogie, and steering
angles. The second role is the inverse kinematics of the
vehicle, which calculates the necessary wheel velocities to
create the desired rover motion.
Figure 4: Rocky 8
5
These algorithms are specific to the rocker-bogie
configuration with six steerable wheels (see Figure 4), but
the techniques used to derive the algorithms could be used
for any vehicle configuration (although there may be a fewer
number of observable DOFs for different configurations).
Additionally, the forward kinematic algorithms could be
used directly for rovers with a subset of functionality (e.g. a
rocker-bogie rover with only 4 steerable wheels, such as
MER) simply by making the relevant parameters constant.
The motivation for developing the full kinematics of this
class of vehicles (rather than making the more common
planar assumption) is twofold. First, it allows for the
observation of 5 DOFs, whereas the planar assumption
limits this to 3 DOFs. Second, as terrain becomes rougher,
the errors due to the planar assumption grow. As shown in
Section 6, these errors can grow to be significantly large and
affect the slip calculations and, consequently, the slip
compensation controller. Secondarily, no formulation of the
inverse kinematics previously existed that would take
advantage of the holonomic nature of the rover (see Inverse
Kinematics below).
The formulation of the forward and inverse kinematics
closely follows that of [10,11], with significant extensions
being made for 6 wheel steering. Greater details of the
kinematic derivations can be found here [10,11,12].
Also note that the implementation of these algorithms has
been parameterized so that changes in the kinematics of the
rover or application of this algorithm to a different rocker-
bogie rover does not require a re-derivation of the
algorithms and can be made with only parameter changes in
code.
Rocker-Bogie Configuration
The rocker-bogie configuration is a suspension system that
is commonly used for planetary rovers and their prototypes.
The configuration analyzed here consists of 15 DOFs: 6
steerable/drivable wheels (12 DOFs), a rocker, and two
bogies. It is beyond the scope of this paper to discuss the
benefits of such a mobility system. The interested reader is
referred to [13] for more details. What is relevant here is
that with a few assumptions, the rocker-bogie system allows
for the observation of 5 of the 6 DOFs of the rover. These
assumptions are: 1) the wheel/terrain contact point is in a
constant location relative to the wheel axle, and 2) slip
between the wheel and the terrain only occurs about the
steering axis (e.g. no side or rolling slip).
The first assumption increases the modeling error in rough
terrain; however, this error still remains smaller than the
error created by the planar assumption. The second
assumption is necessary for a steerable wheel to exist.
Frame Definitions
Denavit-Hartenburg conventions were used to define the
frames of each of the 15 DOFs (see Figure 6) [14]. Two
additional coordinate frames were added for each wheel: the
contact frame, C
i
and the motion frame, M
i
(for i=1,2,…,6).
C
i
defines the wheel/terrain contact point, and M
i
defines
the steering slip and the wheel roll (see Figure 5). Table 1
describes each frame.
Table 1: Frame Descriptions
Frame Identification
Frame Description
R rover frame
D rocker (differential) frame
ρ
1
right bogie frame
ρ
2
left bogie frame
S
1
,…,S
6
steering frames for each wheel
A
1
,…,A
6
axel frames for each wheel
C
1
,…,C
6
contact frames for each wheel
M
1
,…,M
6
motion frames for each wheel
x
Mi
z
Mi
z
Ci
x
Ci
Figure 5: Contact Frame and Motion Frame Definitions
Figure 6: Coordinate Frame Definition For Right Side of
Rover (all dimensions in cm)
