Integrated Path Planning and

ynamic Steering Control for Autonomous Vehicles

Bruce PI. Krogh* Charles E. Thorpe**

*Department of Electrical & Computer Engineering and the Robotics Institute,

**Robotics Institute, Carnegie-Mellon University, Pittsburgh, PA 15213, USA

ABSTRACT
A method is presentedforcombiningtwopreviouslyproposed schemeiscomputationallyfeasiblefor real-time implementation.
algorithmsforpath-planninganddynamicsteeringcontrolintoa Since the steering control algorithm handles local details, the path
computationallyfeasibleschemeforreal-timefeedbackcontrol of planner can work on a coarse scale. The set of critical points must
autonomous vehicles inuncertainenvironments.Intheproposed be
re-evaluatedonlywhen
there
are
extreme
changes
in the
approach to vehicle guidance and control, Path Relaxation is used environmental information. Moreover, the two levels of control can
to compute critical points alongagloballydesirable path using a be executed in parallel with the slower sensor processing and path-
priori informationand sensordata'.Generalized potential fields planningalgorithmgeneratingfuture subgoalswhile thesteering
are then used for local feedback control to drive the vehicle along a controlalgorithm guides the systemthroughthecurrentset of
collision-free path using the critical points as subgoals'. Simulation critical points.
results are presented to demonstrate the control scheme.
The following section reviews the path-relaxation algorithm and its
implementation for selecting the critical points to be passed to the
steering
control
algorithm.
We then describe the generalized
INTRODUCTION potential jield approach to dynamicsteeringcontrol using the
Systemarchitecturesandalgorithms proposedforguidance and criticalpointsandreal-time sensoryinformation to determinethe
control of autonomous vehicles normally
reflect
a hierarchical desired system acceleration a t each control instant. Features of the
proposed scheme are illustrated by simulation inciuding an example
decomposition of the problem3' 49'. Basic levelsin the system
where the environment encountered
by the
steering
control
hierarchy are illustrated in Fig. 1. At the highest level the problem
algorithmdifferssignificantlyfromthemodel used by thepath-
is represented symbolically and artificial intelligence techniques are
planning algorithm to generate the subgoals. The simulation results
used to performhigh-levelplanning. Atthe lowest level, servo
demonstratetheviability of the proposedapproach for real-time
controllers
track reference
signals,
implementing the desired
guidance and control of autonomous vehicles. Directions for future
dynamic trajectory.
This
paper concerns the vehicle guidance
research and issues currently being investigated are discussed in the
problem at the intermediate levels. Using two previouslyproposed
concluding section.
algorithms we integratethefunctions of geometric path planning
and dynamic steering
control.
The
path-planning
algorithm
generates a set of criticalpoints alongagloballydesirable path,
PATH RELAXATION'
while the steering control algorithm performs local navigation and
Forrobotpathplanning, we wantto find anoptimalpathtoa
obstacle avoidance.
destination through a field of obstacles. P a t h Relaxation is a two-
step process that tries to find the path with the lowest total cost.
Based onaninternalmap of the environmentdevelopedfrom a The cost of a path is acombination of several factors, including
priori informationand high-level sensory data,Path Relaxation
distance traveled, nearness to objects, traversability of the terrain,
selects a m i n i m u m cost collision-free path to the goal'. This path and uncertainty about the area. The first step of pathrelaxation
could be executed directly by driving the vehicle through each point finds a preliminary path on an eight-connected grid of points. The
onthepath in turn. Such simple-minded
a scheme,
however, second step adjusts, or "relaxes", the position of eachpreliminary
ignoresvehicledynamics,forces the trajectory to go through each path point to improve the path.
point,and is unable to use newsensory datawithout complete
replanning. A better strategy is to generate a set of critical points One advantage of path relaxation is that it allows manydifferent
along the path and to use them as subgoals for dynamicsteering
factors to be considered in choosing a path. Typical path planning
control using local feedback information. Dynamic steering control algorithms evaluate the cost of alternative paths solely on the basis
is accomplished in our system using Generalized Potential Fields'. of pathlength.The costfunction used by P a t h Relaxation,in
This makesfor a gooddivision of the problem: P a t h Relaxation contrast, alsoincludeshow close thepath comes toobjects(the
takescare of globalissues, like avoidingdeadends, finding an furtheraway,the lower thecost)andcan include penalties for
overalloptimal or near-optimalpath,and decidingbetween areas traveling through particular are=, such as areas outside the field of
withdifferentterrain or better visibility.Localissues,such as view of the robot. The effect is to produce paths that neither clip
cutting corners, slowing the vehicle to maneuver in tight spots, and the corners of obstaclesnor make wide deviations around isolated
generatingsmoothtransitionsfrom one steptothenext,are all objects,andthat prefer to stayinmappedareas unless apath
handledby the Generalized PotentialField feedbackalgorithm. throughunmapped regions issubstantiallyshorter.Otherfactors,
This dynamic steering control also reacts to new sensory data as the based on a priori information or sensor data, such as roughness of
vehicle
moves, taking
into
accountupdated
obstacle positions terrain or visibilityof landmarks,can be added for particular
withouthaving t o invoke the global path planner.This two-level environments.

