Motion Planning for Mars Rover
Zvi Shiller
Dept. of Mechanical and Aerospace Engineering
University of California
Los Angeles, CA 90095
Abstract
This paper presents an global-optimal motion planner for Mars Rover. It accounts for traversability and
dynamic stability due to vehicle dynamics and terrain
topography. A given path is traversable if along it
the vehicle is statically stable. The vehicle is dynamically stable if its velocity does not exceed the velocity
limit, determined from tip-over, sliding, and contact
constraints. The cost function for the optimization
consists of the path distance divided by the lowest velocity limit along the path. It produces paths that are
traversable (and are hence dynamically stable) at the
widest speed range.
1 Introduction
Future exploratory missions to Mars, planned for
the years 2003, 2005, and 2007, will incorporate autonomous Rovers, capable of long range navigation in
the order of 100 m or more per communication cycle
[1]. The rst Rover to reach Mars, Sojourner, was
guided remotely from Earth by an operator who selected traversable paths over images returned from the
lander. Its range of motion was limited to an area no
larger than 20 meter square around the lander. To traverse larger distances, it is necessary to delegate the
motion planning task to the Rover, using terrain information obtained by a pair of mast mounted stereo
cameras. The challenge of the onboard path planner
is thus to select a traversable path that minimizes distance, maximizes dynamic stability, and stays within
the given terrain map.
Current motion planners focus, almost exclusively,
on obstacle avoidance, treating obstacles as forbidden
regions and the rest of the terrain as free-space [2].
Such binary representation of obstacles is inadequate
for typical Martian terrain, since strictly avoiding all
obstacles may result in extremely long paths, or in
no solution, declaring the goal unreachable [3]. It is
therefore desirable to employ a dierent obstacle representation that allows evaluating paths that climb obstacles in order to reach the goal within a given terrain
map.
Few path planners that consider traversability of
rugged terrain have been developed [11, 8, 7, 5, 10].
In [11], the shortest feasible path for o-road vehicles
is computed, considering terrain topography and vehicle geometry. While the selected path is ensured
to be statically safe, it does not account for safety
margins and for the eect of vehicle dynamics. A recently developed planner [8] uses a genetic algorithm
to synthesize paths from segments, each evaluated for
its static stability and for satisfying the mission tasks.
Another genetic-based planner [5] uses fuzzy logic to
account for the heights of obstacles in each region of
the terrain.
Terrain topography and simple vehicle dynamics
were considered in [10] to generate the global optimal path on a general terrain. This planner optimizes
either distance or motion time. While minimizing motion time is not essential for Mars Rover because of
its extremely low speeds (10 , 20m=s), time-optimal
paths make excellent candidates since they can be
traversed safely at the highest speeds. Since a path
traversable at high speeds is obviously safe at lower
speeds, the range of the safe speeds provides a useful
measure for traversability of a given path. This planner, therefore, addresses the main limitation of the
current planners by quantifying the trade-o between
and
traversability and path length. This paper reviews the
planner presented in [10], and discusses its application
to Mars Rover.
This planner formulates the motion planning problem as a three stage optimization. At the lowest level,
a given path is evaluated for its traversability by computing the maximum speeds along the path at which
the vehicle is dynamically stable. The terrain is represented by a smooth cubic B patch, generated from
control points that are calculated from the stereo images produced by the mast mounted cameras. The
vehicle is modeled as a suspended point mass. This
model, though simple, allows the consideration of stability due to such constraints as sliding, tip-over, and
loss of contact with ground. Dividing the path length
by the maximum constant velocity that does not violate any of the constraints produces the motion time
along the path at the maximum constant speed, which
is used as a quality measure for the path: the faster
the better.
The second level consists of a parameter optimization that selects a locally optimal path in the neighborhood of an initial guess. The parameters for this optimization consist of the control points of the B spline
used to represent the path. The dimension of this optimization is kept small by representing the path in
the two dimensional space of the surface variables.
The third and highest level of the optimization selects the initial guesses for the local optimization. The
global optimization is based on a branch and bound
search that prunes the initial set of all paths between
the end points to a small number of candidates for
the local optimization [9]. These candidates represent
the most promising regions, one of which contains the
global optimal path.
Optimizing these paths with the local optimization yields the best path, in addition to a number of
good alternatives. These paths are not necessarily the
shortest, but they are traversable at the widest speed
range of all paths with similar or shorter lengths, as
is demonstrated in several examples in this paper.
(3)
M is the 4 4 matrix specifying the type of spline
used to construct the patch, and R is the 4 4 matrix
of 16 control points:
3
2
r00 r01 r02 r03
r10 r11 r12 r13 77
(4)
r20 r21 r22 r23 5
r30 r31 r32 r33
Each element r in R is a point in three dimensional
space, which makes R a 4 4 3 array. For a B spline
patch, M is selected to be [6]:
6
R = 64
i;j
2
,1 3 ,3 1
6
M = 61 64 ,33 06 33 00
3
7
7
5
(5)
1 4 1 0
It is possible to represent a surface with several
patches by adding control points in each direction.
The control points of the patch are generated by
placing a uniform grid on the map-range data generated from stereo images taken by the on-board mast
camera. The resolution of this grid is chosen economically at about half the Rover size: roughly 20cm between neighboring points. This ensures that obstacles
the size of the Rover and larger are depicted by the
B-patch. Smaller obstacles may be ltered out.
The path is represented by a smooth curve on the
surface, obtained by parameterizing v and w by a single parameter u:
c(u) = p(v(u); w(u)) = V (u)MRM W (u) (6)
Reducing the v , w space to a line reduces the B
patch to a continuous curve, as shown schematically
in Figure 1. The line in the v , w space is represented
by a B spline: v(u) = UMR
(7)
w (u )
where,
(8)
U = [u3 ; u2 ; u; 1]; u = [0; 1];
M is dened in (5), R is a 4x4x2 array of the control
points in v , w space, and u is the independent parameter along the B spline. This path representation
guarantees that the curve stays always on the patch.
The mapping dened in (6) and (7) retains the second
order continuity of the B spline.
T
T
c
2 Terrain and Path Representation
c
The terrain is represented by a bicubic B patch,
which is a parametric surface made of a mesh of cubic
splines. Topologically, the patch is a warped rectangle
in a three dimensional space. A typical point p on a
single patch in three dimensional space is a function
of two parameters, v and w, :
(1)
p = V MRM W
where
(2)
V = [v3 ; v2 ; v; 1]; v = [0; 1]
T
W = [w3 ; w2 ; w; 1] w = [0; 1]
3 Vehicle Model
At top speeds of 10 - 20 cm/s, the motion planning
problem for Mars Rover can be considered a kinematic
problem. However, we do account for Rover dynamics
T
2
acceleration s at a given point along the path. These
equations can be used to determine the required forces
for the prescribed motion, or alternatively, to determine the feasible speed and acceleration for given limits on the friction and normal forces.
Three Dimensional
Curve
Two
Dimensional
Curve
u
w
w
Path Parameter
4 Dynamic Constraints
Three Dimensional
Patch
v
v
Control Points
Several constraints between the vehicle and ground
are considered to ensure vehicle dynamic stability
along the path. The constraints treated here include: a) limits on the coecient of friction (sliding constraint), b) contact between the vehicle and
ground (contact constraint), and c) tip-over. These
constraints apply to zero as well as non-zero speeds,
hence the term "dynamic." Generally, each constraint
can be transformed to constraints on the vehicle's tangential speed and acceleration as discussed next.
Figure 1: A B-patch
for the purpose of quantifying traversability and dynamic stability, with the premise that paths that are
traversable at a wide speed range are safer than those
that are not.
The vehicle is modeled as a point mass, suspended
above ground at the location of the vehicle's center of
mass. The height of the center of mass above ground
and the width between the wheels are used to evaluate
stability with respect to lateral tipover.
4.1 Sliding Constraint
The maximum friction force is a function of the
normal force and the coecient of friction between
the wheels and ground:
jF j R
(12)
Alternatively:
(13)
F 2 = f 2 + f 2 2 R2
Substituting (9) to (11) in (13) yields a quadratic inequality in s:
s2 + 2gk s + 2 (n2 , 2 n2 )s_4 + 2g(k n ,
2 k n )s_2 + g2 (k2 + k2 , 2 k2 ) 0 (14)
Solving (14) for s yields the feasible range of acceleration along the path due to the sliding constraint:
(15)
s s s
where
p
(16)
s = ,gk + pas_4 + 2bs_2 + c
(17)
s = ,gk , as_4 + 2bs_2 + c
and
(18)
a = 2 (2 n2 , n2 )
2
(19)
b = g( k n , k n )
(20)
c = g 2 ( 2 k 2 , k 2 )
Note that the maximum acceleration is not necessarily positive, nor the maximum deceleration is always negative; it depends on k which is the vertical component of the forward slope: k is positive for
a positive (uphill) slope, and negative on a downhill
slope. If the terrain is horizontal, then k = k =
n = 0, n = 1, and (14) reduces to a simple constraint on the acceleration as a function of the vehicle
speed, path curvature, and the coecient of friction:
(21)
s2 2 g2 , 2 s_4 :
R
F
t
mg
t
Figure 2: External Forces
t
t
t
q
q
q
r
r
q
r
The external forces acting on the vehicle consist of
the friction force F (the sum of all the horizontal tire
forces), the normal force R (the sum of all normal
tire forces) applied by ground on the vehicle in the r
direction, and the gravity force ,mgk, as shown in
Figure 2.
The equation of motion of the vehicle are written
in the vehicle xed frame in terms of the tangential
speed s_ and the tangential acceleration s [10]:
(9)
f = mgk + ms
(10)
f = mgk + mn s_2
(11)
R = mgk + mn s_2
where f and f are the components of the friction
force tangent and normal to the path, R is the normal
force (the sum of all normal tire forces), = 1=, and
is the path curvature. The moment of the friction
force around the center of mass is missing from the
equations of motion; it is considered later when we
account for the tipover constraint.
Equations (9) to (11) represent the friction and normal forces required to maintain vehicle speed s_ and
q
q
r
r
q
t
d
r
a
a
t
d
t
r
r
r
q
q
q
r
q
q
q
t
t
t
r
3
q
q
In addition to limits on the acceleration, (14) expresses also limits on the feasible vehicle speed along
the path, since the argument under the square root
should be positive :
(22)
= as_4 + 2bs_2 + c 0
The feasible speed range is determined by the roots of
(22). Only the positive roots are of interest.
Depending on the values of the coecients a; b; and
c; it is possible to obtain a feasible speed range which
does not include s_ = 0. In such a case, the vehicle is
statically unstable due surface characteristics (traction
and geometry).
r
q
b
h
fq
R
Figure 3: A Clockwise Tip-over
Similarly, the condition for not tipping-over clockwise
is:
(27)
f R hb
These conditions can be combined to one constraint
equation:
(28)
f 2 (R hb )2
4.2 Contact Constraint
To ensure that the vehicle does not loose contact
with ground on bumpy terrain, the normal force R
applied on the vehicle should always be positive:
R0
(23)
Setting R = 0 in (11), we obtain the maximum speed
allowed by the contact constraint:
q
q
Substituting (10) and (11) into (28) transforms the
tip-over constraint to a quartic inequality in s_ :
a1 s2 + 2(a2 s_2 + a3 )s + a4 s_4 + 2a5 s_2 + a6 0 (29)
where
(30)
a1 = sin2 (31)
a2 = n sin cos (32)
a3 = g sin (k sin + k cos )
2 2
2
2 2
(33)
a4 = (n cos , n )
2
)
a5 = g(n cos (k sin + k cos ) , n k (34)
2
2
2 2
(35)
a6 = g [(k sin + k cos ) , k ]
and = b=h.
Solving (29) for s, as in the sliding constraint, yields
a constraint similar to (14), which yields a constraint
for s_ similar to (22).
Note that the tipover constraint accounts for steep
slopes, as well as for tight curves on horizontal terrain.
The latter is more meaningful for modern sport utility
vehicles than for Mars Rover.
r
gk :
(24)
s_ , n
The projection of r on k, k , is always positive, otherr
r
r
wise the vehicle moves along an inverted slope. Equation (24) therefore applies only for the cases where n
is negative, which means that path curvature points
opposite to the direction of the surface normal. Note
that the velocity limit is innite for a at terrain (n =
0), and zero for a sharp vertical bump (n = 1), as
expected.
r
q
t
r
q
q
r
q
4.3 Tip-Over Constraint
t
The tip-over constraint is obtained by expressing
the limiting condition before the vehicle is about to
tip-over in terms of s_, s. Referring to Figure 3, the
vehicle is about to tip-over counter clockwise if the
total reaction force R is applied on the left wheels,
and the lateral friction force, f , points downward,
as shown (the vehicle will not tip-over CCW if the
friction force is applied in the opposite direction). The
moment created by the reaction force and the friction
force around the vehicle's center of mass causes the
tip-over. The vehicle will not tip-over CCW if the
resultant moment around its center of mass is positive:
q
r
t
q
q
4.4 Velocity Limit Curve
r
r
r
Plotting the velocity limits due to the dynamic constraints along the path forms the velocity limit curve
in the phase plane s , s_, as shown schematically in Figure 4. It represents the upper bound for vehicle speeds
for which the dynamic constraints discussed earlier are
satised. The height of the velocity limit represents
a measure of safety and traversability: a zero velocity
limit implies static instability, whereas a nonzero but
low velocity limit implies a stable but dangerous position along the path. Obviously, the higher the velocity
f h + Rb 0
(25)
where h and b are dened in Figure 3, and R is the
total reaction force between ground and the vehicle.
The condition for not tipping-over CCW is therefore:
(26)
f ,R hb
q
q
4
problem is formulated as an unconstrained parameter optimization, using the control points of cubic B
splines as the optimization variables.
A penalty function is added to evaluate infeasible
paths, along which the vehicle is statically unstable at
some points. The velocity limit at such points is zero,
sending cost function (motion time) to innity. To
retain the unconstrained nature of the problem, the
objective function is augmented with a penalty value
that is a function of the number of infeasible points
along the path:
J = J (a) + wn
(36)
where w is a weighting factor, and n is the number of
infeasible points. This penalty value is zero if the path
is completely feasible, and it increases gradually when
the path penetrates a forbidden region. The eect of
the penalty function is to push the path away from
those infeasible regions.
Rejecting infeasible regions serves to avoid large obstacles, which are modeled as part of the terrain. This
optimization, however, admits paths that might go
over obstacles if such a path is dynamically feasible
and it is less costly than going around.
limit, the wider the speed range that the vehicle can
move along the path without sliding, tipping over, or
ying o the ground.
•
S
Forbidden Region
c
Velocity Limit Curve
S
Figure 4: Velocity Limit Curves
5 Global Search
The search for the optimal path follows the method
presented in [9]. It combines a grid search in the position space with a local optimization to yield the global
optimal path for a variety of static and dynamic cost
functions, such as distance and motion time. This
approach eliminates the search in the 2n dimensional
state-space without sacricing global optimality.
The cost function for Mars Rover is computed by
dividing the path length by the maximum constant
speed that does not cross the velocity limits for that
path. This cost function is the minimum motion time
at the constant speed along the path. It quanties
the cumulative eects of path distance, terrain topography, and vehicle dynamics. It also favors regions
with high velocity limits, which are traversable at the
widest speed range.
The optimization starts by searching for a set of
best paths along a uniform grid over the terrain, using the Dryfus algorithm for K best paths [4]. K is
determined by an upper bound on the optimal cost.
Paths that score higher than the upper bound are discarded. The remaining paths are pruned by retaining
the best path in each neighborhood, each representing the neighborhood of a potential local minimum.
Submitting these paths to a local optimization that
further minimizes the cost function yields the global
optimal path in addition of a set of good alternatives.
7 Examples
The planner was tested on data produced from a
simulated Martian terrain in JPL's \MarsYard." The
original image was electronically "doctored" by Professor Tarokh, of San Diego State University, to make
the terrain more challenging by adding large rocks in
the central region.
Several paths were computed, as shown in Figures 5 to 7. These paths are not the shortest between
the respective end points, however, they seem to pass
through wider corridors than the shortest paths, and
are hence faster and safer.
8 Conclusions
A motion planner, previously developed for general
ground vehicles moving on uneven terrain, was implemented for Mars Rover. It computes the global optimal path between given end points, considering vehicle dynamics, and terrain topography. This planner
accounts for the vehicle's static and dynamic stability, using the velocity limits due to such constraints
as tip-over, sliding, and contact. Optimizing a cost
function that resembles motion time, produces paths
that avoid steep slopes, along which the vehicle is statically unstable due to sliding and tipover, and bumpy
regions, along which the velocity limits are low due to
the contact constraint. The current implementation is
6 Local Optimization
The local optimization is used to relax the demand
on grid resolution. It obtains the optimal path in the
vicinity of an initial guess, considering vehicle dynamics and terrain topography [10]. The optimization
5
Figure 5: Path #1
Figure 7: Path #3
Figure 6: Path #2
[4] E.L. Lawler. Combinatorial Optimization. Holt,
Rinehart and Winston, New York, 1976.
[5] R. Chan M. Tarokh and C. Song. Path planning
of rovers in rugged terrain using fuzzy logic and
genetic algorithm. submitted for publication.
[6] M.E. Mortenson. Geometric Modeling. John Wiley and Sons, New York, 1985.
[7] E. Papadopoulos and D. Rey. A new measure of
tipover stability margin for mobile manipulators.
IEEE Int'l Conf. on Robotics and Automation,
1996.
[8] Farritor S., Hacot H., and Dubowsky S. Physicsbased planning for planetary exploration. IEEE
Int'l Conf. on Robotics and Automation, 1998.
[9] Z. Shiller and S. Dubowsky. On computing the
global time optimal motions of robotic manipulators in the presence of obstacles. IEEE Transactions on Robotics and Automation, 7(6):785{797,
Dec. 1991.
[10] Z. Shiller and Y.R. Gwo. Dynamic motion planning of autonomous vehicles. IEEE Transactions on Robotocis and Automation., 7(2):241{
249, April 1991.
[11] T. Simeon and B. Darce-Wright. A Practical Motion Planner for All-terrain Mobile Robots. International Conference on Intelligent Robots and
Systems, 1993.
for o-line planning. It can be modied with no great
diculties for an on-board implementation.
References
[1] S. Hayati, Z. Shiller, M. Tarokh, and S. Laubach.
A comparison of three path planners for planetary
rovers. SAIRAS '99, 5th International Symposium on Articial Intelligence, Robotics and Automation in Space, 1999.
[2] Jean-Claude Latombe. Robot Motion Planning.
Kluwer Academic Publishers, Boston, 1991.
[3] S. Laubach, J. Burdick, and L. Matthies. A
practical autonomous path-planner for the rocky7
prototype microrover. IEEE Int'l. Conf. on
Robotics and Automation, 1998.
6