Slip-Compensated Path Following for Planetary Exploration

Rovers

Daniel M. Helmick, Stergios I. Roumeliotis† , Yang Cheng, Daniel S. Clouse,

Max Bajracharya, and Larry H. Matthies
Jet Propulsion Laboratory, 4800 Oak Grove Drive, Pasadena, CA 91109,
Email: firstname.lastname@jpl.nasa.gov
†
Department of Computer Science and Engineering, University of Minnesota,
200 Union St. SE, Minneapolis, MN 55455, Email: stergios@cs.umn.edu

Abstract

A system that enables continuous slip compensation for a Mars rover has been designed, imple-
mented, and field-tested. This system is composed of several components that allow the rover to
accurately and continuously follow a designated path, compensate for slippage, and reach intended
goals in high-slip environments. These components include: visual odometry, vehicle kinematics, a
Kalman filter pose estimator, and a slip-compensated path follower. Visual odometry tracks dis-
tinctive scene features in stereo imagery to estimate rover motion between successively acquired
stereo image pairs. The kinematics for a rocker-bogie suspension system estimates vehicle motion
by measuring wheel rates, and rocker, bogie, and steering angles. The Kalman filter processes mea-
surements from an Inertial Measurement Unit (IMU) and visual odometry. The filter estimate is
then compared to the kinematic estimate to determine whether slippage has occurred, taking into
account estimate uncertainties. If slippage is detected, the slip vector is calculated by differencing
the current Kalman filter estimate from the kinematic estimate. This slip vector is then used to
determine the necessary wheel velocities and steering angles to compensate for slip and follow the
desired path.

keywords: rover navigation, visual odometry, slip compensation, Kalman filter, rover kinematics.

1 INTRODUCTION
This paper describes the design, implementation, and experimental results of an integrated system
for Mars rover navigation in high-slip environments (see Figure 1). The presented algorithms enable
the rover to accurately follow a designated path, compensate for slippage, and reach intended goals
independent of the terrain over which it traverses (within the mechanical constraints of the mobility
system). The proposed system is comprised of several key components that were developed and refined

1
 Figure 1: Rocky8 on a Sandy Slope.

for this task and are described in detail below. These components include: visual odometry, full vehicle
kinematics, a Kalman filter pose estimator, and a slip-compensated path following algorithm. Figure 2
provides a high-level functional block diagram of the system. Visual odometry is an algorithm that
relies on stereo imagery to estimate rover motion independent of mechanical terrain properties and is
described in Section 2. The full vehicle kinematics, described in Section 3, 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 combines inertial measurements from the onboard IMU with motion estimates from
the visual odometry to estimate rover motion at high sampling rates, and is detailed in Section 4.
Since both the IMU measurements and the visual odometry estimates are independent of the vehicle’s
interaction with the ground, the motion estimates from the Kalman filter can be compared to those
computed based on the vehicle kinematics (which are highly dependent upon the vehicle’s interaction
with its environment) to determine if any statistically significant slippage has occurred. If there is no
slippage, the vehicle kinematic motion estimates can contribute to the Kalman filter pose estimates. If,
however, slippage is detected, then the kinematic estimate and the Kalman filter estimate are differenced,
resulting in a rover slip vector. This slip vector is then used by the path following algorithm to calculate
the rover velocity commands required for following a path while compensating for slip. This algorithm
is described in greater detail in Section 6.
The individual components of the system as well as a simplified integrated system have been tested

2
 T
β ρ1 ρ 2 Ψ1 Ψ6
‚W%

[q q] =
†[)

‡\*

†[) †[) †[)

ƒ„XYZ&'(

†[) †[)

stereo pair β ρ1 ρ2 θ1 θ6
€U#

V$

‡\*

x
wL tI

x
xM xM

~S! {P

Forward
T"

uJ rG

IMU y
|Q yN

Visual Odometry x
xM xM

Kinematics
b7 _4

y
uJ rG

c8

T"

|Q yN

z
`5 ]2

uJ rG

y
xM xM

z x IMU =
c8

|Q yN

uJ rG

`5 ]2

xM

T"

xVO = φ
xM

|Q yN

xkin
uJ rG

= z
T"

`5 ]2

c8

φ
c8

T"

uJ

p
rG

|Q yN

`5 ]2

xM

φ
c8

uJ rG

|Q yN

p
`5 ]2

r
T"

uJ rG

vK sH

xM

|Q yN

p
`5 ]2

a6 ^3

c8

r
|Q yN

}R zO

T"

Kalman Mahalanobis
Filter Comparator
x
0 Š-

Ž1

xKF = [x p r]
T
y z φ
‹. ˆ+

y
Ž1

‹. ˆ+

xslip = z
‹. ˆ+

Slip-Compensated
Ž1

Ž1

Desired
‹. ˆ+

Path Following φ
Ž1

‹. ˆ+

Path Controller p
‹. ˆ+

Œ/ ‰,

Ž1

x
[ ]
f;

T
x path = xdes = x y φ
ghi<=> d9

y
j?
e:

j? j? j?

Inverse
Kinematics
Ψ1...6
mB

nopCDE k@

θ1...6
lA

qF

Figure 2: Block Diagram of the Slip-Compensated Path Following System.

onboard a rover. Several independent tests were performed using Rocky 8 (see Figure 1), a Mars rover
research platform. In the first test, visual odometry was tested onboard the rover in the JPL Mars Yard
over two 25 meter traverses. Under nominal conditions, wheel odometry accuracy is not better than
10% of the distance traveled and, in higher slip environments, it can be significantly worse. Results from
our first tests showed that at least 2.5% accuracy can be achieved by using visual odometry, regardless
of the mechanical soil characteristics. The second test was a field test that used the slip compensation
system described above, minus the Kalman filter. This test was a traverse of over 50 meters on sandy
slopes. The third set of tests was performed on a tiltable platform measuring 5×5 meters. In this last
set of tests, the continuous rover-slip compensation algorithm (see Section 6) was employed instead of
stopping the rover to estimate slippage, as was the case in previous tests. Results from these three sets
of experiments are provided in Section 7. This paper extends the work described in [1, 2]. Related
work includes rover trajectory generation [3], rover navigation [4, 5, 6], and path following [7]. To the
best of our knowledge this is the first time that an integrated robotic system for slip detection and
compensation is being presented and tested in realistic conditions.