1664
CH2282-2/86/0000/1664$Ol.000 I986 IEEE
GRID SEARCH minimize the cost of the path sectionson either side of the node.
The first stage of path relaxation finds an approximate path on a Sincemovingonenodemayaffectthecost of its neighbors, the
grid. The grid mesh size can be as large as the minimum dimension entire procedure is repeated until no node moves farther than some
of the robot and still guarantee that no path will be missed. Once small amount.
the grid has been set up, the next step is to assign costs to paths on
the grid and then to search for the best path along the grid from Node motion has to be restricted. If nodes were allowed to move in
the start to the goal. "Best", in this case, is a compromise among any direction, they would all end up a t low cost points, with many
three
potentially conflicting
requirements: shorterpath
length, nodesbunchedtogetherand a few longlinksbetween them.This
greater margin away from obstacles, and less distance in high-cost wouldnot give avery good picture of theactual cost alongthe
areas. These three requirements are explicitly balanced by the way path. So in order to keep the nodes spread out, a node's motion is
path costs are calculated. A path's cost is the sum of the costs of restricted to be perpendicular to a line between the preceding and
the nodes through which it passes, each multiplied by the distance following nodes. Furthermore, at any one step a node is allowed to
to the adjacent nodes. (In a 4-connected graph all lengths are the move no more than one unit. As a node moves, all three factors of
same, but in an 8-connected graph we have to distinguish between cost are affected:distance traveled(fromthe precedingnode, via
orthogonalanddiagonallinks.)The nodecostsconsist of three this node, to the next node), proximity to objects, and relationship
parts to explicitly represent the three criteria. tounmapped regions. Thecombination of these factorsmakesit
difficult to directly solve for minimum cost node position. Instead,
1. Cost for distance. Each node starts out with a cost of a binary search is used to find the minimum cost node position to
one unit, for length traveled. the desired accuracy.

