Patent Description:
Autonomous vehicles for instance, vehicles that may not require a human driver, can be used to aid in the transport of passengers or items from one location to another. Such vehicles may operate in a fully autonomous mode where passengers may provide some initial input, such as a pickup or destination location, and the autonomous vehicle maneuvers itself to that location. Autonomous vehicles are equipped with various types of sensors in order to detect objects in the surroundings. For example, autonomous vehicles may include sonar, radar, camera, lidar, and other devices that scan, generate and/or record data about the vehicle's surroundings in order to enable the autonomous vehicle to plan trajectories in order to maneuver itself through the surroundings.

In some instances, a planning system of an autonomous vehicle may generate a trajectory and send it to another computing system in order to control the autonomous vehicle according to that trajectory. The trajectory may include a first portion that allows the autonomous vehicle to proceed towards its end goal or destination, and thereafter, the trajectory may include a fallback portion. This fallback portion may include instructions for the autonomous vehicle to safely pull over, stop, etc. such that if a new trajectory is not received in time, the autonomous vehicle can safely pull over. The policy planning features described herein may enable the fallback portion of a trajectory to influence the nominal portion of the trajectory, thus improving the likelihood of the fallback portion causing the autonomous vehicle to stop in the best possible circumstances.

In some instances, a planning system of an autonomous vehicle may plan two trajectories and combine portions of them together in order to control the autonomous vehicle towards a destination. For example, the planning system may plan a nominal trajectory or a long term (e.g., <NUM>, <NUM>, <NUM> second or more or less) plan to maneuver the autonomous vehicle towards its destination. However, rather than publishing this trajectory, the planning system may also generate a second or fallback trajectory to be published to other systems of the autonomous vehicle. When planning a fallback trajectory, the planning system may extract or match an initial section of the nominal trajectory, such as <NUM> milliseconds or more or less, and use this as the first portion of the fallback trajectory.

A second or fallback portion of the fallback trajectory may then be determined independently of or based on the nominal trajectory. The fallback trajectory may then be published to other systems of the autonomous vehicle and used to control the autonomous vehicle. In this regard, the autonomous vehicle's computing devices are always following a fallback trajectory, though under normal operation do not execute the entire fallback trajectory. Under normal operation, because the trajectory used by the autonomous vehicle's computing devices is updated before the autonomous vehicle reaches the fallback portion (e.g., the <NUM> milliseconds section where the fallback trajectory diverges from the nominal trajectory), the autonomous vehicle only follows the nominal trajectory. However, if a new trajectory is not published by the time the fallback portion is reached or some point before this, the autonomous vehicle's computing devices may simply continue to follow the fallback portion of the fallback trajectory and bring the autonomous vehicle to a stop.

The fallback portion may be planned in different ways. For example, in some approaches, the fallback portion may simply follow the geometry of the nominal trajectory, but change the speed profile of the nominal trajectory in order to bring the autonomous vehicle to a stop. In this regard, the fallback portion may be influenced by the nominal trajectory rather than the other way around. Reference is made to <CIT>, which presents fall back trajectory systems for autonomous vehicles. Further reference is made to <CIT>, which presents systems for implementing fallback behaviors for autonomous vehicles. Further reference is made to <CIT>, which presents a vehicle controller, vehicle controlling method and computer program therefor. Further reference is made to <CIT>, which presents methods and systems for vehicle path planning.

The invention provides a method for controlling an autonomous vehicle according to claim <NUM>. The method includes identifying, by one or more processors, a set of potential stopping locations based on a current location of an autonomous vehicle; identifying, by the one or more processors, a set of potential states for the autonomous vehicle; determining, by the one or more processors, a fallback cost for each potential state of the set of potential states to reach each potential stopping location of the set of potential stopping locations; planning, by the one or more processors, a trajectory for an autonomous vehicle based on a route to a destination and the determined fallback costs; and controlling, by the one or more processors, the autonomous vehicle according to at least a portion of the trajectory.

In one example, the set of potential states is identified based on the current location and a planning horizon for the trajectory. In this example, the planning horizon is measured in time. In another example, the determined fallback costs are determined using a backward induction approach. In this example, the backward induction approach provides a point in time for each potential state of the set of potential states. In another example, each potential state of the set of potential states includes a location. In this example, each potential state of the set of potential states further includes a combination of characteristics. In another example, a first potential state of the set of potential states includes a first location and a first future point in time, and a second potential state of the set of potential states includes the first location and a second future point in time different from the first future point in time. In another example, a first potential state of the set of potential states includes a first location and a first plurality of characteristics, and a second potential state of the set of potential states includes the first location and a second plurality of characteristics different from the first plurality of characteristics. In this example, the first plurality of characteristics includes a speed of the autonomous vehicle different from a speed of the autonomous vehicle of the second plurality of characteristics. In addition, or alternatively, the first plurality of characteristics includes an acceleration or deceleration of the autonomous vehicle different from acceleration or deceleration of the autonomous vehicle of the second plurality of characteristics. In addition, or alternatively, the first plurality of characteristics includes a rate of change of acceleration or deceleration of the autonomous vehicle different from a rate of change of acceleration or deceleration of the autonomous vehicle for the second plurality of characteristics. In addition, or alternatively, the first plurality of characteristics includes a heading of the autonomous vehicle different from a heading of the autonomous vehicle of the second plurality of characteristics. In addition, or alternatively, first plurality of characteristics includes an orientation of the autonomous vehicle different from an orientation of the autonomous vehicle of the second plurality of characteristics. In another example, determining the determined fallback costs results in a cost map, and the method further comprises planning a fallback trajectory based on at least the cost map.

The invention further provides a system for controlling an autonomous vehicle according to claim <NUM>. The system includes one or more processors configured to identify a set of potential stopping locations based on a current location of an autonomous vehicle; identify a set of potential states for the autonomous vehicle; determine a fallback cost for each potential state of the set of potential states to reach each potential stopping location of the set of potential stopping locations; plan a trajectory for an autonomous vehicle based on a route to a destination and the determined fallback costs; and control the autonomous vehicle according to at least a portion of the trajectory.

