Motion Planning in Urban Environments:Part I
Dave Ferguson Intel Rearch Pittsburgh Pittsburgh,PA dave.
Thomas M.Howard
Carnegie Mellon University
Pittsburgh,PA
u.edu
Maxim Likhachev
University of Pennsylvania
Philadelphia,PA
maximl@as.upenn.edu
Abstract—We prent the motion planning framework for an autonomous vehicle navigating through
urban environments. Such environments prent a number of motion planning chal-lenges,including ultra-reliability,high-speed operation,com-plex inter-vehicle interaction,parking in large unstructured lots,and constrained maneuvers.Our approach combines a model-predictive trajectory generation algorithm for computing dynamically-feasible actions with two higher-level planners for generating long range plans in both on-road and unstructured areas of the environment.In this Part I of a two-part paper, we describe the underlying trajectory generator and the on-road planning component of this system.We provide examples and results from“Boss”,an autonomous SUV that has driven itlf over3000kilometers and competed in,and won,the Urban Challenge.
I.I NTRODUCTION
Autonomous pasnger vehicles prent an incredible op-portunity for thefield of robotics and society at large.Such technology could drastically improve safety on roads,provide independence to millions of people unable to drive becau of age or ability,revolutionize the transportation industry,and reduce the danger associated with military convoy operations. However,developing robotic systems that are sophisticated enough and reliable enough to operate in everyday driving scenarios is tough.As a result,up until very recently,au-tonomous vehicle technology has been limited to either off-road,unstructured environments where complex interaction with other vehicles is
non-existent[1],[2],[3],[4],[5],[6],or very simple on-road maneuvers such as highway-bad lane following[7].
The Urban Challenge competition was designed to extend this technology as far as possible towards the goal of unre-stricted on-road driving.The event consisted of an autonomous vehicle race through an urban environment containing single and multi-lane roads,traffic circles and interctions,open areas and unpaved ctions,road blockages,and complex parking tasks.Successful vehicles had to travel roughly90 kilometers,all in the prence of other human-driven and autonomous vehicles,and all while abiding by speed limits and California driving rules.
This challenge required significant advances over the state of the art in autonomous vehicle technology.In this paper,we describe the motion planning system developed for Carnegie Mellon University’s winning entry into the Urban Challenge,“Boss”.This system enabled Boss to travel extremely quickly through the urban environment to complete its missions;inter-act safely and intelligently with obstacles and other vehicles on roads,at interctions,and in parking lots;and perform sophisticated maneuvers to solve complex parking tasks.
In Part I of the paper we introduce very briefly the software architecture ud by Boss and the role of
motion planning within that architecture.We then describe the trajectory gen-eration algorithm ud to generate every move of the vehicle. In Section V we discuss the motion planning framework ud when navigating on roads.
In Part II of the paper we discuss the framework ud when navigating through unstructured areas or performing complex maneuvers.We then provide results and discussion from hundreds of hours and thousands of kilometers of testing, and describe related work in both on-road and unstructured planning.
II.S YSTEM A RCHITECTURE
Boss’software system is decompod into four major blocks (e Figure1).The Perception component fus and process data from Boss’nsors to provide key environmental infor-mation,including:
•Vehicle State,globally-referenced position,attitude and speed for Boss;
•Road World Model,globally-referenced geometric infor-mation about the roads,parking zones,and interctions in the world;
•Moving Obstacle Set,an estimation of other vehicles in the vicinity of Boss;
•Static Obstacle Map,a2D grid reprentation of free, dangerous,and lethal space in the world;and
•Road Blockages,an estimation of clearly impassable road ctions.
The Mission Planning component computes the fastest route through the road network to reach the next checkpoint in the mission,bad on knowledge of road blockages,speed limits,and the nominal time required to make special maneu-vers such as lane changes or u-turns.
The Behavioral Executive combines the strategic global information provided by Mission Planning with local traffic and obstacle information provided by Perception and gen-erates a quence of local tasks for the Motion Planner.It is responsible for the system’s adherence to various rules of the road,especially tho concerning structured interactions
Fig.1.“Boss”:Tartan Racing’s winning entry in the Urban Challenge,along with its software system architecture.
with other traffic and road blockages,and for detection of and recovery from anomalous situations.The local tasks it feeds to the Motion Planner take the form of discrete motion goals,such as driving along a road lane to a specific point or maneuvering to a specific po or parking spot.The issuance of the goals is predicated on traffic concerns such as precedence among vehicles stopped at an interction.In the ca of driving along a road,desired lane and speed commands are given to the Motion Planner to implement behaviors such as distance keeping,passing maneuvers,and queueing in stop-and-go traffic.
槛外长江空自流怎么读The Motion Planning component takes the motion goal from the Behavioral Executive and generates and executes a trajectory that will safely drive Boss towards this goal, as described in the following ction.Two broad contexts for motion planning exist:on-road driving and unstructured driving.
III.M OTION P LANNING
The motion planning layer is responsible for executing the current motion goal issued from the Behavioral Executive. This goal may be a location within a road lane when per-forming nominal on-road driving,a location within a parking lot or obstaclefield when traversing through one of the areas,or any location in the environment when performing error recovery.
Figure2provides a basic illustration of the nature of the goals provided by the Behavioral Executive.During nominal on-road driving,the goal entails a desired lane and a desired position within that lane(typically a stop-line at the end of the lane).In such cas,the motion planner invokes a high-speed lane-bad planner to generate a path that tracks the desired lane.During unstructured driving,such as when navigating through parking lots,the goal consists of a desired po of the vehicle in the world.In the cas,the motion planner invokes a4D lattice planner that generates a global path to the desired po.The unstructured motion goals are also ud when the vehicle encounters an anomalous situation during on-road driving and needs to perform a complex maneuver (such as when an interction is partially blocked and cannot be traverd through in the desired lane).
As well as issuing motion goals,the Behavioral Exec-utive is constantly providing maximum speed and acceler-ation/deceleration commands to the motion planner.It
南美洲的地形特征is Fig.2.Motion goals provided by the Behavioral Executive to the Motion Planner.Also shown are the frequently updated speed and desired acceleration commands.
through this interface that the Behavioral Executive is able to control the vehicle’s forward progress in distance keeping and interction precedence scenarios.When the vehicle is not constrained by such scenarios,the motion planner computes desired speeds and accelerations bad on the constraints of the environment ad curvature and speed limits). Given a motion goal,the motion planner creates a path towards the desired goal then tracks this path by generating a t of candidate trajectories that follow the path to vary-ing degrees and lecting from this t the best trajectory according to an evaluation function.As mentioned above,the nature of the path generated differs bad on the context of the motion goal and the environment.In addition,the evaluation function differs depending on the context but always includes consideration of static and dynamic obstacles,curbs,speed, curvature,and deviation from the path.The lected trajectory is then directly executed by the vehicle.
IV.T RAJECTORY G ENERATION
Each candidate trajectory is computed using a model-predictive trajectory generator from[8]that produces dynami-cally feasible actions between initial and desired vehicle states. In general,this algorithm can be ud to solve the problem of generating a t of parameterized controls(u(p,x))that satisfy a t of state constraints who dynamics can be expresd by a t of differential equations:
x=
x yθκv...
T
(1)
˙x(x,p)=f(x,u(p,x)),(2) where x is the vehicle state(with position(x,y),heading (θ),curvature(κ),and velocity(v))and p is the t of parameters for which we are solving.The derivative of vehicle state˙x is a function of both the parameterized control input u(p,x)and the vehicle state x becau the vehicle’s respon to a particular control input is state dependent.In this ction we describe the application of this general algorithm to our domain,specifically addressing the state constraints,vehicle model,control parameterization,initialization function,and trajectory optimization approaches ud.
(a)(b)
Fig.3.Velocity and curvature profiles.(a)Several different linear velocity profiles were applied in this system,each with their own parameterization and application.Each parameterization contains some subt of velocity and acceleration knot points(v0,v t,v f,a0,and a f)and the length of the path,measured in time(t0,t f).(b)The curvature profile includes four possible degrees of freedom:the three spline knot points(κ0,κ1,andκ2)and the length of the path (s f).
A.State Constraints
For navigating both on-road and unstructured areas of urban
environments we generated trajectories that satisfied both
target two-dimensional position(x,y)and heading(θ)con-
straints.We defined the constraint equation formula(C(x,p))
as the difference between the target boundary state con-
straints(denoted x C)and the integral of the model dynamics
(the endpoint of the computed vehicle trajectory):
x C=
x C y CθC
T
(3)
x F(p,x)=x I+
t f
˙x(x,p)dt(4)
C(x,p)=x C−x F(p,x)(5)
The constrained trajectory generation algorithm determines
the control parameters p that drive Equation5to zero.This
results in a trajectory from an initial state x I to a terminal
vehicle state x F that is as clo as possible to the desired
terminal state x C.
6英镑
B.Vehicle Modeling
The development of a highfidelity vehicle dynamics model
is important for the accurate prediction of vehicle motion
and thus for the generation of accurate trajectories using our
constraint-bad approach.
Our vehicle model consists of a t of parameterized
functions that werefit to data extracted from human-driven
performance runs in the vehicle.The key parameters in
our model are the controller delay,the curvature limit(the
minimum turning radius),the curvature rate limit(a function
of the maximum speed at which the steering wheel can be
turned),and the maximum acceleration and deceleration of the
vehicle.The controller delay accurately predicts the difference
in time between a command from software and the corre-
sponding initial respon from hardware and is an important
consideration when navigating at high speeds.The curvature,
rate of curvature,acceleration and deceleration limits were
esntial for accurately predicting the respon of the vehicle
over entire trajectories.This model is then simulated using a
fixed timestep Euler integration to predict the vehicle’s motion.
C.Controls Parameterization
For Ackermann steered vehicles,it is advantageous to define
什么食物对肺好
the vehicle controls with a time-bad linear velocity function
鬼谷子全文及译文(v(p,t))and an arclength-bad curvature function(κ(p,s)):
u(p,x)=
v(p,t)κ(p,s)
T
(6)
We allow the linear velocity profile to take the form of
a constant profile,linear profile,linear ramp profile,or a
trapezoidal profile(e Figure3(a)).The motion planner
lects the appropriate profile bad on the driving mode
and aintaining a constant velocity for distance
keeping or slowing down for an upcoming interction).Each
of the profiles consists of a t of dependent parameters
(v0,v t,v f,a0,and a f)and the time to complete the profile
(t0,t f),all of which become members of the parameter t
p.Since all of the dependent profile parameters are typically
known,no optimization is done on the shape of each of the
profiles.
The curvature profile defines the shape of the trajectory and
is the primary profile over which optimization is performed.
Our profile consists of three independent curvature knot point
parameters(κ0,κ1,andκ2)and the trajectory length(s f)
(e Figure3(b)).In general,it is important to limit the
degrees of freedom in the system to minimize the prence of
local optima and to improve the runtime performance of the
algorithm(which is approximately linear with the number of
free parameters in the system)but maintain enoughflexibility
to satisfy all of the boundary state constraints.We cho a
cond order spline profile becau it contains enough degrees
of freedoms(4)to satisfy the boundary state constraints(3).
We furtherfix the initial command knot pointκ0during the
optimization process to the curvature at the initial state x I to
provide smooth controls1.
With the linear velocity profile’s dependent parameters
being fully defined and the initial spline parameter of the
curvature profilefixed,we are left with a system with three
parameterized freedoms:the latter two curvature spline knot
points and the trajectory length:
p free=
κ1κ2s f
T
(7)
官能心理学The duality of the trajectory length(s f)and time(t f)can be
resolved by estimating the time that it takes to drive the entire
1However,this can also befixed to a different value to produce sharp
trajectories,as described in Section V-B.
distance through the linear velocity profile.Arclength was ud for the independent parameter for the curvature profiles becau the shape of teh profiles is somewhat independent of the speed at which they are traveled.This allows solutions with similar parameters for varying linear velocity profiles.
D.Initialization Function
Given the three free parameters and the three constraints in our system,we can u various optimization or rootfinding techniques to solve for the parameter values that minimize our constraint equation.However,for efficiency it is beneficial to pre-compute offline an approximate mapping from relative state constraint space to parameter space to ed the constraint optimization process.This mapping can drastically speed up the algorithm by placing the initial guess of the control pa-rameters clo to the desired solution,reducing the number of online optimization steps required to reach the solution(within a desired precision).Given the high number of state variables and the non-integrable model dynamics,it is infeasible to pre-compute the entire mapping of state space to input space for any nontrivial system,such as the Boss vehicle model.We instead generate an approximation of this mapping through afive-dimensional lookup table with varying relative initial and terminal position( x, y),relative heading( θ),initial curvature(κi),and constant velocities(v).Becau thi
s is only an approximation some optimization is usually required, however the initial ed from the lookup table significantly reduces the number of optimization iterations required from an arbitrary t of parameters.
Figure4provides an illustration of the lookup table genera-tion process.Wefirst discretize ourfive dimensions of interest into a5D table.Next,we uniformly sample from the t of all possible parameter values and record which table position each of the sample trajectories terminates in.We then step through the5D table and for each position in the table we find the sample parameter values that came clost to this position.We take that parameter t and optimize it(using the optimization technique prented in the next ction)to accurately match the table position,and then store the resulting parameter t in the corresponding index of the5D table. E.Trajectory Optimization
Given a t of parameters p that provide an approximate solution,it is then necessary to optimize the parameters to reduce the endpoint error and‘snap’the corresponding trajectory to the desired terminal state.To do this,we linearize and invert our system of equations to produce a correction factor for the free control parameters bad on the product of the inverted Jacobian and the current boundary state error.The Jacobian is model-invariant since it is determined numerically through central differences of simulated vehicle actions.
∆p=−
δC(x,p)
δp
−1
C(x,p)(8)
The control parameters are modified until the residual of the boundary state constraints is within an acceptable
bound Fig.4.Offline lookup table generation.Top-left:Some sampled trajectories (in red)and some table endpoints(red arrows)that we wish to generate trajectories to for storage in the lookup table.Bottom-left:The clost sampled trajectories to the desired table endpoints are lected and then optimized to reach the desired endpoints.The parameter ts corresponding to the optimized trajectories are then stored in the lookup table.Right:The t of all sampled trajectories(red)and table endpoints(blue)for a single initial vehicle
state.
κ1(rad)
κ2(rad)
s f(m)
x(m)y(m)
x(m)y(m)
x(m)y(m) Fig.5.Online Trajectory Generation.Top-left:Given an initial state and desired terminal state(relative terminal state shown in green),wefind the clost elements of the lookup table(in red)and interpolate between the control parameters associated with the elements(interpolation of free parameters shown at bottom row)to come up with an initial approximation of the free parameters(resulting corresponding trajectory shown in blue).Top-right:This approximate trajectory is then optimized by modifying the free parameters bad on the endpoint error,resulting in a quence of trajectories that get clor to the desired terminal state.When the endpoint error is within an acceptable bound,the most recent parameter t is returned(trajectory shown in green).The interpola
tion over the free parametersκ1,κ2,and s f is shown by the three graphs on the bottom row(interpolated solutions shown in blue,final optimized solutions shown in green).
or until the optimization process diverges.In situations where boundary states are unachievable due to vehicle limitations, the optimization process predictably diverges as the partial derivatives in the Jacobian approach zero.The optimization history is then arched for the best candidate action(the action that gets clost to the state constraints)and this candidate is accepted or rejected bad on the magnitude of its error.
Figure5illustrates the online trajectory generation approach in action.Given a desired terminal state,wefirst lookup from our table the clost terminal and initial states and their associated free parameter ts.We then interpolate between the clost parameter ts in5D to produce our initial approximation of the parameter t to reach our desired terminal state.Figure5(top-left,bottom)shows the lookup and
Fig.6.Smooth and sharp
trajectories
(a)(b)(c)(d)
Fig.7.Following a road lane.The images show a single timeframe from the Urban Challenge.
怎样画公主interpolation steps,with the resulting parameter t values and
corresponding trajectory.Next,we evaluate the endpoint error
of the resulting trajectory and we u this error to modify our
parameter values to get clor to our desired terminal state,
using the optimization approach just described.We repeat this
optimization step until our endpoint error is within an allowed
bound of the desired state(e Figure5(top-right)),and the
resulting parameters and trajectory are stored and evaluated
by the motion planner.
V.O N-ROAD P LANNING
A.Path Extraction
During on-road navigation,the motion goal from the Behav-
ioral Executive is a location within a road lane.The motion
planner then attempts to generate a trajectory that moves the
vehicle towards this goal location in the desired lane.To do
this,itfirst constructs a curve along the centerline of the
desired lane,reprenting the nominal path that the center of
the vehicle should follow.This curve is then transformed into
a path in rear-axle coordinates to be tracked by the motion
planner.
B.Trajectory Generation
To robustly follow the desired lane and to avoid static and
dynamic obstacles,the motion planner generates trajectories
to a t of local goals derived from the centerline path.Each
of the trajectories originates from the predicted state that
the vehicle will reach by the time the trajectories will be
executed.To calculate this state,forwards-prediction using an
accurate vehicle model(the same model ud in the trajectory
generation pha)is performed using the trajectories lected
for execution in previous planning episodes.This forwards-
prediction accounts for both the high-level delays(the time
required to plan)and the low-level delays(the time required简笔画小汽车
to execute a command).
The goals are placed at afixed longitudinal distance down
the centerline path(bad on the speed of the vehicle)but vary
in lateral offt from the path to provide veral options for the
planner.The trajectory generation algorithm described above is
ud to compute dynamically feasible trajectories to the local
goals.For each goal,two trajectories are generated:a smooth
trajectory and a sharp trajectory.The smooth trajectory has
the initial curvature parameterκ0fixed to the curvature of the
forwards-predicted vehicle state.The sharp trajectory has this
parameter t to an offt value from the forwards-predicted
vehicle state curvature to produce a sharp initial action.The
sharp trajectories are uful for providing quick respons to
suddenly appearing obstacles or dangerous actions of other
vehicles.
Figure6provides an example of smooth and sharp trajec-
tories.The left-most image shows two trajectories(cyan and
purple)generated to the same goal po.The purple(smooth)
trajectory exhibits continuous curvature control throughout;
the cyan(sharp)trajectory begins with a discontinuous jump
in commanded curvature,resulting in a sharp respon from
the vehicle.In the images,the initial curvature of the vehicle
is shown by the short pink arc.The four center images show
the individual sharp and smooth trajectories,along with the
convolution of the vehicle along the trajectories.The right-
most image illustrates how the trajectories are generated in
practice for following a road lane.
C.Trajectory Velocity Profiles
The velocity profile ud for each of the generated tra-
jectories is lected from the t introduced in Section IV-
C bad on veral factors,including:the maximum speed
given from the Behavioral Executive bad on safe following