2. Cost for near objects. Each object near a node adds to Therelaxationstephasthe effect of turning jaggedlines into
that node's cost. The nearer the obstacle, the more cost straight oneswherepossible, of finding the"saddle"inthe cost
it adds. The exact slope of the cost function will depend function between two objects,and of curving around isolated
on the accuracy of the vehicle (a more accurate vehicle objects. At the boundary between two regions of different cost, the
canafford to come closer to objects), and the vehicle's minimumcost path will "refract" in the same way that a ray of
speed (a faster vehicle can afford to go farther out of its lightrefracts crossing a boundary between two substances. The
way), among other factors. laws of refraction state that a ray of light crossing a border between
two substances with different transmission velocities will follow the
3 . Cost forbeing within or near anunmapped or other paththat minimizestransmission time.Thepath foundby path
high-costregion. The cost for traveling inone of these relaxation will "refract"in ananalogous way, minimizingcost
regions depends on the particular mission of the vehicle. rather than time.
Forinstance, if the goal is primarilyexploration,then
unmapped areas could have lower costs than other areas.
The
output of pathrelaxation is a sequence of positions
There is also a cost added for being near an unmapped
{xo,...X,=X } from the initial position of the vehicle x. to the goal
region, using the same sort of function of distance as is
used for obstacles. This provides a buffer to keep paths x . As demonstrated by the examples below, these positions can be
from coming too close to potentially unmapped hazards. generated on relatively
a coarse
scale. The
steering
control
algorithm performs the function of local navigation, using the set
For costs 2 and 3 we use a cubic cost function, as illustrated in Fig. {x,, ...,x,} as criticalpoints or subgoals along thedynamic
2, which ranges from 0 a t some maximum distance, set by the user, trajectory. Determining the best set of critical points is an area for
to the obstacle's maximum cost at 0 distance. This function has the future research.
advantages of giving good saddlesbetweenneighboringobstacles,
being easy to compute, and being bounded in a local area.
DYNAMIC STEERING CONTROL
The cost calculatedfor eachnodeisbasedon the distances to In this section we presenta feedback algorithm for thedynamic
nearby obstacles and whether that node is within a high-cost area. steering
control of autonomous vehicles using
local
feedback
Next, eachnode is linked toits8 neighbors. Thestart andgoal information. Given set a of subgoals from the path-planning
locations do not necessarily lie on grid points, so special nodes need algorithmwhich roughlydefine a desirable pathtothe goal, the
to be created for them and linked into the graph. steering
control
algorithm
must determine
the
appropriate
The system then searches thisgraph for theminimum-cost path accelerationof thesystemat each instantto guide it alonga
fromthestarttothe goal,using astandard A* search6. The collision-free trajectorytothe goal.Since theenvironment is not
estimated total cost of a path, used by A* to pick which node to knownentirely atthepath-planning level, oursteeringcontrol
expandnext, is thesum of the cost so far plus thestraight-line algorithmincorporatesreal-time sensor information;thatis,the
distance from the current location to the goal. This has the effect, steeringcontrol is specified as a feedbacklaw ratherthanan
in regions of equal cost, of finding thepaththat most closely open-loop algorithm.
approximates the straight-line path to the goal.
The steering control algorithm is based on the generalized potential
RELAXATION field approach for obstacle avoidance control proposed by Krogh'.
Gridsearch
findsan
approximatepath;
the
nextstep is an In other potential field methods proposed by Hogan and Khatib, an
optimization step that fine-tunes the location of eachnodeon the artificial force vector is introduced as the sum of the gradients of
path to minimize the total cost.One way to do thiswould be to position-dependent potential fields forthe obstacles and goal7' 8 .
define precisely the cost of the path by a set of non-linear equations The introduction of generalized potential fields, that is, potential
and solve them simultaneously to determine the optimal position for fields which areboth position and velocity dependent, eliminates
each
node. This
approach is not, in
general, computationally the possibility of the stalling a t local minima in the potential fieldg.
feasible. Theapproach used here is arelaxationmethod.Each Otheradvantages of the generalized potential field approachare
node's position is adjusted in turn, using only local information to briefly described in this section'.