3
 Figure 3: Feature Gap

2 VISUAL ODOMETRY ALGORITHM

Long distance mobile robot navigation on a remote planetary body requires an accurate method for
position estimation in an unknown or poorly known environment. Visual odometry, or image-based
ego-motion estimation, was originally developed by Matthies [8]. Following this work, a number of
variations and modifications have been suggested for improving its robustness and accuracy [9, 10]. The
key idea of this method is to determine the change in position and attitude by solving for the maximum
likelihood estimate of the transformation between a selection of 3D features extracted from consecutive
stereo images. The basic steps of this algorithm are described below. A more detailed description of
the ensuing derivations can be found in [2].

2.1 Feature Selection

The first step in the visual odometry algorithm is to select features that can be easily matched between
stereo pairs and tracked across a single image step. To achieve this, the Forstner interest operator [12]
is applied to the left image of the first stereo pair. The pixels with higher interest values are better
features. In order to ensure that the detected visual points are evenly distributed across the image scene,
a minimum distance between any two features is enforced. Furthermore, the image scene is divided into
grids, with the grid size significantly larger than the minimum distance between features, so as to reduce
the volume of data that needs to be sorted. The one feature with the highest interest value in each grid
is selected as a viable candidate. Finally, a fixed number of these candidates with the highest interest
values which also meet the minimum distance constraint is selected.

2.2 Feature Gap Analysis and Covariance Computation

The 3D positions of the selected features are determined by stereo matching. A template around each
feature in the left image is correlated to a location in the right image. Knowing the location of the
features in the left and right images, a ray corresponding to the feature can be projected out of each
camera. 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

4
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 described below.
Assuming the stereo cameras are located at C1 (X1 , Y1 , Z1 ) and C2 (X2 , Y2 , Z2 ) (see Figure 3), r1 and
r2 are two unit rays from the same feature in both images. As mentioned before, due to noise, r 1 and
r2 do not always intersect in space. The stereo point is selected to be the midway between the closest
points of the two rays.
Assuming the closest points between the two rays are P1 and P2 we have:

P 1 = C 1 + r 1 m1

P 2 = C 2 + r 1 m2 (1)

where m1 and m2 are the lengths of P1 C1 and P2 C2 , respectively. Since P1 and P2 represent the same
point in space, we have

0 = (P2 − P1 ) · r1 = (C2 − C1 + r2 m2 − r1 m1 ) · r1

0 = (P2 − P1 ) · r2 = (C2 − C1 + r2 m2 − r1 m1 ) · r2 (2)

Solving for m1 and m2 , yields

1
m1 = ((r1 · r2 )r2 − r1 ) · (C2 − C1 )
δ
1
m2 = (r2 − (r1 · r2 )r1 ) · (C2 − C1 ) (3)
δ

where δ = (r1 · r2 )2 − 1. The coordinates of point P are given by:

P = (P1 + P2 )/2 (4)

where P1 and P2 are computed by substituting Equations (3) into (1). The covariance for the position
of point P is:
 
Σl 0
ΣP = J   JT (5)
0 Σr

where J is the Jacobian matrix, i.e., the matrix of partial derivatives of P with respect to the 2D
feature locations in the left and right images, and Σl and Σr are 2×2 matrices whose elements are the
curvatures of the biquadratic polynomial along the vertical, horizontal, and diagonal directions, which
can be obtained directly from subpixel interpolation. The quality of a 3D feature is a function of its
relative location, the gap between the two stereo rays and the sharpness of the correlation peak. This
covariance computation fully reflects these three factors. For a more detailed derivation see [2].

2.3 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

5
motion provided by the onboard wheel odometry (forward kinematics). The features are first matched
in the new left image by searching an area around the projected feature locations. Stereo matching
is then performed on these tracked features on the second pair to determine their new 3D positions.
Because the 3D positions of those tracked features are already known from the previous step, the stereo
matching search range can be greatly reduced. Features whose initial and final 3D positions differ by
too large an amount are filtered out.

2.4 Motion Estimation

Given two sets of corresponding 3D features, the transformation between them is determined using a
motion estimation algorithm that takes into account the matching covariance of each feature. This
algorithm is decomposed into two sequential steps. Coarse motion is first computed with Schonemann
motion estimation, and then a more accurate motion estimate is determined by maximum likelihood
motion estimation.
Schonemann motion estimation [13] uses singular value decomposition (SVD) with an orthogonal
constraint to estimate the rotation matrix, R, and translation vector, T , that transforms the feature
positions in I1 to those found in I2 . The Schonemann method is simple and fast, however, it is highly
unstable when large errors are involved. In order to overcome this problem, a least-median-of-squares
approach [14] is adopted. 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 I2 is computed. 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 estimate determined by employing the above procedure is refined using Maximum
Likelihood (ML) motion estimation. The ML estimation algorithm takes into account the 3D feature
(1) (2)
position differences and the associated error models in order to estimate motion. Let P j and Pj be
the observed feature positions before and after a robot motion. Then we have

(2) (1)
Pj = RPj + T + ej (6)