In one example, the set of potential states is identified based on the current location and a planning horizon for the trajectory. In another example, the determined fallback costs are determined using a backward induction approach. In this example, the backward induction approach provides a point in time for each potential state of the set of potential states. In another example, each potential state of the set of potential states includes a location.

The technology relates to planning trajectories for controlling autonomous vehicles. For instance, a planning system of an autonomous vehicle may generate a trajectory and send it to another computing system in order to control the autonomous vehicle according to that trajectory. The trajectory may include a first portion that allows the autonomous vehicle to proceed towards its end goal or destination, and thereafter, the trajectory may include a fallback portion. This fallback portion may include instructions for the autonomous vehicle to safely pull over, stop, etc. such that if a new trajectory is not received in time, the autonomous vehicle can safely pull over. The policy planning features described herein may enable the nominal portion of the trajectory to take into account potential fallback considerations, thus improving the likelihood of the fallback portion causing the autonomous vehicle to stop in the best possible circumstances.

For instance, as described above, in some of today's autonomous vehicles, a planning system of an autonomous vehicle may plan two trajectories; a nominal trajectory and a fallback trajectory. For example, the planning system may plan a nominal trajectory or a long term (e.g., <NUM>, <NUM>, <NUM> second or more or less) plan to maneuver the autonomous vehicle towards its destination. However, rather than publishing this trajectory, the planning system may also generate a second or fallback trajectory. When planning a fallback trajectory, the planning system may extract or match an initial section of the nominal trajectory, such as <NUM> milliseconds or more or less, and use this as the first portion of the fallback trajectory.

The fallback portion may be planned in different ways. For example, in some approaches, the fallback portion may simply follow the geometry of the nominal trajectory, but change the speed profile of the nominal trajectory in order to bring the autonomous vehicle to a stop. In this regard, the fallback portion may be influenced by the nominal trajectory rather than the other way around. To address this, the fallback trajectory may be planned independently of the nominal trajectory.

In order to plan a nominal trajectory, the autonomous vehicle's computing devices may first identify a set of potential stopping locations. These potential stopping locations may be located within a predetermined distance from the autonomous vehicle (in time or space). For example, if an autonomous vehicle's trajectory planning involves generating trajectories that extend a period of <NUM> seconds into the future, the autonomous vehicle's computing devices may identify potential stopping locations that the autonomous vehicle can potentially reach within the next <NUM> seconds based on one or more of the autonomous vehicle's current speed, achievable speed within that period, speed limit of the road on which the autonomous vehicle is driving, etc. Potential stopping locations may include simply stopping at different locations (e.g., nodes in a roadgraph of map information) in a current lane, adjacent lane, a shoulder area, etc. Such potential stopping locations may be identified based on a number of factors including those that are within a visible field of view, road geometry, size of the autonomous vehicle, capabilities of the autonomous vehicle (e.g., maneuverability of the autonomous vehicle), and so on.

For each identified stopping location of the set of potential stopping locations, the planning system may determine a cost map. This cost map may include assessing fallback costs for a plurality of different potential states of the autonomous vehicle. For instance, a set of potential states may be identified. For example, a fallback cost may correspond to the cost of reaching a potential stopping location from the potential state. The higher the fallback cost, the less desirable that potential state.

The potential states may include both a plurality of potential characteristics of the autonomous vehicle as well as locations that the autonomous vehicle can potentially reach within a planning horizon for the nominal trajectory based on one or more of the autonomous vehicle's current speed, achievable speed within that period, speed limit of the road on which the autonomous vehicle is driving, etc. The plurality of characteristics for each potential state may include a combination of different characteristics, such as heading, orientation, speed, acceleration or deceleration, rate of change of acceleration or deceleration, time, and so on.

Starting at the potential stopping location, the planning system may use a backward induction approach to plan a trajectory from each of the potential stopping locations to each of the potential states of the set of potential states. The backward induction may start from each of the potential stopping locations and use the map information in order to compute the fallback cost of reaching the potential stopping location from any of the potential states of the set of potential states. The result is a cost map that the planning system can greedily follow to get a fallback trajectory that leads to the best of the potential stopping locations from any of the potential states. In this regard, the cost map may encode a minimum fallback cost that can be achieved to reach a stopping location starting from each potential state.

The cost map may then be input into the planning system in order to plan or generate nominal trajectory. In the instances where a plurality of nominal trajectories is generated, the overall costs of all nominal trajectories may then be used to select a nominal trajectory using the costs of the cost map. A final trajectory may then be generated by using a first portion of the selected nominal trajectory and a second portion of a fallback trajectory generated based on the first portion of the selected nominal trajectory. The final trajectory may then be published to other systems of the autonomous vehicle and used to control the autonomous vehicle.

The features described herein may enable an autonomous vehicle to plan a nominal trajectory which is influenced by the costs of potential fallbacks. This, in turn, may enable the autonomous vehicle to improve its overall performance in the event of failure which requires the autonomous vehicle to stop and/or pull over.

As shown in <FIG>, an autonomous vehicle <NUM> in accordance with one aspect of the disclosure includes various components. Vehicles, such as those described herein, may be configured to operate in one or more different driving modes. For instance, in a manual driving mode, a driver may directly control acceleration, deceleration, and steering via inputs such as an accelerator pedal, a brake pedal, a steering wheel, etc. An autonomous vehicle may also operate in one or more autonomous driving modes including, for example, a semi or partially autonomous driving mode in which a person exercises some amount of direct or remote control over driving operations, or a fully autonomous driving mode in which the vehicle handles the driving operations without direct or remote control by a person. These vehicles may be known by different names including, for example, autonomously driven vehicles, self-driving vehicles, and so on.

National Highway Traffic Safety Administration (NHTSA) and the Society of Automotive Engineers (SAE) have each identified different levels to indicate how much, or how little, a vehicle controls the driving, although different organizations may categorize the levels differently. Moreover, such classifications may change (e.g., be updated) overtime.