1665
PROBLEM FORMULATION finite
sampling
time
(discussed
below).
Thus, u is chosen to
minimize
As in the previoussection, we consider the problem of obstacle
avoidance in the plane, modeling the system as a point a t position qx,v)=vxP& V Y P ; (4)
x=[x1,x2IT withvelocity V = [ V ~ , Y (in
~ Cartesian
]~ coordinates).
The steering control is the acceleration vector u = [ u ~ , u ~which
] ~ is which gives the control
assumed to be constrained in magnitude, that is,
u=-cr~~vYq~-lvYP (5)
llull i a. (1)
when only constraint (1) is active.
The velocity can also be limited by a maximum speed, IC,that is
The potential function is given by
llvll F IC. (2)
qx,v)'P~(x,v)+Po(x,v) (6)
Thus, the dynamics of the system are given by
.- .
x=v=u (3)
where P (x,v), P , ( x , v ) a r e independent potential functions for
goal (or subgoal)andobstacles respectively. As described below
the

these
potential
functions
are defined so that the system is
subject to the constraints (I), (2). "attracted" by the goal and"repulsed" by the obstacles. The
dependence of P and Po on the system velocity as well as position
This simpledynamicmodelis used tofacilitatethereal-time takes into account the dynamic aspects of the trajectory and control
computation of the desiredsystemacceleration u. Thesteering constraints.
control problem is mosteasily formulated in the global coordinate GOAL POTENTIAL
frame of the system environment since
set of criticalpoints,
distancesto obstacles andthe goalposition are specifiedin these T o compute thegoal-attractionpotential, P (x,v), anappropriate
coordinates. The mapping of the actual system dynamic model and subgoal
first
is selected
from the
set of critical
points
control constraints (most easily specified in generalized coordinates) {x1, ...,x =x }. Thecriticalpointsarepursued in sequence; the
into globalcoordinatesis discussed byKroghand Graettinger". subgoal is updated to the next critical point when the system begins
To use the model (3) i t is necessary to choose the acceleration and to decelerate toward the current subgoal. If the next critical point
velocity constraints (l), (2) so that the desired acceleration is not visiblefrom the current system position x, an intermediate
computedby thesteeringcontrolalgorithm is feasible. A method subgoalis chosen as the edge of the obstructing obstacle which is
for computing these maneuverability constraints and applications closest tothe desired
subgoal.Thus, if thereal-time sensing
to autonomous vehicle control are currently under investigation". indicatesthe presence of anobstacle which wasnottakeninto
In the present paper we use the model (1)-(3) to illustrate our basic accountby the path-planner, this intermediate subgoal directs the
approach to steering control. systemaroundtheobstacle.Notethatthisleadstoatrajectory
that follows the general path specified by the sequence of critical
Thesteeringcontrolobjectiveistodrivethesystem along
a points, but the vehicle does not stop or even pass through each x ..
collision-free pathfromitsinitialposition x. to a specified goal
position x , The higher-level path planningalgorithmprovidesa For the selected subgoal x . the goal attraction potential P (x,v) is
set of critical points {x1, ...,xn=x } which represents the best path computed so that the gradient results in an acceleration which will
J!
in terms of global information about the locations of obstacles and eventuallybring the system torest a t x . Krogh proposeda
the environment. As the vehicle progresses through
the heuristic decomposition was used to compute orthogonal
environment,real-timesensingprovidesmoredetailed and more components of the goal attractingpotential gradient". This was
exact feedback information on the locations of the obstacles within based on the time required to reach the goalusing themaximum
the local vicinity of the vehicle.Using thesedata,thesteering accelerationavailable ineachorthogonaldirection,ignoring the
control algorithm must determine the appropriate acceleration u a t obstaclesand
the acceleration allocationfor
the
orthogonal
each instant. direction. Recently, a computationally feasible method was derived
to find the optimal time to go for the two-dimensional problem".
Computational complexityprecludes the use of optimal control or Using this result we define P (x,v) as the minimum time to reach
dynamic
programming for real-time
steering
control.
The the goal subjecttothe acceleration limit (1). Thus, -VPg(x,v)
remainder of this section describes
computationally
a feasible
directstheacceleration as theoptimalcontrol if no obstacles are
algorithm for determining the acceleration based on the concept of
present.
satisficingstrategies Tather than optimality". The underlying
principleforthe proposedfeedbackalgorithm is t o choose the
control a t each instant based on the available local information so OBSTACLE POTENTIAL
as to guarantee the acceptability (but not necessarily the optimality) A potential function can he computed for every visible obstacle in
of the resulting global trajectory.
theenvironment2.In the presentformulation of the algorithm we
compute P,(x,v) onlyforvisibleobstaclesin thedirection of the
currentsystem velocty v. This reduces the computationalburden
THE FEEDBACK ALGORITHM^ while yielding an acceleration which avoids collisions with the local
obstacles. If an
obstacle is present the obstacle potential is
A t each control instant the acceleration u is chosen to maximize the computed as proposedbyKrogh2. In particular, Po(x,v) is defined
rate of decrease in a generalized potential function p(x,.), subject
as the inverse of the reserve avoidance time, rM - rm, wherethe
to the constraints (I), (2) and a further constraint imposed by the
minimum avoidance time T~ is the minimumtimeinwhich the