where R = R(Θ) and T are the rotation and translation of the robot and ej is the combined errors
in the observed positions of the j th features. In this estimation process, the 3-axis rotations (Θ) and
translation (T ) are directly determined by minimizing the summation in the exponents Σj eTj Wj ej where
(2) (1)
ej = P j − RPj − T and Wj is the inverse covariance matrix of ej . The minimization of this nonlinear
problem is implemented by iterative linearization [8]. Two nice properties of ML estimation make this
algorithm significantly more accurate compared to the Schonemann method. 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). Second, it incorporates error models for the feature positions in
the estimation process (see Equation (5)), which greatly improves the achieved accuracy.

6
 Figure 4: Rocky 8.

3 KINEMATIC ALGORITHMS
The forward and inverse kinematics were determined for the rover (Rocky 8) shown in Figure 2. The
forward kinematics of the vehicle are used for estimating rover motion given the wheel rates and rocker,
bogie, and steering angles. The inverse kinematics of the vehicle are employed for calculating the
necessary wheel velocities and steering angles to create a desired rover motion.
These algorithms are specific to the rocker-bogie configuration with six steerable wheels (see Fig-
ure 4). However, the techniques used to derive these algorithms could be applied to any vehicle con-
figuration, though there may be a fewer number of observable degrees of freedom (DOFs) for different
configurations. Additionally, these forward kinematic algorithms can be used directly for rovers with a
subset of functionality (e.g., a rocker-bogie rover with only four of six wheels capable of steering, such
as the Mars Exploration Rovers; or a rover with only four wheels with or without a rocker, etc.) simply
by restricting the relevant parameters to be constant.
The motivation for developing the full kinematics of this class of vehicles, rather than adopting the
more common planar simplification, is twofold. First, it allows for the observation of 5 DOFs, whereas
the planar approximation limits this to 3 DOFs. Second, as terrain becomes rougher, the errors due
to the planar assumption grow. These errors can become significantly large (up to 30% of the distance
traveled) and affect the slip calculations and, consequently, the slip compensation controller.
The formulation of the forward and inverse kinematics closely follows that of [15, 16], with significant
extensions being made for 6-wheel steering. A detailed description of the kinematic derivations can be
found in [2, 15, 16, 17].

7
 Figure 5: Coordinate Frame Definition for the Right Side of the Rover (all dimensions in cm).

3.1 Rocker-Bogie Configuration

The rocker-bogie configuration is a suspension system that is commonly used for planetary rovers and
their prototypes. The configuration analyzed in this work consists of 15 DOFs: 6 steerable/drivable
wheels (12 DOFs), a rocker, and two bogies (all of these DOFs are sensed using either encoders or
potentiometers).1 In what follows, we show that under two 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). These slip assumptions, however, are
only made for the kinematics algorithm and not for the slip compensation system as a whole. The
second assumption is what makes the difference between the Kalman filter motion estimate and the
kinematic motion estimate a measurement of vehicle slippage.

3.2 D-H Table Formulation

The Denavit-Hartenberg convention was adopted for defining the frames of each of the 15 DOFs [19].
From the frame definitions (see Figure 5), a unique set of D-H parameters can be derived that completely
describes the kinematics of the rover. From these parameters, wheel Jacobians are computed as described
in [2].
1 It is beyond the scope of this paper to describe the benefits of such a mobility system; the interested reader is referred
to [18] for a discussion on this topic.

8
3.3 Forward Kinematics

Once the wheel Jacobians are known, rover motion estimation can be performed using the least-squares
formulation
 
v
  = (AT A)−1 AT · Jcomp q̇comp (7)
η̇
h i
T
where v = ẋ ẏ ż φ̇ ṗ ṙ is the vector of rover linear and rotational velocities, η̇ is a vector
of unobservable wheel turning rates (the sum of the rotational slip rate and the steering rate), A is the
matrix of the unsensed elements of the Jacobians, Jcomp is the block-diagonal composite matrix of the
wheel Jacobians, and q̇comp is the composite vector of the measured joint kinematic rates. For details
on the derivation and simplified solution to this least-squares problem see [15, 17]. Note that it is not
necessary to actually perform the inversion of AT A. The matrix equations can be greatly simplified
algebraically to make these computations significantly more efficient.

3.4 Inverse Kinematics

As depicted in Figure 2, the inverse kinematics algorithm receives as input the commanded rover motion,
and the current kinematic angles and angle-rates, and produces six steering angles and six wheel rates.
An interesting feature of the six steerable wheels is the fact that this configuration creates a holonomic
rover, under the assumption of instantaneous steering. Consequently all three controllable DOFs of
the rover, [ ẋ, ẏ, φ̇ ], are independent, which enables the isolation of several different control loops as
described in Section 6.2.
The first step of the inverse kinematics algorithm is to calculate an instantaneous center of rotation,
[ xO yO ], in the rover frame, by employing the following equations

xO = ẏcmd /φ̇cmd

yO = ẋcmd /φ̇cmd (8)

where ẋcmd , ẏcmd , and φ̇cmd are the commanded vehicle velocities from the slip-compensated path
follower (see Section 6). Because the z-axis of each wheel is the steerable axis, the instantaneous center
of rotation needs to be projected from the rover frame {R} to the x-y plane of the steering contact
coordinate frame {Mi }, i = 1 . . . 6, by employing the first two rows of the transformation matrix TRMi
between these frames:
 
xO
     
xO 1 0 0 0  yO
 
 · T Mi

  = R ·  (9)
yO 0 1 0 0  0
 

Mi  
1
R

The steering angle for each wheel is then calculated using

ψi = arctan(xO,Mi /yO,Mi ) , i = 1 . . . 6 (10)

9
and each wheel rate is determined as

θ˙i = [Jai
T T
∆(Jui )Jai ]−1 Jai ∆(Jui )E · vcmd , i = 1 . . . 6 (11)

where
T T
∆(Jui ) = Jui (Jui Jui )−1 Jui −I , (12)

 T
1 0 0 0 0
 