As described herein, in a semi or partially autonomous driving mode, even though the vehicle assists with one or more driving operations (e.g., steering, braking and/or accelerating to perform lane centering, adaptive cruise control or emergency braking), the human driver is expected to be situationally aware of the vehicle's surroundings and supervise the assisted driving operations. Here, even though the vehicle may perform all driving tasks in certain situations, the human driver is expected to be responsible for taking control as needed.

In contrast, in a fully autonomous driving mode, the control system of the vehicle performs all driving tasks and monitors the driving environment. This may be limited to certain situations such as operating in a particular service region or under certain time or environmental restrictions, or may encompass driving under all conditions without limitation. In a fully autonomous driving mode, a person is not expected to take over control of any driving operation.

Unless indicated otherwise, the architectures, components, systems and methods described herein can function in a semi or partially autonomous driving mode, or a fully-autonomous driving mode.

While certain aspects of the disclosure are particularly useful in connection with specific types of vehicles, the vehicle may be any type of vehicle including, but not limited to, cars, trucks (e.g., garbage trucks, tractor-trailers, pickup trucks, etc.), motorcycles, buses, recreational vehicles, street cleaning or sweeping vehicles, etc. The vehicle may have one or more computing devices, such as computing device <NUM> containing one or more processors <NUM>, memory <NUM> and other components typically present in general purpose computing devices.

The memory <NUM> stores information accessible by the one or more processors <NUM>, including data <NUM> and instructions <NUM> that may be executed or otherwise used by the processor <NUM>. The memory <NUM> may be of any type capable of storing information accessible by the processor, including a computing device or computer-readable medium, or other medium that stores data that may be read with the aid of an electronic device, such as a hard-drive, memory card, ROM, RAM, DVD or other optical disks, as well as other write-capable and read-only memories.

The one or more processors <NUM> may be any conventional processors, such as commercially available CPUs or GPUs. Alternatively, the one or more processors may include a dedicated device such as an ASIC or other hardware-based processor. Although <FIG> functionally illustrates the processor, memory, and other elements of computing device <NUM> as being within the same block, it will be understood by those of ordinary skill in the art that the processor, computing device, or memory may actually include multiple processors, computing devices, or memories that may or may not be stored within the same physical housing. For example, memory may be a hard drive or other storage media located in a housing different from that of computing device <NUM>. Accordingly, references to a processor or computing device will be understood to include references to a collection of processors or computing devices or memories that may or may not operate in parallel.

Computing devices <NUM> may include all of the components normally used in connection with a computing device such as the processor and memory described above as well as a user input <NUM> (e.g., one or more of a button, mouse, keyboard, touch screen and/or microphone), various electronic displays (e.g., a monitor having a screen or any other electrical device that is operable to display information), and speakers <NUM> to provide information to a passenger of the autonomous vehicle <NUM> or others as needed. For example, internal display <NUM> may be located within a cabin of autonomous vehicle <NUM> and may be used by computing devices <NUM> to provide information to passengers within the autonomous vehicle <NUM>.

Computing devices <NUM> may also include one or more wireless network connections <NUM> to facilitate communication with other computing devices, such as other client computing devices and server computing devices. The wireless network connections may include short range communication protocols such as Bluetooth, Bluetooth low energy (LE), cellular connections, as well as various configurations and protocols including the Internet, World Wide Web, intranets, virtual private networks, wide area networks, local networks, private networks using communication protocols proprietary to one or more companies, Ethernet, WiFi and HTTP, and various combinations of the foregoing.

Computing devices <NUM> may be part of an autonomous control system for the autonomous vehicle <NUM> and may be capable of communicating with various components of the vehicle in order to control the vehicle in an autonomous driving mode. For example, returning to <FIG>, computing devices <NUM> may be in communication with various systems of autonomous vehicle <NUM>, such as deceleration system <NUM>, acceleration system <NUM>, steering system <NUM>, signaling system <NUM>, planning system <NUM>, routing system <NUM>, positioning system <NUM>, perception system <NUM>, behavior modeling system <NUM>, and power system <NUM> in order to control the movement, speed, etc. of autonomous vehicle <NUM> in accordance with the instructions <NUM> of memory <NUM> in the autonomous driving mode.

As an example, computing devices <NUM> may interact with deceleration system <NUM> and acceleration system <NUM> in order to control the speed of the vehicle. Similarly, steering system <NUM> may be used by computing devices <NUM> in order to control the direction of autonomous vehicle <NUM>. For example, if autonomous vehicle <NUM> is configured for use on a road, such as a car or truck, steering system <NUM> may include components to control the angle of wheels to turn the vehicle. Computing devices <NUM> may also use the signaling system <NUM> in order to signal the vehicle's intent to other drivers or vehicles, for example, by lighting turn signals or brake lights when needed.

Routing system <NUM> may be used by computing devices <NUM> in order to generate a route to a destination location using map information. Planning system <NUM> may be used by computing device <NUM> in order to generate short-term trajectories that allow the vehicle to follow routes generated by the routing system. In this regard, the planning system <NUM> and/or routing system <NUM> may store detailed map information, e.g., pre-stored, highly detailed maps identifying a road network including the shape and elevation of roadways, lane lines, intersections, crosswalks, speed limits, traffic signals, buildings, signs, real time traffic information (updated as received from a remote computing device), parking spots, vegetation, or other such objects and information.

<FIG> and <FIG> is an example of map information <NUM> for a small section of roadway. <FIG> depicts a portion of the map information <NUM> that includes information identifying the shape, location, and other characteristics of lane markers or lane lines <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> which define the boundaries of lanes <NUM>, <NUM>, <NUM>, <NUM>, as well as shoulder areas <NUM>, <NUM>. In this regard, some areas which may not necessarily be lanes (for example shoulder areas) may be identified as drivable areas (for example, lanes). In this example, a median <NUM> divides lanes <NUM>, <NUM> from lanes <NUM>,<NUM>. Although not shown in this example, the map information also includes other features such as intersection, traffic control devices, crosswalk areas, sidewalks, and so on. In addition to the aforementioned features and information, the map information may also include information that identifies the direction of traffic for each lane (represented by arrows <NUM>, <NUM>, <NUM>, <NUM> in <FIG>) as well as information that allows the computing devices <NUM> to determine whether the vehicle has the right of way to complete a particular maneuver (for example, complete a turn or cross a lane of traffic or intersection).