1666
velocity toward the obstacle can be brought to zero using maximum FUTURE RESEARCH
deceleration, andthe m a x i m u m avoidance l i m e rM is the Thispaperpresentsanddemonstratesthe basicelements foran
maximumtimein which the velocity towardthe obstaclecanbe integrated approach to path planning and dynamic steering control
broughtto zero underconstant deceleration withouthittingthe for autonomous vehicles. The concepts have been demonstrated by
obstacle.This gives simulationand
work is currentlyunderway
to
implement the
proposed approachto guidance andcontrol for anactualtest
P,(x,v)= vo(vo-2axo)-1
2 vehicle.

where -wo is the velocity toward the obstacle and zo is the distance There are several directions for future research, including:
to the obstacle barrier. We note that Po(x,v)grows to infinity as methods for determining the appropriate set of critical
the capability to avoid crossing the obstacle barrier goes to zero. points at the path planning level
REAL-TIME IMPLEMENTATION
representation of a priori informationatthesteering
The generalized potential field feedback algorithm described above control level for interpreting real time data
requires the
computation of the feasible
acceleration
which
minimizes (4). Thus, only the gradients of P(x,v) (6) with respect criteria for deciding when the path should be re-planned,
to v are required, andthis is computedanalytically forreal-time that is,whenthesteeringcontrolalgorithm should
implementation.The simple magnitudeconstraints (I), (2) permit request new points from the path-planner
an analytical solution of the minimization problem (e.g., (5)) which
meansthecontrol a t each instant iscomputabledirectly from incorporation of learning in the high-level feedback loop
sensor data giving the distances to visible obstacles.
ACKNOWLEDGEMENTS
One practical issue is the finite sampling time for processing sensor
data. To account for this delay, an additional constraint is imposed The authors thank Dai Feng for programming and generating the
on the acceleration a t each control instant. A sampled-data control graphics for the examples, and Barbara Stone forhelping prepare
is assumed with constant acceleration during the sampling interval. themanuscript.This research supportedinpartbytheNational
It is necessary to assure that when the new sensor data is available, Science FoundationundergrantECS-8404607andinpartby
the system will still have the capability to avoid collisions with the DARPA under contract number DACA76-85-C-0003.
obstacles. This is guaranteed by limitingthe accelerationin the References
direction of theobstacle so that the position and velocity a t the
next control instant will still be safe with respect to the obstacle. 1. C.E. Thorpe, “Path Relaxation: Path Planning for a Mobile
Robot”, American Association for Arti ficial Intelligence
Analyticalmethods for guaranteeingthe efficacy of this feedback Conference, Austin, Texas, Aug 1984.
algorithm have been proposed and are currently being extended to
2. B.H. Krogh,“A Generalized Potential Field Approach to
accommodate more realistic steering control problemsg. In the next Obstacle Avoidance Control”, Robotics International
sectionexamples are presented toillustratetheintegration of the Robotics Research Conference, Bethlehem, PA, August 1984.
feedback algorithmwith
the
path-planning algorithm of the
previous section. 3. G. Giratt, R. Chatila, M. Vaisset, “An Integrated Navigation
andMotionControlSystemforAutonomousMultisensory
EXAMPLES Mobile Robots”, 1stInternationalSymposiumofRobotic
As an example, consider the obstacle-strewn environment illustrated Research, Bretton Woods, N H , September 1983.
in Fig. 3 which includes a detailed path from x. to xg generated by
4. E. Koch, et al, “Simulation of Path Planning for a System
the Path Relaxation algorithm. Passing these points to the steering
with Vision and Map Updating”, 1985 IEEE International
control algorithm, the resulting dynamic trajectory is shown in Fig.
C o n f on RoboticsandAutomation, St. Louis, MO,Mar
4. Thedynamicsteeringcontrol, which is basedononly local
1985.
informationaboutthe
obstacle locations,
generates dynamic
a
trajectory to the goal which follows the general path received from 5. J.L. Crowley, “Navigation for an Intelligent Mobile Robot”,
the path-planner. I E E E Journal of Robotics and Automation, Vol. RA-1, No.
1, Mar 1985, pp. 31-41.
Onemotivation for introducing feedbackin thesteeringcontrol
6. N. Nilsson, Problem Solving
Methods in Artificial
loop is to reduce the required level of detail at the path planning
Intelligence, McGraw-Hill, 1971.
stage. In Fig.5amuch coarser set criticalpoints isspecified
along the desired path to the goal. Using these points as subgoals, 7. N. Hogan,
“Impedance
Control:
Approach
An to
the feedback algorithm again generates smooth
a dynamic Manipulation: Parts I-111”, A S M E J Dynamic Syst., Meas.,
trajectory to the goal, as shown in Fig. 6. Thus, the local feedback and Control, Vol. 107, March 1985, pp. 1-24.
algorithm produces an acceptable trajectory without having a
detailed path plan. 8. 0. Khatib, “Real-Time Obstacle Avoidance for Manipulators
and Mobile Robots”, 1 9 8 5I E E EC o n f . o n Roboticsand
Finally,Fig.7illustratestheability of the feedback algorithm to Automation, March 1985, pp. 500-505.
usereal-time informationnotavailabletothepath-planner.The 9. B.H. Krogh, “Guaranteed Steering Control”, 1985American
same set of critical points (Fig. 5) were used as subgoals, but as the Control Conference, Boston, June 1985.
systemmoved throughthe obstacles, three new obstacleswere
detectedwhich hadnot been takenintoaccountbythepath 10. B.H. Krogh
and T. Graettinger,
“Maneuverability
planner.
,shown
Fig.
in 7, the generalized potential field Constraints Supervisory
for Steering
Control”, 244th
algorithm successfullyguided the systemalong the desired path Conference o n Decision and Control, Fort Lauderdale, FL,
while avoiding collisions with the new obstacles. December 1985.

1667
11. H. Simon, TheSciences of the Artificial, MIT Press, Second
Edition, 1981.
12. D. Fengand B.H. Krogh, “Acceleration-ConstrainedTime- a priori map
OptimalControl in Three Dimensions”, IEEE Trans. on
Automatic Control, submitted for publication

ACtUJtwS Sysltm
Servo-Controllrrr Servo Srnrws Dynamics

Figure 1: Hierarchical allocation of functions for

Figure 2: Cross-section of cubic costfunctions

Figure 3: Detailed path plan from Path Relaxation algorithm Figure 4: Dynamic trajectory following detailed path from Fig. 3

1668
Figure 5: Sparse set of critical points along desired path. Figure 6: Dynamic trajectory using sparse set of critical
points from Fig. 5 as subgoals.

Figure 7: Dynamic steermg control for local obstacle avoidance

1669