E = 0 1 0 0 0  , (13)
 
 
0 0 0 1 0
h iT
vcmd = ẋcmd ẏcmd φ̇cmd , (14)

and Jai and Jui are the actuated and un-actuated Jacobians defined in [17]. Note that Equation (11)
is the actuated inverse solution from Muir and Neumann [17]. As before, these matrix inversions can
be algebraically simplified so that each wheel-rate calculation is relatively simple and computationally
efficient.

4 KALMAN FILTER
In this section, we present our approach for estimating the position and orientation of the rover using
inertial measurements from the IMU and relative pose (position and orientation measurements) from
visual odometry and vehicle odometry (forward kinematics). Since our formulation is based on sensor
modeling, we use the indirect form of the Extended Kalman Filter (EKF) that estimates the errors in
the estimated states instead of the states themselves. The interested reader is referred to [21, 23, 24] for
a detailed description of the advantages of the indirect KF vs. the direct KF. Within this framework,
the IMU measurements are integrated in order to propagate the state estimate [20, 22], while the visual
odometry and vehicle kinematics (only when no slip has occurred), are employed for updating the state
estimate and providing periodic corrections. The equations of the EKF designed for this particular
nonlinear system are described in detail in [22].

4.1 System Propagation Model

The state vector of interest in this estimation problem is:

h i
xT = qT bTg vT bTa pT (15)

where q is the 4×1 quaternion of rotation that represents the attitude of the vehicle, v and p are the
3×1 vectors of linear velocity and position of the rover, and bg and ba are the 3×1 vectors of biases in
the gyroscope and accelerometer signals. The corresponding error state vector is:
h i
∆xT = δθT ∆bTg ∆v T ∆ba T ∆pT (16)

10
where ∆ζ = ζ − ζˆ is the algebraic difference (error) between the real value of a state ζ ∈ {b g , v, ba , p}
ˆ and δθ is the 3×1 error vector of the tilt angles determined based on the small angle
and its estimate ζ,
approximation:
h i
q = δq ⊗ q̂, δq T = (1/2)δθT 1 (17)

The continuous-time equation for the error-state propagation is

δ ẋ(t) = Fc (t)δx(t) + Gc (t)w(t) (18)

where w(t) is the white zero-mean continuous time noise process affecting the IMU signals,
   
−bω̂c −I3×3 03×3 03×3 03×3 −I 03×3 03×3 03×3
   3×3 
 03×3 03×3 03×3 03×3 03×3   03×3 I3×3 03×3 03×3 
   
   
Fc (t) = −C T (q̂)bâc 03×3 03×3 −C T (q̂) 03×3  , Gc (t) =  03×3 03×3 −C T (q̂) 03×3  ,
   
   
 03×3 03×3 03×3 03×3 03×3   03×3 03×3 03×3 I3×3 
   
   
03×3 03×3 I3×3 03×3 03×3 03×3 03×3 03×3 03×3

bω̂c (bâc) is the skew-symmetric matrix of the vector ω̂ = ωm − b̂g (â = am − b̂a ), ωm (am ) is the measured
rotational velocity (linear acceleration) vector, and C T (q̂) is the rotational matrix that projects vectors
from the sensor to the global coordinate frame. By discretizing Equation (18) we obtain:

δxk+1 = Fk (tk+1 , tk )δxk + wk (19)

R tk+1
where Fk (tk+1 , tk ) = e tk Fc (τ )dτ , and wk is the equivalent discrete-time white zero-mean noise process
Rt
with covariance Qk = tkk+1 Fk (tk+1 , τ )Gc (τ )Qc GTc (τ )FkT (tk+1 , τ )dτ , Qc is a block diagonal matrix with
matrix elements σi2 I3×3 , i = 1 . . . 4, and σi2 is the power of the gyroscope, gyroscope-bias, accelerometer,
and accelerometer-bias continuous-time noise components, respectively. The interested reader is referred
to [22] for the details of the derivation for Equations (18) and (19).

4.2 Relative Pose Measurement Model

The motion measurement provided by the visual odometry and/or the forward kinematics of the vehicle
corresponds to a relative pose measurement, i.e., the algebraic difference in position z p and attitude
quaternion zq . In order to process measurements that relate state estimates at different time steps (e.g.,
tk and tk+m ), the state vector needs to be augmented with a copy of the vehicle state estimate at t k .
This process enables the filter to explicitly account for the correlations of the vehicle state estimates
between the time instants that e.g., two sets of images were recorded [25]. In what follows, we assume
that at time tk the vehicle is at position G p(tk ) = p1 with quaternion attitude 1
G q(tk ) = q1 , and after m
G 2
steps it has moved to position p(tk+m ) = p2 with attitude G q(tk+m ) = q2 . Frames {G}, {1}, and {2}
are the inertial frames of reference attached to the vehicle at times t0 , tk , and tk+m respectively.

11
 The errors in the relative position and attitude (pose) measurements are given by:
   
∆zp ∆zp
∆z̃k+m =  =X 
∆z̃q ∆zq
   
h i ∆x1 ∆x1
= Γ Jx 1 Jx 2   + X nr = H   + ñr (20)
∆x2 ∆x2

where Γ is a 6×6 block diagonal matrix with both 3×3 matrix elements equal to the rotational matrix
1
G C(q), and Jx1 , Jx2 are the measurement Jacobians with respect to the state vector at the two time
instants tk and tk+m , correspondingly. In this last expression, X is the 6×7 block diagonal matrix
with matrix elements the 3×3 identity matrix and the 3×4 Jacobian matrix that projects the algebraic
quaternion difference to the tilt angles’ error. All these quantities are described in detail in [22]. Both
the original measurement noise nr and the projected ñr are assumed to be a zero-mean white noise
Gaussian processes with covariances:
 
Rq Rpq
Rr = E[nr nTr ] =   , R̃r = E[ñr ñTr ] = X Rr X T
Rpq Rq