The map information may be configured as a roadgraph. The roadgraph may include a plurality of graph nodes and edges representing features such as crosswalks, traffic lights, road signs, road or lane segments, etc., that together make up the road network of the map information. Each edge is defined by a starting graph node having a specific geographic location (e.g., latitude, longitude, altitude, etc.), an ending graph node having a specific geographic location (e.g., latitude, longitude, altitude, etc.), and a direction. This direction may refer to a direction the autonomous vehicle <NUM> must be moving in in order to follow the edge (i.e., a direction of traffic flow). The graph nodes may be located at fixed or variable distances. For instance, the spacing of the graph nodes may range from a few centimeters to a few meters and may correspond to the speed limit of a road on which the graph node is located. In this regard, greater speeds may correspond to greater distances between graph nodes. The edges may represent driving along the same driving lane or changing driving lanes. Each node and edge may have a unique identifier, such as a latitude and longitude location of the node or starting and ending locations or nodes of an edge. In addition to nodes and edges, the map may identify additional information such as types of maneuvers required at different edges as well as which lanes are drivable.

<FIG> includes a few example representations (not all) of the aforementioned nodes (represented by open circles) corresponding to a portion of the map information <NUM>. These nodes, when linked together via edges (represented by arrows between the nodes), may form paths within lanes that the autonomous vehicle can follow to correctly position itself within a lane and/or through an intersection. These paths may be formed by following edges between nodes in the aforementioned roadgraph. As an example, an edge may correspond to a segment of drivable road surface between two nodes in the map information. Thus, each mapped drivable road surface (e.g., lanes, shoulder areas, parking lots, etc.) may be associated with nodes and edges in the roadgraph. Moreover, the nodes and edges are depicted herein as being relatively large for ease of representation. However, each node may actually correspond to a point (e.g., coordinates such as latitude, longitude, altitude), and the edges in the roadgraph may range from a few centimeters to a few meters as noted above. Each of these edges has an overlapping starting and/or end node with adjacent edges depending upon the direction of the lane to which the edge corresponds.

The routing system <NUM> may use the aforementioned map information to determine a route from a current location (e.g., a location of a current node) to a destination location. Routes may be generated using a cost-based analysis which attempts to select a route to the destination location with the lowest cost. Costs may be assessed in any number of ways such as time to the destination location, distance traveled (each edge may be associated with a cost to traverse that edge), types of maneuvers required, convenience to passengers or the vehicle, etc. For example, each individual edge in the roadgraph may be associated with a cost. These costs may be summed together with or without additional costs (e.g., additional costs for maneuvers or convenience, etc.) in order to determine the overall cost of a particular route. When multiple routes are generated, the route with the lowest overall cost may be selected by the routing system and published to the various other systems of the autonomous vehicle. For example, between a route with a large number of intersections with traffic controls (such as stop signs or traffic signal lights) versus one with no or very few traffic controls, the latter route may have a lower cost (e.g., because it is faster) and therefore be preferable. Each route may include a list of a plurality of nodes and edges which the vehicle can use to reach the destination location. Routes may be recomputed periodically as the vehicle travels to the destination location.

The map information used for routing may be the same or a different map as that used for planning trajectories. For example, the map information used for planning routes not only requires information on individual driving lanes, but also the nature of driving and bicycle lane boundaries (e.g., solid white, dash white, solid yellow, etc.) to determine where lane changes are allowed. However, unlike the map used for planning trajectories, the map information used for routing need not include other details such as the locations of crosswalks, traffic lights, stop signs, etc., though some of this information may be useful for routing purposes.

Positioning system <NUM> may be used by computing devices <NUM> in order to determine the vehicle's relative or absolute position on a map or on the earth. For example, the positioning system <NUM> may include a GPS receiver to determine the device's latitude, longitude and/or altitude position. Other location systems such as laser-based localization systems, inertial-aided GPS, or camera-based localization may also be used to identify the location of the vehicle. The location of the vehicle may include an absolute geographical location, such as latitude, longitude, and altitude, a location of a node or edge of the roadgraph as well as relative location information, such as location relative to other cars immediately around it, which can often be determined with less noise than the absolute geographical location.

The positioning system <NUM> may also include other devices in communication with computing devices <NUM>, such as an accelerometer, gyroscope or another direction/speed detection device to determine the direction and speed of the vehicle or changes thereto. By way of example only, an acceleration device may determine its pitch, yaw or roll (or changes thereto) relative to the direction of gravity or a plane perpendicular thereto. The device may also track increases or decreases in speed and the direction of such changes. The device's provision of location and orientation data as set forth herein may be provided automatically to the computing device <NUM>, other computing devices and combinations of the foregoing.

The perception system <NUM> also includes one or more components for detecting objects external to the vehicle such as other road users (vehicles, pedestrians, bicyclists, etc.) obstacles in the roadway, traffic signals, signs, trees, buildings, etc. For example, the perception system <NUM> may include Lidars, sonar, radar, cameras, microphones and/or any other detection devices that generate and/or record data which may be processed by the computing devices of computing devices <NUM>. In the case where the vehicle is a passenger vehicle such as a minivan or car, the vehicle may include Lidar, cameras, and/or other sensors mounted on or near the roof, fenders, bumpers or other convenient locations.

