A self-driving car must always have a plan for safely coming to a halt. Often, finding these safe plans is treated as an afterthought. In this paper, we demonstrate that techniques explicitly designed for safety can yield higher quality plans and lower latency than conventional planners in an urban driving setting. We adopt ideas from a previously-proposed safe online real-time heuristic search method to the spatiotemporal state lattices used when planning for autonomous driving. We experimentally compare our proof-of-concept implementation to conventional methods and find significantly improved performance while still maintaining passenger comfort and safety.
A central goal of artificial intelligence is the construction of autonomous systems. It is becoming more common that these systems interact closely with humans. Perhaps the most intimate way that humans can interact with an autonomous system is to climb inside it and put their lives in the hands of its control system. Interestingly, it is exactly such systems, in the form of self-driving automotive mobility systems, that are predicted to become widespread in the coming decades. It is crucial that the AI systems that decide on the actions of autonomous vehicles be designed with safety as a fundamental aspect.
Self-driving vehicle technology has many potential benefits for individuals, such as reduced collisions, improved mobility, and reclaiming time spent driving. They also have the potential for broad benefits to society and the environment, such as reduced car ownership, space devoted to parking, and the number of vehicles required. However, there are still significant problems to be surmounted to achieve this vision. For example, in addition to avoiding actual collisions, for this technology to be successfully adopted, individuals need to feel subjectively safe and comfortable while using an autonomous vehicle. In this paper, we address the problem of planning trajectories for autonomous vehicles, taking into account both objective safety, given the predicted trajectories of nearby vehicles and pedestrians, and subjective passenger comfort.
Background Trajectory planning for urban driving has proven to be a complex problem because of the inherently dynamic environment. A typical driver can encounter hundreds of other vehicles and many more pedestrians. Not only does the planner need to consider the spatial location of each part of this environment but their temporal evolution needs to be built into the planning techniques that are employed. These two aspects need to be intelligently combined to produce high quality and computationally feasible algorithms to address the urban driving problem.
Hierarchical methods are state-of-the-art for addressing
the difficulty of autonomous urban driving
Our work addresses urban driving at the level of motion
planning: we want to send trajectories to the vehicle
controller such that the motions are comfortable for passengers
while simultaneously ensuring that they are safe. We define
a safe trajectory as one that reaches a safe state. In urban
driving, a safe state is one in which the vehicle is stopped
The planner also needs to take into account the comfort of 1For highway driving, a safe state might be one where the car is pulled over at the side of the road. We do not address this in this paper. its passengers. Specific ranges of acceleration, when applied to the autonomous vehicle, create an uncomfortable riding experience. Allowing a high amount of acceleration to be chosen by the planner and executed on the vehicle leads to passenger discomfort due to the physical stress the human body undergoes when the vehicle quickly accelerates.
In this paper, we study a general online method for guaranteeing that the planner will find a series of safe actions for the agent to execute. We dynamically allow the agent to plan for its current goals and devise a way to balance the safety of the trajectories the agent is executing while being as close to the edge of non-safe action execution as possible. We define a general problem setting for the use of this technique and study the use of it in the domain of simple urban driving. We empirically test this method on a simulated vehicle with inertia and find that the new techniques dramatically outperform the conventional ones.
A state lattice
CL-RRT
scurrent :predecessor in P
10 BEST-FIRST SEARCH to find a safe trajectory T from
sroot to sgoal
11 return T
the RRT could be a highly sub-optimal plan
SafeRTS
We now turn to applying
This na¨ıve solution is inherently unsafe as it does not consider upcoming obstacles and dead-ends states beyond the explored spaces or partial goal. To ensure safety SafeTLP proves that a prefix of the na¨ıve trajectory is safe by constructing a safe trajectory starting with this prefix. The algorithm first attempts to prove that the last state of the trajectory is safe (line 4). If the safety proof is not successful, SafeTLP falls back to the preceding state on the trajectory(line 9). The last few states in the na¨ıve trajectory may be skipped if a maximum allowed deceleration would not allow the agent to come to a full stop from these states. Sroot a
SA b
SB D c Partial Goals
Fast State Safe Goal Fast State Safe Goal
As we attempt to prove safety, we store every node encountered in a special safety closed list. If reencountered during a subsequent attempt, such nodes are not expanded as we have already attempted to prove safety from them.
Similar to SafeRTS’s dsafe heuristic , SafeTLP prioritizes
states that are closer to the goal state. The safety proof
expands the search tree under the state scurrent
We use Fig. 1 to demonstrate the behavior of SafeTLP. SafeTLP first expands a search tree (blue on Fig. 1) from sroot that optimizes for velocity until it reaches a state in the partial goal set. In our example the sequence of a, b, and c trajectory segments represents a trajectory that leads to a partial goal state spartial goal . Then SafeTLP attempts to find a safe trajectory to sgoal from each intermediate state identified by the ha; b; ci trajectory starting from the state closest to the partial goal. First, it attempts to prove that spartial goal is safe, then it falls back to Sb, Sa, and lastly to sroot . These proofs are independent search trees marked with orange on Fig. 1. The proof searches are optimized on dsafe . In our example the safety proof from spartial goal and Sb fails and only succeeds from Sa. SafeTLP constructs a safe trajectory to sgoal by appending the prefix hai trajectory that leads to Sa with the trajectory segment sequence D.
Theorem 1 SafeTLP is guaranteed to find a safe plan if one exists in the state space.
Proof: If all safety proofs initiated from the states on the na¨ıve path (line 5 of Alg. 1) fail, SafeTLP will perform a best-first search from the agent’s current state (line 10). Because best-first search does not prune states from the search tree and the state space is finite, this search will eventually find paths from sroot to every other reachable state. Bestfirst search is exhaustive and complete in finite state spaces, thus eventually it will identify a path to a safe state if one exists. 2 y it c o l e v naive SafeTLP plan-to-stop distance (a) (b) Theorem 2 In the worst case, SafeTLP expands no more than twice as many states as the na¨ıve plan-to-stop method expands in the worst case.
Proof: Recall that, due to the safety closed list, any node will be expanded at most once by a safety proof attempt. If all the safety proofs fail, then SafeTLP will perform bestfirst search initiated from the agent’s current state. This exhaustive search will expand every state at most once, as the standard closed list of best-first search will prevent duplicate expansions. Thus any state will be expanded at most twice (once by a safety proof attempt and once by the exhaustive search). The na¨ıve method expands each node at most once. 2
In motivating our approach, we present three different approaches in Figure 2a. The abstract diagram shows the distance towards the goal along the horizontal axis and the velocity of the vehicle along the vertical axis. Ideally, we want an algorithm which allows the vehicle to travel as fast as possible towards the goal while guaranteeing that there is an alternate course of actions to travel to a safe state in the case of an emergency or unexpected situation.
The basic na¨ıve approach, shown in orange, performs a best-first search towards the goal. By optimizing distanceto-goal, this search tends to accelerate the vehicle until it reaches the maximum velocity and then maintain that speed. This technique is clearly not safe as it does not guarantee a way for the vehicle to reach a safe state in case of an emergency. However, it does fulfill the task of accelerating the vehicle to a very high speed. On the other hand, the current state-of-the-art is a more exhaustive approach, shown in green and labeled plan-to-stop. This performs an exhaustive breadth-first search expanding all the possible trajectories for the vehicle to undertake with the goal of completely stopping the vehicle at the end as shown in Fig. 2b. This approach is able to find a safe way of reaching the goal at the price of many node expansions and a slow velocity for the vehicle. To be able to plan to slow down at the current planning horizon, it needs to plan to have a lower average velocity than the na¨ıve approach would find. In essence, we have devised a hybrid approach that performs a best-first search to find a trajectory leading to a very high speed, and then performs a search on safety to find a series of safe actions leading us towards the goal safely represented by the blue line. In this fashion, we guarantee that even though we are comfortable acceleration comfortable deceleration aggressive deceleration 1.0 1.0 1.8 1.2 1.2 2.0 1.5 1.5 2.2 accelerating the vehicle very quickly towards the goal, there will be a series of actions the vehicle can take to reach a safe state in the event of an emergency or unexpected action. This purpose-designed hybrid algorithm combines the low number of expansions of the best-first search’s high-velocity actions with the guarantee of safety of the exhaustive search. Ultimately, this approach yields a high-quality solution that is both fast and guaranteed to be safe.
We demonstrate planning in simplified spatiotemporal
lattice. The goal is 100 meters away from the start position of
the vehicle. The road is discretized by half meters, so 200
states in total on the distance dimension of the lattice. From
each state, the vehicle can apply three actions: accelerate,
maintain the velocity, and decelerate. The following studies
Figure 3a shows the number of expanded node for the algorithms to find a safe trajectory. As we can see, SafeTLP expands many (about three magnitudes) fewer nodes than plan-to-stop. This is because plan-to-stop performs an exhaustive search of the state space while SafeTLP explicitly reasons about the optimal safe solution. Figure 3b shows the planning time. As expected, SafeTLP consumes much less time than plan-to-stop to find a safe plan. In a real application setting, fast planning enables the planner to run in a higher frequency, thereby enabling the vehicle to react to the 0.8 0.8 1.6 environment more quickly and thus be safer.
Figure 3c present violin plots showing the distribution of the average velocity for each test instances. As we can see, SafeTLP produces higher performance trajectories than plan-to-stop.
We have introduced a new and more effective method for safe action selection in spatiotemporal planning. Previous methods are extremely conservative in how they guarantee safety by expanding drastically more nodes than is required. Our planner can quickly generate comfortable and safe plans online. We demonstrate that planning time is drastically reduced while simultaneously being able to achieve a higher average velocity for the vehicle. In combining techniques from real-time heuristic search and spatiotemporal planning, we introduced a method for selecting safe and fast plans quickly. We hope this work encourages further development in the applicability of safe real-time and online search.