As is evident from Equation (20), the relative pose measurement error is expressed in terms of the
current 4x2 = 4x(tk+m ) and the previous 4x1 = 4x(tk ) (error) state of the system. Note that tk
and tk+m are the time instants when, e.g., the two images (encoder readings) processed by the visual
(vehicle) odometry algorithm were recorded and thus the relative pose (motion estimate) measurement
provided by it corresponds to the time interval [ tk tk+m ].

4.3 Augmented-state propagation

If 4xk/k is the state estimate at time tk (when the first image or encoder measurement was recorded)
we augment the state vector with a second copy of this estimate:
h iT
4x̆ = 4xTk/k 4xTk/k

Since initially, at time tk , the two copies of the state estimate are identical, the covariance matrix for
the augmented system would be:  
Pkk Pkk
P̆k/k =  
Pkk Pkk
where Pkk is the covariance matrix for the (error) state of the vehicle at the time tk . In order to conserve
the estimate of the state at tk , necessary for evaluating the relative pose measurement error at tk+m ,
only the second copy of the state estimate is propagated during this interval, while the first remains
stationary. The propagation equation for the augmented system is:
      
∆xs I 0 ∆xs 0
  =   +   wk
∆x 0 Fk+1 ∆x I
k+1/k k/k

12
or
∆x̆k+1/k = F̆k+1 ∆x̆k/k + Ğk+1 wk

where ∆xs is the stationary copy of the error state of the vehicle and wk is the system noise due to
errors in the IMU measurements [22]. The covariance of the augmented system is propagated and after
m steps it is:  
Pkk Pkk F T
P̆k+m/k =   (21)
FPkk Pk+m/k
Qm
where F = i=1 Fk+1 and Pk+m/k is the propagated covariance of the evolving state at time tk+m .
At this point, we should note that by employing the error models for the propagation and update
(see Section 4.2), the state vector can be updated every time a relative pose measurement from the
visual odometry or the kinematics algorithm becomes available. The equations for the augmented-state
Kalman filter are described in detail in [22, 25].

5 MAHALANOBIS COMPARATOR (SLIP ESTIMATION)

In this section, we present our approach to rover slippage detection. Based on the kinematic equations of
the rover and assuming no wheel slippage, the wheel and rocker-bogie joint measurements are processed
to produce a relative position and orientation measurement over a certain time (sampling) interval.
Before updating the state estimate of the EKF, this kinematics-based displacement measurement needs
to be validated by comparing it to the equivalent one estimated by the EKF. If significant wheel slippage
has occurred, the residual (i.e., difference between odometry-based and EKF-based displacement) will
be significantly larger compared to the case where the rover moves on solid ground without any of the
wheels slipping. A statistical measure for assessing the validity of these measurements is the Mahalanobis
distance
T
md = rk,k+m S −1 rk,k+m (22)

where rk,k+m is the residual and S is the corresponding residual covariance matrix. In the case of this
vehicle’s odometry measurement, the Mahalanobis distance follows a chi-square distribution with five
degrees of freedom. A sufficient test for validating vehicle odometry measurements z k,k+m is to require
that these match the expected (estimated by the EKF) measurements ẑk,k+m of the same quantities
with a certain level of confidence. By requiring the fit between the expected and actual measurements
to be valid with probability, e.g., p=95%, odometric measurements are processed by the EKF only
when md ≤ t , with t = 11.07. If this inequality does not hold, these measurements are discarded and
wheel slippage is detected. In this case, the residual is provided to the slip compensation algorithm for
appropriately modifying the rover steering and driving commands.
The incentive to include the kinematics measurement in the Kalman filter estimator, when it passes
the Mahalanobis distance criterion (as shown in Figure 2), is mainly for off-nominal conditions, when
visual odometry does not perform at 2.5% accuracy (for example, under rare conditions, when it does

13
 (xcarrot , ycarrot )

err


xˆ

yˆ

circle
radius

Figure 6: Carrot Heading Calculation.

not converge to a solution). Under nominal conditions for visual odometry, and when the vehicle is
slipping, is when the vehicle kinematics are most useful since they allow for the estimation of slippage.

6 SLIP-COMPENSATED PATH FOLLOWER

At the center of Figure 2 is the slip-compensated path following controller. This is comprised of two
algorithms that are used in close conjunction to achieve the system’s end-goal of enabling the traversal
of a desired path through high-slip environments. At the highest level, the algorithms receive a 3×1 slip
h i h i
vector ẋ ẏ φ̇ , a 3×1 rover pose vector x y φ , and a 2×n desired path vector (where
slip h i pose
n is the number of waypoints) x y , and then outputs a 3×1 commanded rover velocity vector
h i path

ẋ ẏ φ̇ .
cmd

6.1 Carrot Heading Algorithm

The carrot heading algorithm calculates the desired heading, φcarrot , of the vehicle given the desired
path and the current rover pose. This algorithm was employed for its robustness to path error [26, 27].
The desired path consists of a set of linear segments between waypoints. The waypoints, however,
can be spaced at any distance apart, thus allowing for paths of arbitrary complexity. The algorithm
determines the desired heading by calculating the intersection of a circle centered on the rover frame
origin with the desired path and computing the direction of that point (Figure 6). The intersection
point, (xcarrot , ycarrot ), that is furthest along the path is always selected. The heading error is then
computed as:
φerr = φcarrot − φpose (23)

A large radius results in a smooth motion of the rover, but tends to filter out small features of the path.
A small radius reduces the total path following error. It requires, however, large heading changes of the
rover for small path errors, which is extremely inefficient. A circle radius is selected that balances the

14
desire to closely follow the path and the magnitude of the heading changes. Under nominal conditions,
the rover path error will always be smaller than the circle radius. If this is not the case, then the radius
is grown until an intersection occurs.