For instance, <FIG> are an example external views of autonomous vehicle <NUM>. In this example, roof-top housing <NUM> and upper housing <NUM> may include a Lidar sensor as well as various cameras and radar units. Upper housing <NUM> may include any number of different shapes, such as domes, cylinders, "cake-top" shapes, etc. In addition, housing <NUM>, <NUM> (shown in <FIG>) located at the front and rear ends of autonomous vehicle <NUM> and housings <NUM>, <NUM> on the driver's and passenger's sides of the vehicle may each store a Lidar sensor and, in some instances, one or more cameras. For example, housing <NUM> is located in front of driver door <NUM>. Autonomous vehicle <NUM> also includes a housing <NUM> for radar units and/or cameras located on the driver's side of the autonomous vehicle <NUM> proximate to the rear fender and rear bumper of autonomous vehicle <NUM>. Another corresponding housing (not shown may also be arranged at the corresponding location on the passenger's side of the autonomous vehicle <NUM>. Additional radar units and cameras (not shown) may be located at the front and rear ends of autonomous vehicle <NUM> and/or on other positions along the roof or roof-top housing <NUM>.

Computing devices <NUM> may be capable of communicating with various components of the vehicle in order to control the movement of autonomous vehicle <NUM> according to primary vehicle control code of memory of computing devices <NUM>. For example, returning to <FIG>, computing devices <NUM> may include various computing devices in communication with various systems of autonomous vehicle <NUM>, such as deceleration system <NUM>, acceleration system <NUM>, steering system <NUM>, signaling system <NUM>, planning system <NUM>, routing system <NUM>, positioning system <NUM>, perception system <NUM>, behavior modeling system <NUM>, and power system <NUM> (i.e. the vehicle's engine or motor) in order to control the movement, speed, etc. of autonomous vehicle <NUM> in accordance with the instructions <NUM> of memory <NUM>.

The various systems of the vehicle may function using autonomous vehicle control software in order to determine how to control the vehicle. As an example, a perception system software module of the perception system <NUM> may use sensor data generated by one or more sensors of an autonomous vehicle, such as cameras, Lidar sensors, radar units, sonar units, etc., to detect and identify objects and their characteristics. These characteristics may include location, type, heading, orientation, speed, acceleration, change in acceleration, size, shape, etc..

In some instances, characteristics may be input into a behavior prediction system software module of the behavior modeling system <NUM> which uses various behavior models based on object type to output one or more behavior predictions or predicted trajectories for a detected object to follow into the future (e.g., future behavior predictions or predicted future trajectories). In this regard, different models may be used for different types of objects, such as pedestrians, bicyclists, vehicles, etc. The behavior predictions or predicted trajectories may be a list of positions and orientations or headings (e.g., poses) as well as other predicted characteristics such as speed, acceleration or deceleration, rate of change of acceleration or deceleration, etc..

In other instances, the characteristics from the perception system <NUM> may be put into one or more detection system software modules, such as a traffic light detection system software module configured to detect the states of known traffic signals, construction zone detection system software module configured to detect construction zones from sensor data generated by the one or more sensors of the vehicle as well as an emergency vehicle detection system configured to detect emergency vehicles from sensor data generated by sensors of the vehicle. Each of these detection system software modules may use various models to output a likelihood of a construction zone or an object being an emergency vehicle.

Detected objects, predicted trajectories, various likelihoods from detection system software modules, the map information identifying the vehicle's environment, position information from the positioning system <NUM> identifying the location and orientation of the vehicle, a destination location or node for the vehicle as well as feedback from various other systems of the vehicle may be input into a planning system software module of the planning system <NUM>. The planning system <NUM> may use this input to generate planned trajectories for the vehicle to follow for some brief period of time into the future based on a route generated by a routing module of the routing system <NUM>.

A similar process as described above with regard to assessing costs of routes may be used for computing costs of and selecting trajectories by the planning system <NUM>. In other words, during a planning iteration, the planning system may generate a plurality of potential trajectories. The costs of each edge of a potential trajectory may be summed together with or without additional costs (e.g., additional costs for maneuvers or convenience, getting too close to other objects, etc.) in order to determine the overall cost of the potential trajectory. The lowest cost potential trajectory may then be selected by the planning system as the next trajectory for the autonomous vehicle to follow.

Each planned trajectory may provide a planned path and other instructions for an autonomous vehicle to follow for some brief period of time into the future, such as <NUM> seconds or more or less. In this regard, the trajectories may define the specific characteristics of acceleration, deceleration, speed, direction, etc. to allow the vehicle to follow the route towards reaching a destination location. A control system software module of computing devices <NUM> may be configured to control movement of the vehicle, for instance by controlling braking, acceleration and steering of the vehicle, in order to follow a trajectory.

The computing devices <NUM> may control the vehicle in one or more of the autonomous driving modes by controlling various components. For instance, by way of example, computing devices <NUM> may navigate the vehicle to a destination location completely autonomously using data from the detailed map information and planning system <NUM>. Computing devices <NUM> may use the positioning system <NUM> to determine the vehicle's location and perception system <NUM> to detect and respond to objects when needed to reach the location safely. Again, in order to do so, computing device <NUM> and/or planning system <NUM> may generate trajectories and cause the vehicle to follow these trajectories, for instance, by causing the vehicle to accelerate (e.g., by supplying fuel or other energy to the engine or power system <NUM> by acceleration system <NUM>), decelerate (e.g., by decreasing the fuel supplied to the engine or power system <NUM>, changing gears, and/or by applying brakes by deceleration system <NUM>), change direction (e.g., by turning the front or rear wheels of autonomous vehicle <NUM> by steering system <NUM>), and signal such changes (e.g., by lighting turn signals) using the signaling system <NUM>. Thus, the acceleration system <NUM> and deceleration system <NUM> may be a part of a drivetrain that includes various components between an engine of the vehicle and the wheels of the vehicle. Again, by controlling these systems, computing devices <NUM> may also control the drivetrain of the vehicle in order to maneuver the vehicle autonomously.

<FIG> provides an example flow diagram <NUM> for controlling an autonomous vehicle, which may be performed by one or more processors, such as the one or more processors of the computing devices <NUM> and/or the planning system <NUM>. As shown in block <NUM>, based on a current location of an autonomous vehicle, a set of potential stopping location is identified. <FIG> represents a geographic area <NUM> corresponding to the area of the map information <NUM>. In this example, the shape, location, and other characteristics of lane markers or lane lines <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> correspond to the shape, location, and other characteristics of lane markers or lane lines <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>. In this regard, the shape, location, and other characteristics of lanes <NUM>, <NUM>, <NUM>, <NUM>, shoulder areas <NUM>, <NUM>, and median <NUM> correspond to the shape, location, and other characteristics of lanes <NUM>, <NUM>, <NUM>, <NUM>, shoulder areas <NUM>, <NUM>, and median <NUM>, respectively. In this example, autonomous vehicle <NUM> is driving in lane <NUM> (again, corresponding to lane <NUM>), and another vehicle <NUM> is parked in the shoulder area <NUM>.

In order to plan or generate a nominal trajectory, the autonomous vehicle's computing devices may first identify a set of potential stopping locations. These potential stopping locations may be located within a predetermined distance from the autonomous vehicle (in time or space). For example, if an autonomous vehicle's trajectory planning involves generating trajectories that extend a period of <NUM> seconds into the future, the autonomous vehicle's computing devices may identify potential stopping locations that the autonomous vehicle can potentially reach within the next <NUM> seconds based on one or more of the autonomous vehicle's current speed, achievable speed within that period, speed limit of the road on which the autonomous vehicle is driving, etc. Potential stopping locations may include simply stopping at different locations (e.g., nodes in a roadgraph of map information) in a current lane, adjacent lane, a shoulder area, etc. Such potential stopping locations may be identified based on a number of factors including those that are within a visible field of view, road geometry, size of the autonomous vehicle, characteristics of the autonomous vehicle (e.g., maneuverability of the autonomous vehicle), and so on.

For example, turning to <FIG>, the autonomous vehicle's location in <FIG> is depicted with respect to the map information <NUM>. In this example, a set of potential stopping locations may include all of the nodes in front of the autonomous vehicle and within a certain distance in time (e.g., <NUM> seconds). In this example, the nodes of the set of potential stopping locations A-Z and <NUM>-<NUM> are depicted and all other nodes (of the map information) are not depicted.

In some instances, the set of potential stopping locations may be identified, for instance, by scoring the potential stopping location. As with typical scoring approaches, the scoring may be based on road geometry, how wide the stopping location is (e.g., if a shoulder or parking spot), probability that the stopping location will block or inconvenience other road users (e.g., if there is construction ahead which blocks a left lane, stopping to the left may reduce the likelihood of blocking other road users), whether the autonomous vehicle's perception system is able to perceive the stopping location, whether the stopping location is across <NUM> lanes, within a lane, within a shoulder, within an intersection, at a railroad crossing, high-speed lanes, and so on. One or more of the highest-scoring stopping locations may be included in the set of potential stopping locations.

In this regard, the set of potential stopping locations may include all of the identified potential stopping locations or simply one or more of the highest-scoring stopping locations may be identified. By using all of the potential stopping locations, this may enable the autonomous vehicle to avoid potential states that may result in less desirable stopping locations as discussed further below.

Returning to <FIG>, at block <NUM>, a set of potential states may be identified. Each of the potential states may include a combination of a location and a plurality of characteristics. Each location may correspond to a node of the roadgraph that the autonomous vehicle can potentially reach within the planning horizon (e.g., the next <NUM> seconds) based on one or more of the autonomous vehicle's current speed, achievable speed within that period, speed limit of the road on which the autonomous vehicle is driving, etc. Alternatively, the planning system may simply select the X number of reachable nodes in the roadgraph given the current direction of travel of the autonomous vehicle <NUM>. As noted above, each potential state may also include a plurality of characteristics. The plurality of characteristics for each potential state may include a combination of different characteristics, such as heading, orientation, speed, acceleration or deceleration, rate of change of acceleration or deceleration, and so on. The exact combinations used may be predefined (e.g., manually or by using a machine learned process to select the most likely potential states at any given node of the roadgraph) and depend upon the desires of the transportation service, the current driving conditions for the autonomous vehicle (e.g., traffic, current speed of the autonomous vehicle, etc.), as well as the capabilities of the autonomous vehicle.

A given location or node may be assigned a plurality of different combinations of characteristics, such that a single node may be used in multiple states with different combinations of characteristics. As an example, each node may be assigned to up to or even more than six potential states for a plurality of different times. For instance, the plurality of potential states may include a first potential state including a first node with a first combination of characteristics, the first node with a second combination of characteristics, a second node with the first combination of characteristics, the second node with the second plurality of characteristics, a third node with the second combination of characteristics, and so on. In this regard, each of nodes A-Z and <NUM>-<NUM> may be assigned one or more combinations of characteristics.

Returning to <FIG>, at block <NUM>, a fallback cost for each potential state of the set of potential states to reach each potential stopping location of the set of potential stopping locations may be determined. Starting at the potential stopping location, the planning system may use a backward induction approach to plan a trajectory from each of the potential stopping locations to each of the potential states of the set of potential states. The backward induction may start from each of the potential stopping locations and use the map information in order to compute the cost (or fallback cost) of reaching the potential stopping location from any of the potential states of the set of potential states. In this regard, as noted above the map information may include costs for transitioning between nodes, making certain maneuvers (e.g., lane changes), for each potential stopping location, and so on.

In addition, this backward induction may also be used to determine a point in time for each of the potential states or rather at what time the autonomous vehicle can be at a particular node with a particular plurality of characteristics in order to reach a particular potential stopping location with the lowest cost. This point in time may be a future point in time, and may be based on the characteristics of the potential state and capabilities of the autonomous vehicle. In this regard, in addition to the fallback costs, the cost map may include a point in time for each potential state.

In some instances, the backward induction may also be used to determine other characteristics of the autonomous vehicle for each potential state which may also be included in the cost map. For example, if the autonomous vehicle includes a trailer or other feature which may have a different orientation or heading than the rest of the autonomous vehicle, the backward induction may be used to infer that orientation or heading. This may be repeated if there are multiple such features (e.g., multiple trailers, a bus with articulating portions, etc.). Alternatively, such information could be included as part of the plurality of characteristics for the different potential states of the set of potential states.

As shown in <FIG>, the edges (represented by straight arrows) and other connections representing lane changes (represented by curved arrows) between the nodes, are depicted. By connecting these nodes together, the various possible paths that the autonomous vehicle can use to reach each particular node may be identified. The costs of the edges of these paths, costs of the lane changes, as well as other costs as noted above may be summed to determine the costs of each potential stopping location. Of course, as noted above, each node in the set of potential stopping locations may have various different costs depending upon the different potential states of the set of potential states. In this regard, the costs of a path including a plurality of such transitions can be summed to determine the fallback cost of each potential state of the set of potential states or rather stopping the vehicle at the best potential fallback location from any of the set of potential states.

The result is a cost map that the planning system can greedily follow to get a fallback trajectory that leads to the best of the potential stopping locations from any of the potential states. In this regard, the cost map may encode a minimum fallback cost that can be achieved starting from each potential state. Thus, a high fallback cost for a particular potential state of the set of potential states may indicate the optimal fallback trajectory starting from that potential state is more difficult to achieve, and a low fallback cost may indicate that the optimal fallback trajectory starting from that potential state is easier to achieve (e.g., more comfortable braking, etc.).

As an example, <FIG> represents a gradient of fallback costs <NUM> for a plurality of potential states represented by nodes A-Z and <NUM>-<NUM> of the autonomous vehicle reaching a potential stopping location corresponding to the location of node <NUM>. In addition, each potential state represented by the nodes of <FIG> therefore includes the same characteristics for the vehicle, for example, the same speed and heading. Thus, the gradient of fallback costs <NUM> may represent the cost for the autonomous vehicle <NUM> to reach node <NUM> from each of the other nodes of the gradient. In this regard, the gradient of fallback costs <NUM> represents only a portion of the cost map as other gradients of fallback costs would be determined for different stopping locations and characteristics of the autonomous vehicle (e.g., combinations of speed and heading). In this example, the darkest nodes represent potential states having the highest costs, and the lightest nodes representing potential states having the lowest costs.

For instance, nodes that are very close to the autonomous vehicle, such as nodes A, B may have infinitely high costs as the autonomous vehicle <NUM> may not be capable of reaching those nodes. Nodes that are close to the potential stopping location may also have higher costs, for example, as there may not be enough braking power given the autonomous vehicle's current speed, etc.). Similarly, nodes that are beyond or directly adjacent to the potential stopping location may have infinitely high costs as the autonomous vehicle <NUM> may not be capable of reaching those nodes if stopping at the potential stopping location (here, node <NUM>). Thus, as the autonomous vehicle <NUM> moves closer to the node <NUM>, the costs generally increase, and at the same time, nodes in lane <NUM> (which would require the autonomous vehicle to make a lane change) may have higher costs than nodes in lane <NUM> or shoulder area <NUM> (which would allow the autonomous vehicle to pull out of a driving lane). The cost map may therefore provide a simple way to find the "optimal" fallback trajectory for all stopping locations for each potential state.

In some instances, as in the example of gradient of fallback costs <NUM>, when determining these costs, the planning system may ignore information from the perception system such as the location of other objects (e.g., other road users or obstacles such as the vehicle <NUM>) not included in the map information <NUM>. However, in some instances, occluded areas resulting from the locations of this object. For example, an additional cost may be added to any nodes that are occluded or the costs of nodes within occluded areas may be set to infinitely high values. <FIG> depicts occluded area <NUM> resulting from the location of the vehicle <NUM>. In this example, the occluded area includes the locations of nodes O, R, U, X, <NUM>, <NUM>, <NUM>. The occluded areas may be provided to the planning system by the perception system or another occlusion system which identifies which nodes of the roadgraph are occluded based on information generated by the perception system. As an example, the cost of stopping at an occluded node may be increased by a fixed amount or may be set to an infinitely high value so that such nodes (or potential stopping locations have infinitely high costs).

<FIG> represents a gradient of fallback costs <NUM> for a plurality of potential states represented by nodes A-Z and <NUM>-<NUM> of the autonomous vehicle reaching a potential stopping location corresponding to the location of node <NUM> (as in the example of <FIG>). In this example, the gradient of fallback costs <NUM> is based on the occluded area <NUM> resulting from the location of the vehicle <NUM>, the costs of nodes within the occluded area <NUM>, such as nodes O, R, U, X, <NUM>, <NUM>, <NUM>, may be set to high or even infinitely high values. Comparing gradients of fallback costs <NUM> and <NUM>, the costs of nodes O and R reaching the node <NUM> are greater in the gradient of fallback costs <NUM> than in the gradient of fallback costs <NUM>. Thus, the gradients of fallback costs <NUM> and <NUM> are different. Again, like the gradient of fallback costs <NUM>, the gradient of fallback costs <NUM> represents only a portion of the cost map as other gradients of fallback would be determined for different stopping locations and characteristics of the autonomous vehicle (e.g., combinations of speed and heading).

Returning to <FIG>, at block <NUM>, a trajectory for an autonomous vehicle is planned based on a route to a destination and the determined fallback costs. The cost map (including any additional characteristics such as points in time and other characteristics of the autonomous vehicle) may then be input into the planning system in order to plan a nominal trajectory which attempts to avoid higher cost potential states while still making progress towards the autonomous vehicle <NUM>'s destination according to a route provided by the routing system. When multiple alternative nominal trajectories are generated, the overall costs of all nominal trajectories may then be used to select a nominal trajectory. For example, when evaluating the overall costs of a trajectory as described above, if that trajectory includes any potential states of the cost map, the fallback costs of those potential states may be added to the other costs of a trajectory to determine the overall cost of the trajectory. In some instances, these fallback costs may be added based on how close to any of the potential states the trajectory comes. In this regard, the closer the trajectory comes to any particular potential state, the higher the percentage of the fallback cost of that potential state that may be added to the cost of the trajectory. The overall costs of different nominal trajectories may then be compared to select the lowest cost nominal trajectory. In this regard, the planning system may select a nominal trajectory which attempts to avoid higher fallback cost potential states from the cost map.

Alternatively, rather than utilizing costs as described above, the fallback costs for each potential state may be input as hard or soft constraints for trajectory planning. As an example, hard constraints may be those that cannot be violated, while soft constraints may be those that can be violated if no other solution is available. In this regard, when trajectory planning, the overall cost of a particular trajectory may be increased based on how close to any of the potential states of the set of potential states that the trajectory gets.

In some instances, the fallback costs can be used in trajectory searches to avoid potential states along the nominal trajectory with a high fallback cost. For example, trajectories that would include potential states with infinitely high costs may simply be discarded immediately or avoided at the search stage. In this regard, highly undesirable potential states may have the highest fallback costs and thus the most influence on the nominal trajectory (e.g., a heavy penalty for braking prior to entering a railroad crossing). A nominal trajectory search can explore alternative potential states to avoid paying a large fallback cost for any particular potential state according to the cost map. Alternatively, rather than include all potential states as costs or constraints, to reduce the processing power and time needed to complete the planning, only those potential states that have fallback costs higher than a threshold value may be input as costs or constraints. <FIG> is a representation of a nominal trajectory <NUM> which may be generated and selected using the cost map as described above.

A portion of the nominal trajectory may then be used to plan a fallback trajectory to one of the set of potential stopping locations as described above. <FIG> includes a representation of a portion <NUM> of the nominal trajectory <NUM>. The fallback trajectory includes both the portion <NUM> as well as a fallback portion <NUM>, which results in the autonomous vehicle <NUM> stopping at node <NUM>. In this example, the location of node <NUM> may be the potential stopping location with the lowest fallback cost and/or the lowest overall cost as described above. However, in some instances, the cost map may be used to determine the fallback trajectory as described above with respect to the nominal trajectory. A final trajectory may then be generated by using a first portion of the selected nominal trajectory and a second portion of the fallback trajectory. In this regard, the portion <NUM> and fallback portion <NUM> may together form the final trajectory or rather, the fallback trajectory which is used to control the autonomous vehicle <NUM>.

Returning to <FIG>, at block <NUM>, the autonomous vehicle is controlled according to at least a portion of the trajectory. The final trajectory may be published to other systems of the autonomous vehicle and used to control the autonomous vehicle. In this regard, at least a subportion of the portion of the selected nominal trajectory (e.g., something less than the portion of the selected nominal trajectory) may be used to control the autonomous vehicle until a new trajectory is generated and published to the various systems of the autonomous vehicle. In other words, some smaller portion of the portion <NUM> may be used to control the autonomous vehicle <NUM> until a new final trajectory, determined as described above, is generated and published. If a new final trajectory is not received in time, the autonomous vehicle <NUM> may continue to follow the final trajectory, including the fallback portion <NUM> in order to cause the autonomous vehicle <NUM> to stop at the location of node Q.

The results of this approach may improve the autonomous vehicle's performance in the event that the autonomous vehicle needs to stop and/or pull over. For instance, because the fallback costs of different potential states are used to generate each nominal trajectory, the nominal trajectories may be generated in order to position the autonomous vehicle as best as possible in the event that the autonomous vehicle needs to stop because of a failure in the planning system (e.g., because a new trajectory is not generated in time). As an example, the autonomous vehicle may be more likely to generate nominal trajectories which cause the autonomous vehicle to slow down around turns or curves in order to enable the autonomous vehicle to still stop or pullover within a visible range of the autonomous vehicle's perception system or which cause the autonomous vehicle to drive through locations where stopping is not safe or advisable (e.g., intersections or railroad crossings).

In some instances, the features described herein may extend the amount of time the autonomous vehicle has to stop or pull over providing for additional options for stopping the autonomous vehicle. For example, in the case of a situation where the autonomous vehicle is in heavy traffic with a small (e.g., a <NUM> to <NUM>-foot distance) between the autonomous vehicle and another vehicle in front of the autonomous vehicle, without the features, the fallback trajectory may require that the autonomous vehicle stop within that short distance. For example, a fallback trajectory generated as described herein might be able to maneuver the autonomous vehicle to the left or right side of a lead vehicle and stop there by avoiding potential states that require the autonomous vehicle to stop behind the lead vehicle and encouraging a nominal trajectory that provides enough room to steer around the lead vehicle. At the same time, a fallback system that only changes the speed profile of the nominal trajectory would only be able to stop before the lead vehicle.

In addition, when the nominal trajectory is planned while considering the costs of potential fallback outcomes, the autonomous vehicle may be able to travel beyond its current location and therefore make additional progress towards the destination or stop in a more desirable location. For instance, if a nominal trajectory had an autonomous vehicle making a turn at a corner and the autonomous vehicle is not able to perceive the area around the corner, because the fallback trajectory is considered when planning the nominal trajectory, the nominal trajectory may allow the autonomous vehicle to take a corner in such a way that allows the autonomous vehicle to keep the optionality to go straight and avoid the turn.

Claim 1:
A method for controlling an autonomous vehicle, the method comprising:
identifying, by one or more processors (<NUM>), a set of potential stopping locations based on a current location of an autonomous vehicle (<NUM>);
identifying, by the one or more processors, a set of potential states for the autonomous vehicle;
determining, by the one or more processors, a fallback cost for each potential state of the set of potential states to reach each potential stopping location of the set of potential stopping locations;
planning, by the one or more processors, a trajectory (<NUM>) for an autonomous vehicle based on a route to a destination and the determined fallback costs; and
controlling, by the one or more processors, the autonomous vehicle according to at least a portion of the trajectory.