6.2 Slip-Compensated Path Following Algorithm

When the Mahalanobis comparator determines that slippage has actually occurred, the calculation of
rover slip is made by comparing the output from the Kalman Filter and the output from the forward
kinematics. If statistically significant slippage is not detected then the slip vector consists of zeros and
the compensation algorithm described below converges to a heading controller.
The slip compensation algorithm consists essentially of two separate control loops. The first control
loop, the heading controller, is described by the equation:

φ̇cmd = (K1 · φerr + K2 · φ̇slip )/Ts (24)

This loop determines the commanded yaw rate of the vehicle as a combined function of the heading
error, φerr (as calculated by the carrot heading algorithm), and the yaw slip rate, φ̇slip . It attempts
to achieve the optimal heading, determined by the carrot algorithm, even when slipping in the yaw
direction. The second loop is described by the equation:

ẏcmd = K3 · ẏslip /Ts (25)

This loop calculates the rate of the rover along the y-axis based entirely on the slip in the y direction
during the previous sample period. A ẏcmd command results in a crabbing maneuver, where all six
wheels have a steering angle offset in the same direction. ẋslip is implicitly compensated for by the rover
driving for a longer period of time along the path towards the goal. In Equations (24) and (25), K 1 , K2 ,
and K3 are tuned controller gains and Ts is the controller sample period. ẋcmd is then determined to be
the maximum value allowed that keeps the rover within its operational constraints (i.e., the maximum
speed of the drive motors). These rover commands, [ ẋ ẏ φ̇ ]cmd , are then passed to the inverse
kinematics.

6.3 Continuous Motion

With minor changes made to the algorithm it was possible to allow continuous motion of the rover while
compensating for slippage. Images are taken, visual odometry is run, the slip vector is estimated, and
the compensation command is then computed, all while the rover is moving.
There are many benefits for selecting a continuous motion profile: (i) more efficient motion (both
in energy and time); (ii) less slippage due to smaller changes in momentum (this becomes particularly
significant as the rover increases in mass and the terrain becomes steeper); and (iii) it enables a higher
rate of slip compensation, thus achieving greater accuracy when following a path. It also removes the
requirement of many navigation algorithms to use arcs as the fundamental motion of the rover and

15
 Table 1: Rocky 8 Rover Specifications
number of driveable wheels 6
number of steerable wheels 6
wheel diameter (meters) 0.20
wheel width (meters) 0.10
mass (kg) 60
wheelbase (meters) 0.75
track width (meters) 0.62
maximum linear velocity (meters/sec) 0.09
maximum rotational velocity (rad/sec) 0.24
camera resolution (pixels) 640×480
camera field of view (horizontal × vertical )
◦ ◦
79.5×64.0
camera baseline (meters) 0.084
camera angle from horizontal ( )◦
45.0
processor type and speed (GHz) PentiumIII 1.2

permits more complex spline paths to be the basis of path planning. Another advantage is that it
supports more optimal spacing between navigation stops (allowing for other requirements, such as IMU
bias zeroing, to determine the appropriate spacing between stops).

7 EXPERIMENTAL RESULTS
Three sets of experiments have been performed using Rocky 8 (Figure 4), a Mars rover research platform
developed at JPL. This rover has a very similar mobility system to Sojourner, Mars Exploration Rovers
(MER), and the current design of the 2009 Mars Science Laboratory (MSL) rover. Rocky 8 utilizes
a rocker-bogie suspension (see Section 3.1) which allows for excellent mobility over very rough terrain
[18]. Specifications of the Rocky 8 rover are in Table 1.
With the processor onboard the rover, visual odometry runs at approximately 1 Hz. This rate dictates
the sample period of the slip-compensated path follower, Ts . Several parameters of the slip-compensated
path follower also had to be tuned in the field. The three gains of the control loops K1 , K2 , and K3 in
Equations (24) and (25) were tuned by first increasing K1 until the heading error was maintained at an
acceptably low value without inducing oscillations in the heading of the rover. Then K 2 was increased
until the rover acceptably compensated for yaw slippage under the conditions experienced in the field.
Finally K3 was increased until the slippage in the y direction was effectively compensated for without
inducing crabbing oscillations. Additionally, a radius of the carrot heading algorithm was determined
that balanced the need to closely follow the path with the need to minimize heading changes.
The first set of experiments was performed in the JPL Marsyard, a 20×20 meter space designed as an

16
 4

2 ground truth

y (meters)
visual odometry
1

−1
0 2 4 6 8 10 12 14 16
x (meters)

Figure 7: Mars Yard Run 1 Visual Odometry Results.

analogue (in rock size/distribution and soil characteristics) to the Viking Lander sites. The experiment
consisted of two consecutive 25-meter runs with visual odometry running onboard. The second set
of experiments was performed in the Mojave Desert. The terrain of this area contains slopes up to
25◦ consisting of loose granular sand (Figure 1). This set of experiments was a test of a simplified
integrated slip-compensated path following system. It was simplified in the sense that the Kalman filter
and Mahalanobis comparator had not yet been implemented, however, a slip estimate was calculated
and compensated for every time the visual odometry provided a new estimate, which was approximately
every 20-30 cm. Another simplification, due to limitations of the vehicle, was to assume the rocker and
bogie angles were zero. The third set of experiments was performed on JPL’s tiltable platform, a 5×5
meter “sandbox” that could be tilted in discrete increments from 0◦ to 30◦ . The terrain on the tiltable
platform can be characterized as cohesionless beach sand. In the experiments presented here it was set
at 10◦ . This set of experiments consisted of multiple runs at varying approach angles (ranging from
straight up the 10◦ slope to horizontally across the slope), and used the updated continuous motion
algorithm described in Section 6.2.
In the first two sets of experiments, ground truth data were collected with a Leica Total Station (LTS),
which is a laser-based position measurement system. The LTS was used to measure the absolute position
of four prisms mounted on the vehicle (Figure 4) every time the rover was stationary (approximately
every 20-30cm). This system gives an accuracy of ±2 mm in position and ±0.2 ◦ in attitude. In the field
test experiments the waypoints for the rover were also designated using the LTS and a single prism. In
the third set of experiments, the LTS could only be used for ground truth at the beginning and end of
the runs because the rover did not stop during each test.

7.1 Visual Odometry Results

Visual odometry results are shown from the Marsyard (Figures 7 and 8) and the Mojave Desert ex-
periments (Figure 9). The errors at the end of both Marsyard runs are less than 2.5% of the distance
traveled. As shown in Figure 9, the error (0.37 m) at the end of the field test run, at the Mojave Desert,
is less than 1.5% of the distance traveled (29 m).

17
 10

ground truth
6
visual odometry

y (meters)
4

0 2 4 6 8 10 12 14 16 18
x (meters)

Figure 8: Mars Yard Run 2 Visual Odometry Results.

7.2 Slip-Compensated Path Follower Results

Results of the slip-compensated path following algorithm are shown in Figures 10 and 11. The entire
section of the path shown in Figure 10 was on a slope of between 10◦ and 15◦ . Figure 10 is an expansion
of the box shown in Figure 9 (note that the axes of Figure 10 have been rotated in order to minimize
the vertical size of the plot). Figure 11 is an expansion of the box shown in Figure 10. These two
figures show three important pieces of information that the slip-compensated path following algorithm
uses to calculate the rover commands: visual odometry pose (as described in Section 2), kinematics
pose (as described in Section 3), and the desired path. From this information the slip-compensated path
follower calculates the rover commands as described in Section 6. Carrot heading, which is calculated
in an intermediate step, is also shown. In Figure 10, the rover was able to accurately and efficiently
follow the desired path, despite significant slippage. As can be seen in Figure 11, there is a noticeable
bias between the visual odometry pose and the kinematics pose in the y direction. This is due to the
downhill slippage of the rover; this bias is being compensated for by the slip compensation algorithm,
as is evident from the fact that the rover accurately follows the desired path.

7.3 Continuous Motion Results

Results from the continuous motion slip compensation experiments are shown in Figure 12 (see Sec-
tion 7.2 for a description of the elements of this plot). The top graph in the figure shows a 4.5 meter
run using continuous slip compensation on a 10◦ slope (the slope is going down towards the positive
y-axis). Again, notice the constant offset between the kinematic and visual odometry estimates, indi-
cating slippage. The bottom graph shows an identical run without using slip compensation. As can be
seen, the significant slippage shown in the bottom graph was compensated for in the run shown in the
top graph. The mean square path offset error is 0.05 and 0.54 meters2 for the runs with and without
slip compensation, respectively. This is a significant improvement in performance.

18
 2
expanded below
y (meters) 0 in Fig. 10

-2
ground truth
visual odometry
-4

0 5 10 15 20 25
x (meters)

Figure 9: Field Test Visual Odometry Results.

kinematics pose expanded below

2 visual odometry pose in Fig. 11
1.5 carrot heading
desired path
y (meters)

0.5

0 2 4 6 8 10
x (meters)

Figure 10: Field Test Slip Compensation Results.

2.2
y (meters)

2
1.8
1.6
1.4

5 5.5 6 6.5 7 7.5 8

x (meters)

Figure 11: Expanded Slip Compensation Results.

19
 0.4

y (meters)
0.2
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
x (meters)
1.5
kinematics pose
visual odometry
1 pose
carrot heading
y (meters)

desired path
0.5

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
x (meters)

Figure 12: Tiltable Platform Slip Compensation Results (top: slip compensation enabled; bottom: slip
compensation disabled).

8 CONCLUSIONS
In this paper, we have described the design, implementation, and testing of a system that enables a rover
to accurately follow a designated path, compensate for slippage, and reach intended goals, independent
of terrain geometry and soil characteristics along the path (within the mechanical constraints of the
mobility system). Individual components have been simulated and tested; additionally, an integrated
system has been tested onboard a rover in a desert field test. The results from the individual and
integrated tests are encouraging. Visual odometry is able to consistently estimate rover motion to
within 2.5% of distance traveled. Given this knowledge, the slip-compensated path following algorithm
is able to accurately estimate and effectively compensate for slip and thus accurately follow a desired
path and reach the intended goal while traversing through a high-slip environment. The algorithm has
also been extended to allow for continuous motion slip compensation. Future work includes integration
of this slip-compensated path following algorithm with a path planning/obstacle avoidance system such
as Morphin or GESTALT [28, 29].

9 ACKNOWLEDGEMENTS
The research described in this publication was carried out at the Jet Propulsion Laboratory, Califor-
nia Institute of Technology under contract from the National Aeronautics and Space Administration
(NASA), with funding from the Mars Technology Program, NASA Science Mission Directorate. The
work of S.I. Roumeliotis was supported by the Jet Propulsion Laboratory (Grant No. 1248696), and
the National Science Foundation (ITR-0324864, MRI-0420836).

20
REFERENCES
[1] D. Helmick, S.I. Roumeliotis, Y. Cheng, D. Clouse, M. Bajracharya, and L. Matthies, Slip Com-
pensation for a Mars Rover, In Proc. 2005 IEEE International Conference on Intelligent Robots
and Systems, Edmonton, Canada, Aug. 2-6, 2005, pp. 1419-1426.

[2] D. M. Helmick, Y. Chang, S. I. Roumeliotis, D. Clouse, and L. Matthies, Path Following using
Visual Odometry for a Mars Rover in High-Slip Environments, In Proc. of the 2004 IEEE Aerospace
Conference, Big Sky, MT, Mar. 6-13.

[3] Alonzo Kelly and Bryan Nagy, Reactive Nonholonomic Trajectory Generation via Parametric Op-
timal Control, The International Journal of Robotics Research, Vol. 22, No. 7-8, 583-601 (2003).

[4] S. Hayati, R. Volpe, P. Backes, J. Balaram, R. Welch, R. Ivlev, G. Tharp, S. Peters, T. Ohm, R.
Petras, and S. Laubach, The Rocky 7 Rover: A Mars Sciencecraft Prototype, Proc. of the IEEE
International Conference on Robotics and Automation, Albuquerque, NM, April 20-25, 1997.

[5] R. Volpe, J. Balaram, T. Ohm, and R. Ivlev, Rocky 7: a next generation Mars rover prototype,
Advanced Robotics, Vol. 11, No. 4, pp. 341-358 (1997).

[6] P. Schenker, et. al., Reconfigurable Robots for All Terrain Exploration, Proceedings of SPIE, Vol.
4196, October, 2000.

[7] R. Hogg, A. Rankin, S. Roumeliotis, M. McHenry, D. Helmick, C. Bergh, and L. Matthies, Algo-
rithms and Sensors for Small Robot Path Following, In Proc. 2002 IEEE International Conference
on Robotics and Automation, Washington D.C., May 11-15.

[8] L. Matthies, Dynamic Stereo Vision, PhD thesis, Carnegie Mellon University, October 1989.

[9] C. F. Olson, L.H. Matthies, M. Shoppers, and M. Maimone, Robust stereo ego-motion for long
distance navigation, Proc. of the IEEE Conference in Computer Vision and Pattern Recognition,
Vol. 2. 2000.

[10] C. F. Olson, L.H. Matthies, M. Shoppers, and M. Maimone, Stereo ego-motion Improvements for
robust rover navigation, Proc. of the 2001 IEEE International Conference on Robotics & Automa-
tion.

[11] Yang Cheng, Mark Maimone, Larry Matthies, Visual Odometry on the Mars Exploration Rovers,
IEEE Conference on Systems, Man and Cybernetics, The Big Island, Hawaii, USA, October 2005.

[12] R. Deriche and G. Giraudon, A computational approach for corner and vertex detection, Int’l J. of
Computer Vision Vol. 10, No 2. pp 101-124, 1993.

[13] P.H. Schonemann, A generalized solution of the orthogonal Procrustes problem, Psychometrika,
31:1-10, 1966.

21
[14] P. J. Rousseeuw, Least median-of-squares regression, Journal of the American Statistical Associa-
tion, 79:871–880, 1984.

[15] M. Tarokh, G. McDermott, S. Hayati, and J. Hung, Kinematic Modeling of a High Mobility Mars
Rover, Proc. of the IEEE International Conference on Robotics & Automation, May 1999.

[16] M. Tarokh, G. McDermott, and J. Hung, Kinematics and Control of Rocky 7 Mars Rover, Prelim-
inary Report, Dept. of Math & Computer Sciences, San Diego State University, August 1998.

[17] P. F. Muir and C. P. Neumann, Kinematic Modeling of Wheeled Mobile Robots, Journal of Robotics
Systems, Vol. 4, No. 2, pp. 281-340, 1987.

[18] D. Bickler, A New Family of JPL Planetary Surface Vehicles, In Missions, Technologies, and Design
of Planetary Mobile Vehicles, pages 301-306, Toulouse, France, September 28-30, 1992.

[19] J. J. Craig, Introduction to Robotics, 2nd Ed., (Reading, Massachusetts: Addison Wesley), 1989.

[20] B. Friedland, Analysis strapdown navigation using quaternions, IEEE Transactions on Aerospace
and Electronic Systems, AES-14(5): 764-768, Sep. 1978.

[21] E. J. Lefferts and F. L. Markley, Dynamics modeling for attitude determination, AIAA Paper
76-1910, Aug. 1976.

[22] S. I. Roumeliotis, A Kalman filter for processing 3-D relative pose measurements, Technical report,
Robotics Laboratory, California Institute of Technology, Sep. 2001.
http://robotics.caltech.edu/∼stergios/tech reports/relative 3d kf.pdf

[23] S. I. Roumeliotis, G. S. Sukhatme, and G. A. Bekey, Circumventing dynamic modeling: Evaluation

of the error-state Kalman filter applied to mobile robot localization, In Proc. of IEEE International
Conference on Robotics and Automation, Detroit, MI, May 1999.

[24] S. I. Roumeliotis, Robust Mobile Robot Localization: From single-robot uncertainties to multi-robot
interdependencies, PhD thesis, Electrical Engineering Department, University of Southern Califor-
nia, Los Angeles, CA, May 2000.

[25] S.I. Roumeliotis and J.W. Burdick, Stochastic Cloning: A generalized framework for processing
relative state measurements, In Proc. 2002 IEEE International Conference on Robotics and Au-
tomation, Washington D.C., May 11-15, pp. 1788-95.

[26] A. Kelly, A Feedforward Control Approach to the Local Navigation Problem for Autonomous Vehi-
cles, Robotics Institute Technical Report, CMU-RI-TR-94-17, Carnegie Mellon University, 1994.

[27] S. Singh et. al, FastNav: A System for Fast Navigation, Robotics Institute Technical Report CMU-
RI-TR-91-20, Carnegie Mellon University, 1991.

22
[28] S. B. Goldberg, M. W. Maimone, and L. H. Matthies, Stereo Vision and Rover Navigation Software
for Planetary Exploration, In 2002 IEEE Aerospace Conference Proc., Vol. 5, Big Sky, MT, March
2002.

[29] S. Singh, K. Schwehr, R. Simmons, T. Smith, A. Stentz, V. Verma, and A. Yahja, , Recent
progress in local and global traversability for planetary rovers, In Proc. of International Confer-
ence on Robotics and Automation, 2000.

23