Constraint-Based Speed Profile

Determining a speed plan for an autonomous vehicle (AV) is disclosed. Planned locations of the AV for future time steps are placed in an occupancy grid. The planned locations are based on a strategic speed plan that is determined without taking world objects into account. Predicted locations of the world objects for at least some of the future time steps are placed in the occupancy grid. Respective buffer distances corresponding to the predicted locations are added in the occupancy grid. An estimated speed plan is identified for the AV based on the occupancy grid. The speed plan is obtained from the estimated speed plan. The AV is then controlled according to the speed plan.

TECHNICAL FIELD

This application relates to autonomous driving and, more particularly, to using constraints to generate a speed profile for an autonomous vehicle.

BACKGROUND

Increasing autonomous vehicle usage creates the potential for more efficient movement of passengers and cargo through a transportation network. Moreover, the use of autonomous vehicles can result in improved vehicle safety and more effective communication between vehicles. However, external objects make traversing the transportation network autonomously difficult.

SUMMARY

A first aspect of the disclosed implementations is a method for determining a speed plan for an autonomous vehicle (AV). The method includes placing, for future time steps, planned locations of the AV in an occupancy grid, where the planned locations are based on a strategic speed plan that is determined without taking world objects into account; placing, for at least some of the future time steps, predicted locations of the world objects in the occupancy grid; adding respective buffer distances corresponding to the predicted locations in the occupancy grid; obtaining an estimated speed plan for the AV based on the occupancy grid; obtaining the speed plan from the estimated speed plan; and controlling the AV according to the speed plan.

A second aspect of the disclosed implementations is an AV that includes a memory and a processor. The processor is configured to execute instructions stored in the memory to determine a speed plan for the AV. The instructions include instructions to place, for future time steps, planned locations of the AV in an occupancy grid, where the planned locations are based on a strategic speed plan that is determined without taking world objects into account; place, for at least some of the future time steps, predicted locations of the world objects in the occupancy grid; add respective buffer distances corresponding to the predicted locations in the occupancy grid; obtain an estimated speed plan for the AV based on the occupancy grid; obtain the speed plan from the estimated speed plan; and control the AV according to the speed plan.

A third aspect of the disclosed implementations is a non-transitory computer readable medium storing instructions operable to cause one or more processors to perform operations for determining a speed plan for an autonomous vehicle (AV). The operations include placing, for future time steps, planned locations of the AV in an occupancy grid, where the planned locations are based on a strategic speed plan that is determined without taking world objects into account; placing, for at least some of the future time steps, predicted locations of the world objects in the occupancy grid; adding respective buffer distances corresponding to the predicted locations in the occupancy grid; obtaining an estimated speed plan for the AV based on the occupancy grid; obtaining the speed plan from the estimated speed plan; and controlling the AV according to the speed plan.

Variations in these and other aspects, features, elements, implementations, and embodiments of the methods, apparatus, procedures, and algorithms disclosed herein are described in further detail hereafter.

DETAILED DESCRIPTION

A self-driving vehicle, such as an autonomous vehicle (AV) or a semi-autonomous vehicle that includes an advanced driver-assistance system (ADAS), may traverse a portion of a vehicle transportation network using information derived from sensors. For simplicity of explanation, and unless otherwise indicated, both AVs and ADAS-enabled vehicles are both referred to as AVs.

An AV uses advanced sensors such as cameras, LiDAR, and radar to continuously scan monitor its scene (e.g., surrounding environment). Data from these sensors may be processed by onboard computers to create a detailed map of the scene. Combined with real-time traffic, GPS information, and map information, an optimal route is determined for the AV. Advanced control systems enable quick decisions on acceleration, braking, and steering, ensuring safe navigation in dynamic settings.

Constraint-based speed profile integrates the planned locations (e.g., positions) of an AV (e.g., a host vehicle), and the predicted (e.g., anticipated) locations of external world objects that might obstruct the planned route of the AV into an occupancy grid. The planned locations of the AV are obtained from (e.g., based on) a strategic speed plan, while the predicted positions of external objects may be derived from hypotheses that the AV maintains (e.g., via internal components, software, or modules therein) with respect to the world objects. In certain scenarios, virtual world objects may also be added to the occupancy grid.

A search algorithm then estimates a tactical speed plan (also referred to herein as a modified speed plan, detailed-planned trajectory, a short-term speed plan, or tactical speed plan), adjusting the initial strategic speed plan based on the world objects in the scene. The occupancy grid can be used to set constraints for a constraint-based optimization problem to solve for an optimal speed plan relative to the objects in the occupancy grid. Constraint-based speed profile can be calculated (e.g., obtained) at regular intervals (e.g., at every time step), ensuring that speed plan is constantly refined in response to the evolving scene, guaranteeing safe operation of the AV. To be clear, an estimated speed plan is obtained using the search algorithm; and then the speed plan is then obtained from the estimated speed plan using by solving an optimization problem. To be even clearer, while the output of the search algorithm is referred to as an estimated speed plan,” the output is not technically a speed plan; rather, it is an estimate of the distance along the path the AV will travel over the time, relative to the occupied space in the occupancy grid. The estimated speed plan does not include any velocity or acceleration data—it only includes distance values at discrete time steps.

A “speed plan,” as used herein, can be a dataset that associates a vehicle speed with specific longitudinal positions or travel times during autonomous driving. The speed plan dictates the desired speed or other motion parameters (e.g., acceleration or jerk) for the vehicle at various points or moments on its (planned) route, ensuring the vehicle adheres to these targets as it navigates autonomously. Thus, the speed plan can include speeds and positions. Determining a speed plan can include identifying associated speeds (or motion parameters) for portions of a path identified by a route planner. The portions of the path are those corresponding to a planning window, as described herein. A “strategic” speed plan is a speed plan that is planned without taking into account other identified world objects. A “tactical” (or “modified”) speed plan modifies the strategic plan based on the world objects in the scene.

To describe some implementations of the teachings herein in greater detail, reference is first made to the environment in which this disclosure may be implemented.FIG.1is a diagram of an example of a portion of a vehicle100in which the aspects, features, and elements disclosed herein may be implemented. The vehicle100includes a chassis102, a powertrain104, a controller114, wheels132/134/136/138, and may include any other element or combination of elements of a vehicle. Although the vehicle100is shown as including four wheels132/134/136/138for simplicity, any other propulsion device or devices, such as a propeller or tread, may be used. InFIG.1, the lines interconnecting elements, such as the powertrain104, the controller114, and the wheels132/134/136/138, indicate that information, such as data or control signals, power, such as electrical power or torque, or both information and power, may be communicated between the respective elements. For example, the controller114may receive power from the powertrain104and communicate with the powertrain104, the wheels132/134/136/138, or both, to control the vehicle100, which can include accelerating, decelerating, steering, or otherwise controlling the vehicle100.

The powertrain104includes a power source106, a transmission108, a steering unit110, a vehicle actuator112, and may include any other element or combination of elements of a powertrain, such as a suspension, a drive shaft, axles, or an exhaust system. Although shown separately, the wheels132/134/136/138may be included in the powertrain104.

The power source106may be any device or combination of devices operative to provide energy, such as electrical energy, thermal energy, or kinetic energy. For example, the power source106includes an engine, such as an internal combustion engine, an electric motor, or a combination of an internal combustion engine and an electric motor, and is operative to provide kinetic energy as a motive force to one or more of the wheels132/134/136/138. In some embodiments, the power source106includes a potential energy unit, such as one or more dry cell batteries, such as nickel-cadmium (NiCd), nickel-zinc (NiZn), nickel metal hydride (NiMH), lithium-ion (Li-ion); solar cells; fuel cells; or any other device capable of providing energy.

The transmission108receives energy, such as kinetic energy, from the power source106and transmits the energy to the wheels132/134/136/138to provide a motive force. The transmission108may be controlled by the controller114, the vehicle actuator112, or both. The steering unit110may be controlled by the controller114, the vehicle actuator112, or both and controls the wheels132/134/136/138to steer the vehicle. The vehicle actuator112may receive signals from the controller114and may actuate or control the power source106, the transmission108, the steering unit110, or any combination thereof to operate the vehicle100.

In the illustrated embodiment, the controller114includes a location unit116, an electronic communication unit118, a processor120, a memory122, a user interface124, a sensor126, and an electronic communication interface128. Although shown as a single unit, any one or more elements of the controller114may be integrated into any number of separate physical units. For example, the user interface124and the processor120may be integrated in a first physical unit, and the memory122may be integrated in a second physical unit. Although not shown inFIG.1, the controller114may include a power source, such as a battery. Although shown as separate elements, the location unit116, the electronic communication unit118, the processor120, the memory122, the user interface124, the sensor126, the electronic communication interface128, or any combination thereof can be integrated in one or more electronic units, circuits, or chips.

In some embodiments, the processor120includes any device or combination of devices, now-existing or hereafter developed, capable of manipulating or processing a signal or other information, for example optical processors, quantum processors, molecular processors, or a combination thereof. For example, the processor120may include one or more special-purpose processors, one or more digital signal processors, one or more microprocessors, one or more controllers, one or more microcontrollers, one or more integrated circuits, one or more Application Specific Integrated Circuits, one or more Field Programmable Gate Arrays, one or more programmable logic arrays, one or more programmable logic controllers, one or more state machines, or any combination thereof. The processor120may be operatively coupled with the location unit116, the memory122, the electronic communication interface128, the electronic communication unit118, the user interface124, the sensor126, the powertrain104, or any combination thereof. For example, the processor may be operatively coupled with the memory122via a communication bus130.

The processor120may be configured to execute instructions. Such instructions may include instructions for remote operation, which may be used to operate the vehicle100from a remote location, including the operations center. The instructions for remote operation may be stored in the vehicle100or received from an external source, such as a traffic management center, or server computing devices, which may include cloud-based server computing devices.

The memory122may include any tangible non-transitory computer-usable or computer-readable medium capable of, for example, containing, storing, communicating, or transporting machine-readable instructions or any information associated therewith, for use by or in connection with the processor120. The memory122may include, for example, one or more solid state drives, one or more memory cards, one or more removable media, one or more read-only memories (ROM), one or more random-access memories (RAM), one or more registers, one or more low power double data rate (LPDDR) memories, one or more cache memories, one or more disks (including a hard disk, a floppy disk, or an optical disk), a magnetic or optical card, or any type of non-transitory media suitable for storing electronic information, or any combination thereof.

The electronic communication interface128may be a wireless antenna, as shown, a wired communication port, an optical communication port, or any other wired or wireless unit capable of interfacing with a wired or wireless electronic communication medium140.

The electronic communication unit118may be configured to transmit or receive signals via the wired or wireless electronic communication medium140, such as via the electronic communication interface128. Although not explicitly shown inFIG.1, the electronic communication unit118is configured to transmit, receive, or both via any wired or wireless communication medium, such as radio frequency (RF), ultra violet (UV), visible light, fiber optic, wire line, or a combination thereof. AlthoughFIG.1shows a single one of the electronic communication unit118and a single one of the electronic communication interface128, any number of communication units and any number of communication interfaces may be used. In some embodiments, the electronic communication unit118can include a dedicated short-range communications (DSRC) unit, a wireless safety unit (WSU), IEEE 802.11p (WiFi-P), or a combination thereof.

The location unit116may determine geolocation information, including but not limited to longitude, latitude, elevation, direction of travel, or speed, of the vehicle100. For example, the location unit includes a global positioning system (GPS) unit, such as a Wide Area Augmentation System (WAAS) enabled National Marine Electronics Association (NMEA) unit, a radio triangulation unit, or a combination thereof. The location unit116can be used to obtain information that represents, for example, a current heading of the vehicle100, a current position of the vehicle100in two or three dimensions, a current angular orientation of the vehicle100, or a combination thereof.

The user interface124may include any unit capable of being used as an interface by a person, including any of a virtual keypad, a physical keypad, a touchpad, a display, a touchscreen, a speaker, a microphone, a video camera, a sensor, and a printer. The user interface124may be operatively coupled with the processor120, as shown, or with any other element of the controller114. Although shown as a single unit, the user interface124can include one or more physical units. For example, the user interface124includes an audio interface for performing audio communication with a person, and a touch display for performing visual and touch-based communication with the person.

The sensor126may include one or more sensors, such as an array of sensors, which may be operable to provide information that may be used to control the vehicle. The sensor126can provide information regarding current operating characteristics of the vehicle or its surroundings. The sensor126includes, for example, a speed sensor, acceleration sensors, a steering angle sensor, traction-related sensors, braking-related sensors, or any sensor, or combination of sensors, that is operable to report information regarding some aspect of the current dynamic situation of the vehicle100.

In some embodiments, the sensor126includes sensors that are operable to obtain information regarding the physical environment surrounding the vehicle100. For example, one or more sensors detect road geometry and obstacles, such as fixed obstacles, vehicles, cyclists, and pedestrians. The sensor126can be or include one or more video cameras, laser-sensing systems, infrared-sensing systems, acoustic-sensing systems, or any other suitable type of on-vehicle environmental sensing device, or combination of devices, now known or later developed. The sensor126and the location unit116may be combined.

Although not shown separately, the vehicle100may include a trajectory controller. For example, the controller114may include a trajectory controller. The trajectory controller may be operable to obtain information describing a current state of the vehicle100and a route planned for the vehicle100, and, based on this information, to determine and optimize a trajectory for the vehicle100. In some embodiments, the trajectory controller outputs signals operable to control the vehicle100such that the vehicle100follows the trajectory that is determined by the trajectory controller. For example, the output of the trajectory controller can be an optimized trajectory that may be supplied to the powertrain104, the wheels132/134/136/138, or both. The optimized trajectory can be a control input, such as a set of steering angles, with each steering angle corresponding to a point in time or a position. The optimized trajectory can be one or more paths, lines, curves, or a combination thereof.

One or more of the wheels132/134/136/138may be a steered wheel, which is pivoted to a steering angle under control of the steering unit110; a propelled wheel, which is torqued to propel the vehicle100under control of the transmission108; or a steered and propelled wheel that steers and propels the vehicle100.

A vehicle may include units or elements not shown inFIG.1, such as an enclosure, a Bluetooth® module, a frequency modulated (FM) radio unit, a Near-Field Communication (NFC) module, a liquid crystal display (LCD) display unit, an organic light-emitting diode (OLED) display unit, a speaker, or any combination thereof.

FIG.2is a diagram of an example of a portion of a vehicle transportation and communication system200in which the aspects, features, and elements disclosed herein may be implemented. The vehicle transportation and communication system200includes a vehicle202, such as the vehicle100shown inFIG.1, and one or more external objects, such as an external object206, which can include any form of transportation, such as the vehicle100shown inFIG.1, a pedestrian, cyclist, as well as any form of a structure, such as a building. The vehicle202may travel via one or more portions of a transportation network208, and may communicate with the external object206via one or more of an electronic communication network212. Although not explicitly shown inFIG.2, a vehicle may traverse an area that is not expressly or completely included in a transportation network, such as an off-road area. In some embodiments, the transportation network208may include one or more of a vehicle detection sensor210, such as an inductive loop sensor, which may be used to detect the movement of vehicles on the transportation network208.

The electronic communication network212may be a multiple access system that provides for communication, such as voice communication, data communication, video communication, messaging communication, or a combination thereof, between the vehicle202, the external object206, and an operations center230. For example, the vehicle202or the external object206may receive information, such as information representing the transportation network208, from the operations center230via the electronic communication network212.

The operations center230includes a controller apparatus232, which includes some or all of the features of the controller114shown inFIG.1. The controller apparatus232can monitor and coordinate the movement of vehicles, including autonomous vehicles. The controller apparatus232may monitor the state or condition of vehicles, such as the vehicle202, and external objects, such as the external object206. The controller apparatus232can receive vehicle data and infrastructure data including any of: vehicle velocity; vehicle location; vehicle operational state; vehicle destination; vehicle route; vehicle sensor data; external object velocity; external object location; external object operational state; external object destination; external object route; and external object sensor data.

Further, the controller apparatus232can establish remote control over one or more vehicles, such as the vehicle202, or external objects, such as the external object206. In this way, the controller apparatus232may teleoperate the vehicles or external objects from a remote location. The controller apparatus232may exchange (send or receive) state data with vehicles, external objects, or a computing device, such as the vehicle202, the external object206, or a server computing device234, via a wireless communication link, such as the wireless communication link226, or a wired communication link, such as the wired communication link228.

The server computing device234may include one or more server computing devices, which may exchange (send or receive) state signal data with one or more vehicles or computing devices, including the vehicle202, the external object206, or the operations center230, via the electronic communication network212.

In some embodiments, the vehicle202or the external object206communicates via the wired communication link228, a wireless communication link214/216/224, or a combination of any number or types of wired or wireless communication links. For example, as shown, the vehicle202or the external object206communicates via a terrestrial wireless communication link214, via a non-terrestrial wireless communication link216, or via a combination thereof. In some implementations, a terrestrial wireless communication link214includes an Ethernet link, a serial link, a Bluetooth link, an infrared (IR) link, an ultraviolet (UV) link, or any link capable of electronic communication.

A vehicle, such as the vehicle202, or an external object, such as the external object206, may communicate with another vehicle, external object, or the operations center230. For example, a host, or subject, vehicle202may receive one or more automated inter-vehicle messages, such as a basic safety message (BSM), from the operations center230via a direct communication link224or via an electronic communication network212. For example, the operations center230may broadcast the message to host vehicles within a defined broadcast range, such as three hundred meters, or to a defined geographical area. In some embodiments, the vehicle202receives a message via a third party, such as a signal repeater (not shown) or another remote vehicle (not shown). In some embodiments, the vehicle202or the external object206transmits one or more automated inter-vehicle messages periodically based on a defined interval, such as one hundred milliseconds.

The vehicle202may communicate with the electronic communication network212via an access point218. The access point218, which may include a computing device, is configured to communicate with the vehicle202, with the electronic communication network212, with the operations center230, or with a combination thereof via wired or wireless communication links214/220. For example, an access point218is a base station, a base transceiver station (BTS), a Node-B, an enhanced Node-B (eNode-B), a Home Node-B (HNode-B), a wireless router, a wired router, a hub, a relay, a switch, or any similar wired or wireless device. Although shown as a single unit, an access point can include any number of interconnected elements.

The vehicle202may communicate with the electronic communication network212via a satellite222or other non-terrestrial communication device. The satellite222, which may include a computing device, may be configured to communicate with the vehicle202, with the electronic communication network212, with the operations center230, or with a combination thereof via one or more communication links216/236. Although shown as a single unit, a satellite can include any number of interconnected elements.

The electronic communication network212may be any type of network configured to provide for voice, data, or any other type of electronic communication. For example, the electronic communication network212includes a local area network (LAN), a wide area network (WAN), a virtual private network (VPN), a mobile or cellular telephone network, the Internet, or any other electronic communication system. The electronic communication network212may use a communication protocol, such as the Transmission Control Protocol (TCP), the User Datagram Protocol (UDP), the Internet Protocol (IP), the Real-time Transport Protocol (RTP), the Hyper Text Transport Protocol (HTTP), or a combination thereof. Although shown as a single unit, an electronic communication network can include any number of interconnected elements.

In some embodiments, the vehicle202communicates with the operations center230via the electronic communication network212, access point218, or satellite222. The operations center230may include one or more computing devices, which are able to exchange (send or receive) data from a vehicle, such as the vehicle202; data from external objects, including the external object206; or data from a computing device, such as the server computing device234.

In some embodiments, the vehicle202identifies a portion or condition of the transportation network208. For example, the vehicle202may include one or more on-vehicle sensors204, such as the sensor126shown inFIG.1, which includes a speed sensor, a wheel speed sensor, a camera, a gyroscope, an optical sensor, a laser sensor, a radar sensor, a sonic sensor, or any other sensor or device or combination thereof capable of determining or identifying a portion or condition of the transportation network208.

The vehicle202may traverse one or more portions of the transportation network208using information communicated via the electronic communication network212, such as information representing the transportation network208, information identified by one or more on-vehicle sensors204, or a combination thereof. The external object206may be capable of all or some of the communications and actions described above with respect to the vehicle202.

For simplicity,FIG.2shows the vehicle202as the host vehicle, the external object206, the transportation network208, the electronic communication network212, and the operations center230. However, any number of vehicles, networks, or computing devices may be used. In some embodiments, the vehicle transportation and communication system200includes devices, units, or elements not shown inFIG.2.

Although the vehicle202is shown communicating with the operations center230via the electronic communication network212, the vehicle202(and the external object206) may communicate with the operations center230via any number of direct or indirect communication links. For example, the vehicle202or the external object206may communicate with the operations center230via a direct communication link, such as a Bluetooth communication link. Although, for simplicity,FIG.2shows one of the transportation network208and one of the electronic communication network212, any number of networks or communication devices may be used.

The external object206is illustrated as a second, remote vehicle inFIG.2. An external object is not limited to another vehicle. An external object may be any infrastructure element, for example, a fence, a sign, a building, etc., that has the ability transmit data to the operations center230. The data may be, for example, sensor data from the infrastructure element.

Regardless of the sensor source, each vehicle traveling in the vehicle transportation network determines its (e.g., optimal) operation based on the sensed data. Collective action based on the sensed data as described herein can improve the operation of multiple vehicles and can also improve the operation of the vehicle transportation system itself.

FIG.3illustrates a system that includes an optimized speed plan tool (OSP) for constraint-based speed profile generation. The system300includes a world302and an AV304. The world302can be as described with respect to the transportation and communication system200ofFIG.2. As such, the world302can include other world objects that are traversing roads that may be included or described in a map (e.g., a high-definition (HD) map) included in or accessible by the AV304. The AV304can be the vehicle100ofFIG.1or the vehicle202ofFIG.2.

The AV304includes a set of tools that may collectively to be referred to as the AV software stack. The AV software stack encompasses a set of algorithms that seamlessly collaborate to enable various aspects of the autonomous operations of the AV304. The software stack may orchestrate sensor data fusion, perception, decision-making, and control to safely navigate and interact with the world302.

The AV304(i.e., the AV software stack therein or associated therewith) is shown as including a perception tool306, a world model prediction tool308, a route planning/decision making tool310, an OSP tool312, a proactive risk mitigation tool314, a trajectory following tool316, and an AV control tool318. The disclosure herein is mainly focused on the functional aspects, operations, and capabilities of the OSP tool312.

At least some of the tools can be implemented as respective software programs that may be executed by one or more processors, such as the processor120ofFIG.1. A software program can include machine-readable instructions that may be stored in a memory such as the memory122ofFIG.1, and that, when executed by a processor, such as the processor120, may cause the performance of the instructions of the software program. In some implementations, more or fewer tools may be included in the AV software stack. Some of the tools or aspects thereof may be implemented directly in hardware, firmware, software executed by hardware, circuitry, or a combination thereof.

The perception tool306includes sensors and obtains sensor data from the world302. For example, the perception tool may obtain images of the world302, points clouds corresponding to objects in the world302, and so on. The world model prediction tool308receives the sensor data and determines (e.g., converts to, detects, etc.) world objects from the sensor data. That is, for example, the world model prediction tool308determines the world objects from the received sensor data. For example, the world model prediction tool308can convert a point cloud received from a light detection and ranging (LiDAR) sensor (i.e., a sensor of the sensor126) into a world object. Sensor data from several sensors can be fused together to determine (e.g., guess the identity of, classify, etc.) the world objects. Examples of world objects include a bicycle, a pedestrian, and a vehicle.

The world model prediction tool308can receive sensor information that allows the world model prediction tool308to obtain (e.g., determine, calculate, identify, select, etc.) and maintain additional information for at least some of the detected world objects. For example, the world model prediction tool308can maintain respective states for at least some of the determined world objects. For example, the state associated with a world object can include zero or more of a velocity, a pose, a geometry (such as width, height, and depth), a classification (e.g., bicycle, large truck, pedestrian, road sign, etc.), and a location. As such, the state of an object includes discrete state information (e.g., classification) and continuous state information (e.g., pose and velocity).

The world model prediction tool308fuses sensor information, tracks world objects, maintains lists of hypotheses for at least some of the dynamic objects (e.g., an object A might be going straight, turning right, or turning left), creates and maintains predicted trajectories for each hypothesis, and maintains likelihood estimates of each hypothesis (e.g., object A is going straight with probability of 90% considering the object pose/velocity and the trajectory poses/velocities).

The route planning/decision making tool310determines a road-level plan. For example, given a starting location and a destination location, the route planning/decision making tool310determines a route from the starting location to the destination location. The route planning/decision making tool310can determine the list of roads (i.e., the road-level plan) to be followed by the AV to navigate from the starting location to the destination location.

The route planning/decision making tool310determines (e.g., identifies) decisions along the road-level plan. High level descriptive examples of discrete-level decisions may include: stop at the interaction between road A and road B, move forward slowly, accelerate to a certain speed limit and then merge onto the rightmost lane, prepare to stop because a stop light may be turning red, etc. The decisions may be based on data included in the map. For example, the map may indicate the presence of a traffic light or that a lane merges onto another and so on. The output of the route planning/decision making tool310may be referred to as a strategic speed plan. As mentioned above, the speed plan is “strategic” because it is planned without taking into account the other identified world objects. WhileFIG.3illustrates that route planning/decision making tool310may follow the world model prediction tool308, that need not be the case.

The OSP tool312, which is further described herein, modifies the strategic speed plan based on world objects that may interfere with the path (e.g., with the strategic speed plan) of the AV304based on an occupancy grid. The OSP tool312can be said to plan for a current scenario (e.g., world objects observed in the scene at a current time step to) as well as how the scenario will develop at future time steps (tn, where n=1, . . . , N) within a planning horizon (e.g., 6 seconds into the future or any other number of seconds). The planning horizon can be divided into a pre-defined number of time steps N. The OSP tool312accounts for multiple world objects interacting with the AV304. That is, the OSP tool312can handle multiple constraints related to multiple respective world objects at once (e.g., simultaneously). While certain scenarios (e.g., edging for unprotected turns at intersections and crossing intersections) are described herein with respect to the OSP tool312, as a person skilled in the art can appreciate, the teachings herein can be easily extended or adapted to other scenarios encountered by the AV304, such as fitting into gaps during merges or other road scenarios.

Whereas the route planning/decision making tool310generates a strategic speed plan, the OSP tool312may modify the strategic speed plan (more accurately, the portion of the strategic speed plan corresponding to the planning window, also referred to as a time horizon) to obtain a tactical speed plan (or a detailed-planned trajectory). The OSP tool312can receive the discrete-level decisions of the strategic speed plan, the world objects (and corresponding state information), and the predicted trajectories and likelihoods of the external objects. The OSP tool312can use at least some of the received information to determine the detailed-planned trajectory (e.g., the tactical speed plan) for the AV304.

The OSP tool312can be summarized as performing the steps of: filling out (e.g., generating, constructing, updating, etc.) an occupancy grid, performing a search algorithm (e.g., A*) to identify an estimated path, smoothing the estimated path, adding stop conditions (such as described below with respect to stop lines), formulating constraints, and then solving an optimization problem based on the constraints. The solution to the optimization problem is the short-term or tactical speed plan.

The proactive risk mitigation tool314may further adjust the tactical speed plan to account for potential hazards. A potential hazard is one that is not currently determined to interfere with the path of the AV304but which might in the future. To illustrate, a vehicle may be parked at a side of a street. The driver's door is currently closed. As such, the door does not currently interfere with the path of the AV304. However, the door may open in the future and may cause the AV to perform an emergency maneuver to avoid the door.

The proactive risk mitigation tool314considers the reactive capabilities of the AV304in planning a proactive trajectory for the vehicle that minimizes speed and/or lateral changes in movement responsive to a potential hazard while still allowing for a comfortable and safe reactive response (i.e., a reactive trajectory) in the event a hazard object interferes with the path of the vehicle. A proactive trajectory for the AV304may be determined that adjusts the planned path and speed proactively for collision avoidance if the hazard object materializes as predicted. The proactive trajectory is such that if the hazard materializes, the AV304would not have to make an emergency, evading maneuver that would be uncomfortable for occupants of the AV304. Again, with respect to the door scenario, the AV304may be moved just enough to the left.

The trajectory following tool316produces control signals (e.g., steering, acceleration, etc.) to cause the AV304to be controlled according to the output of the route planning/decision making tool310. The route planning/decision making tool310may operate at a first frequency (e.g., 10 Hertz) and the trajectory following tool316may operate at a second, different, frequency (e.g., 100 Hertz). The AV control tool381may output control signals to control actuators of the AV304so that the AV304is controlled according to the speed plan.

FIG.4is a process flow400that illustrates the operations of the OSP tool312ofFIG.3. The OSP tool312constructs an occupancy grid that includes an AV and other world objects. Constraints are associated with the other world objects. A search algorithm is used to find a path (e.g., a tactical speed plan) for the AV based on the occupancy grid. In an example, an A* (pronounced A-star) search algorithm is used. However, other search algorithms are possible.

For ease of understanding, visual representations of occupancy grids are shown and described herein. However, any suitable data structure maintainable in a memory, such as the memory122ofFIG.1, and usable by the search algorithm can be used. An occupancy grid can be thought of as a tool to visualize and store constraints relative to distance and time as the AV travels along its path. The occupancy grid essentially stores constraints (further described herein) that are relevant to the path of the AV. The process flow400is further described with reference toFIGS.6-12.

At402, upper limits are set in (e.g., added to) the occupancy grid based on the strategic speed plan. Upper limits set the possible distances that the AV could be based on its predicted future locations based on the strategic speed plan.

FIG.5illustrates an example500of setting upper limits in the occupancy grid. The locations of an AV502within the planning window (e.g., time horizon) and according the strategic speed plan are identified. The example500illustrates that the AV502is planned to be at locations504,506,508,510, etc. at time steps t=1, t=2, t=3, t=4, etc., respectively. The planned locations are placed in an occupancy grid512A. The occupancy grid512A is a two-dimensional graph where the x-axis represents time and the y-axis represents distance along the path of the AV502from a current location513(e.g., the location at time step t=0) of the AV502. More specifically, the y-axis can indicate distance traveled by the AV502from one step to the immediately succeeding time step. The occupancy grid illustrates that points514and516of an occupancy grid512A correspond to the locations504and510, respectively.

While the locations may visually appear to be equally spaced inFIG.5, that may not necessarily be case as the strategic speed plan is generated relative to (e.g., based on) road speed limits and road curvatures. Additionally, the road speed limits and road curvatures act as (e.g., are used to set) the upper limit of how fast the AV502can drive with no other road users present. An occupancy grid512B shows that the occupancy grid512A has been updated to include the upper limits.

Each of the vertical bars, such as a vertical bar518, indicates possible locations of the AV502at the corresponding time step. For example, the vertical bar518illustrates the possible locations of the AV502at t=4. An area520illustrates possible positions of the AV502if the AV502were to be moving faster than the speed indicated by the strategic speed plan. An area522indicates would include positions of the AV502if the AV502were to be moving slower than the speed indicated by the strategic speed plan. The occupancy grid512B includes vertical bars (upper limits) corresponding the time steps of the planning window. As such, a vertical bar524corresponds to the last time step in the current planning window.

Referring again toFIG.4, at404, some of the world objects are added to the occupancy grid. More specifically, world objects whose paths are predicted to interact with that of the AV are added to the occupancy grid. Adding a world object to the occupancy grid includes adding the predicted locations of the world object to the occupancy grid. Distances that are based on the predicted paths of the respective world objects are added to the occupancy grid. Buffer distances are also added to the occupancy grid. The distances and the buffer distances are based on whether the world object is classified as an along path world object or a crossing world object. An along path world object is one that is traveling in the same direction as the AV and the AV and world object are currently or will be in the same road lane. A crossing world object is one whose path crosses (e.g., intersects) that of the AV.

FIG.6illustrates an example600of adding along world objects to an occupancy grid. A scene602illustrates that an AV604, a leading vehicle606(e.g., a vehicle that is ahead of the AV604), and a trailing vehicle608(e.g., a vehicle that is behind the AV604) are traveling in the same direction along a lane610. As such, both of the leading vehicle606and the trailing vehicle608are classified as along world objects. An occupancy grid612has been generated and includes upper limits, as described above. As such, a vertical bar614can be similar to the vertical bar518ofFIG.5.

Predicted locations of each of the leading vehicle606and the trailing vehicle608are added to the occupancy grid612. Locations labeled (1), such as a location616, correspond to the predicted locations of the leading vehicle606; and locations labeled (2), such a location618, correspond to the predicted locations of the trailing vehicle608. A respective buffer distance is then added with respect to each of the predicted locations. The calculations of the buffer distances is further described below. The buffer distances are considered to be constraints on the locations of the AV as the tactical speed plan is searched for. Said another way, the buffer distances can be considered to correspond to prohibited locations of the AV when the tactical speed plan is searched for. To illustrate, a buffer distance620is added corresponding to the location616; and a buffer distance622is added corresponding to the location618. In all of the FIGS., buffer distances are those filled with a pattern628. Buffer distances are shown as being in front of and behind world objects added to the occupancy grid. The buffer distance ahead of a leading vehicle can be used in cases where a vehicle may be merging into the AV lane (such as in the case where the AV may be making a left turn at an unprotected intersection). In such cases, the buffer distance can be used to determine whether there is enough distance for the AV to be (e.g., to fit) ahead of the merging vehicle. This scenario, while not specifically described as such, is illustrated with respect toFIG.11Awhere the AV1104will merge in front of the second along path vehicle1114.

FIG.7illustrates another example700of adding world objects to an occupancy grid. A scene702illustrates that an AV704is traveling on a lane706along a planned path708(i.e., according to a strategic speed plan) towards a T-intersection710where the AV704is planned to turn left heading westward on a lane712. The scene702also includes an along path vehicle714that is also predicted to be traveling on the lane712. The along path vehicle714is initially observed to be at a location714′. The scene702also includes a crossing vehicle716. The crossing vehicle716is predicted to be traveling eastward along a path718on a lane720. The paths of the AV704and the crossing vehicle716are predicted to intersect at a location722.

An occupancy grid724has been generated and includes upper limits, as described above. As such, a vertical bar726can be similar to the vertical bar518ofFIG.5. Predicted locations (such as a location728) of the along path vehicle714are added to the occupancy grid724; and locations (such as a location730) of the crossing vehicle716are added to the occupancy grid724. The locations of the crossing vehicle716are not based on a predicted path, over time, of the crossing vehicle716. Rather, and as can be observed in the occupancy grid724, the crossing vehicle716appears to be stationary over time. As the crossing vehicle716briefly crosses the path of the AV704, the locations are time-based (as opposed to being distance based as for along world objects). The location of the crossing vehicle716is the location of intersection (the location722).

Buffer distances, such as buffer distances732and734, are then added with respect to the locations. In the occupancy grid724, a location736corresponds to the location722(i.e., the intersection). The occupancy grid can include additional (e.g., extra) buffer times738for after the crossing vehicle716passes the AV704as an additional safety measure, just in case the crossing vehicle716, for example, stalls.

Buffer distances for along path world objects are set (e.g., calculated, configured, selected, etc.) based on time headway (THW), such as using equation (1); and buffer distances for crossing world objects are set based time-to-collision (TTC), such as using equation (2).

In equation (1), vel is the velocity of the AV704, 1.3 seconds and 1.6 seconds indicate, respectively, the minimum time headway and the maximum time headway; and 14 represents a speed of 14 meters per second. Equation (1) is essentially a linear equation that can interpreted relative to the velocity vel of the AV704. The faster the AV704is traveling, the AV704will eventually reach 1.3 seconds for the THW; and the slower the AV704is traveling, the AV704will stay at 1.6 seconds for the THW. Equation (1) (i.e., the values 1.6, 1.3, 14, and 0.033 used therein) is empirically derived and has been found to result in a comfortable result (e.g., ride) during testing. As is known, THW is the time interval between two vehicles passing a specific point on a roadway, typically measured from the front of one vehicle to the front of the following vehicle.

In equation (2), vxingis the observed speed the crossing vehicle. Equation (2) sets out a minimum required time that is comfortable for the crossing vehicle716to allow the AV704to pass it or for the AV704to wait for the crossing vehicle716to pass the AV704first, specifically at intersections.

Referring again toFIG.4, at406, a virtual lead vehicle is added to the occupancy grid, if necessary (e.g., based on blocked visibility). When virtual predictions (i.e., predicted path or locations of a virtual world object) intersect with the path of the AV, special “virtual” lead vehicles are created (e.g., added to the occupancy grid) to produce an edging effect, causing the AV to slowly inch through the beginning of an intersection for more visibility. This “virtual” lead vehicle disappears (e.g., is removed from the occupancy grid) when visibility becomes possible.

To illustrate, if the AV is turning left at an intersection and the lane onto which the AV is planned to turn is occluded, then a virtual crossing vehicle may be placed close to (e.g., at) a last location on the lane that is observable by sensors of the AV along its path. As further described herein, a virtual lead vehicle is added to the occupancy grid to induce (e.g., cause or generate) an edging motion by the AV towards the intersection. More generally, other types of virtual vehicles can be added, as necessary, depending on the road geometry.

A virtual vehicle is one that does not in fact exist in the scene (e.g., the world302ofFIG.3) and is not one that is perceived by the perception tool306ofFIG.3. Virtual vehicles may be added to the world model maintained by the world model prediction tool308ofFIG.3in cases where portions of a road are occluded, such as when driving around tight corners and/or driving in limited visibility environments (e.g., on a foggy day or a lane is not completely visible) or in anticipation of vehicles unexpectedly appearing. Virtual vehicles are added to the world model in occluded regions of a road (e.g., a lane therein) or at a maximum perception range. The edging motion allows the AV to slowly gain more visibility of the scene while maintaining the ability to stop for real vehicles that may appear and are not currently perceptible by sensors on the AV. The edging motion contributes to the process of crossing unprotected intersections with limited visibility.

FIG.8illustrates another example800of adding a virtual lead vehicle to an occupancy grid. A scene802illustrates that an AV804is traveling along a planned path806towards a T-intersection816where the AV804is planned (based on the strategic speed plan) to turn left heading in an westward direction. The scene802illustrates that a portion of an opposite traffic lane810is occluded by, for example, parked vehicles, such as an obstruction812. The scene802illustrates that a vehicle814may possibly be on the opposite traffic lane810but is not perceptible by the sensors of the AV804because of the obstruction812. As such, the AV804is to be controlled to move cautiously towards and enter the T-intersection816. In such a scenario, a virtual lead vehicle818is added to an occupancy grid820.

The occupancy grid820has been generated and includes upper limits, as described above. As such, a vertical bar822can be similar to the vertical bar518ofFIG.5. The virtual lead vehicle818is treated as an along path world object and predicted locations therefor, such as a location824, are added to the occupancy grid820along with buffer distances, such as a buffer distance826.

Whereas the buffer distances for real (e.g., observed or sensed) along path world objects are calculated as described above with respect to equation (1), the THW with respect to a virtual lead vehicle can be set to a small constant (e.g., 0.5 seconds) so that a close following distance can be maintained. The virtual lead vehicle818is configured to be at an offset ahead of the AV804and have a non zero speed at all times. As such, the virtual lead vehicle818can only slow down the AV804without bringing it to a complete stop. As further described below, the AV804may be stopped if other factors (e.g., a stop line) is added to the occupancy grid. The AV804may also be stopped when/if other world objects are observed and added to the occupancy grid as the scene802evolves.

Referring again toFIG.4, at408, an estimated speed plan (e.g., a short-term or tactical speed plan) for the planning window is determined (e.g., searched, calculated, identified, etc.) based on the occupancy grid. That is, after all relevant world objects are included into the occupancy grid, a search algorithm (e.g., the A* search algorithm) searches for the estimated, quickest path from a current AV position (e.g., a start position, such as a current position624ofFIG.6) to the upper right-hand corner (e.g., a goal position, such as a position626ofFIG.6) representing the farthest distance in the planning window. The result of A* is an estimated speed plan that is then used to formulate constraints in the optimization problem that generates the real speed plan (e.g., the short-term speed plan) used. As already mentioned, the estimated speed plan is in fact a set of distance values at discrete time steps.

A conventional occupancy grid breaks a space down into cells represented in the occupancy grid based on their x and y coordinates; and a conventional A* search is performed in this two-dimensional space based of which cells are occupied or are otherwise unavailable. However, the occupancy grid described herein includes time on the horizontal axis and distance on the vertical axis. The occupancy grids described herein convey information such as: at 50 meters along the path of the AV and 3 seconds from now, that location will be occupied and the AV cannot be allowed to be at that location. The A* search algorithm can be thought of as a variant of the conventional A* search and uses a heuristic that follows the strategic plan as closely as possible to estimate distances along the path of the AV vs. time. For example, the strategic speed plan, world model objects and their relative time headways all represent occupied spaces in the occupancy grid. The occupancy grid indicates to the A* search algorithm the spaces that the AV cannot occupy. To further clarify, everything plotted on the occupancy grid at this point (e.g., strategic speed plan, world objects, THWs) represent an occupied space. To perform the search, at least the following operations are performed: lead vehicles are identified, the strategic speed plan is obtained and the occupancy grid is generated.

Referring again toFIG.4, at410, the estimated short-term speed plan may be smoothed. A path smoothing interpolation algorithm can be applied to the estimated predicted values of the AV therewith resulting in a more realistic (and smoother) speed plan.FIG.9illustrates an example900of an estimated speed plan and an example950of a smoothed estimated speed plan (i.e., a smoothed path). The estimated speed plan illustrated in the example,900is obtained at408and includes a set of locations (shown as black-filled squares), such as a location902, shown on an occupancy grid904. A smoothed estimated speed plan (i.e., a smooth curve952) illustrates the results of the smoothing of the estimated speed plan (or, simply, smoothed speed plan). Again, the estimated (or smoothed) speed plan consists of distance values at discrete time steps.

At412ofFIG.4, the OSP checks whether the smoothed speed plan results in the AV having to be stopped inside (e.g., in the middle) of an intersection at any point in time within the planning window. If not, then the AV is controlled to proceed according to the speed plan (i.e., the smoothed speed plan). On the other hand, if the AV is planned to be stopped in the middle of an intersection, then a virtual stop line is created at the beginning of the intersection preventing the AV from entering until a clear path from the start to end of the intersection is detected (e.g., identified, calculated, planned, etc.). The beginning and the ending of an intersection can be identified based on (e.g., obtained from) a map (e.g., a high-definition map).

FIG.10illustrates an example1000of a stop line when an AV is planned to stop inside of an intersection. A scene1002illustrates that an AV1004is planned to travel along a path1006(e.g., a smooth speed plan), through an intersection1007, westward onto a lane1008that includes an along lead vehicle1010. The scene1002also includes a crossing vehicle1012that is traveling eastward on a lane1014. An occupancy grid1016has been constructed to include predicted locations (such as a location1018) of the along lead vehicle1010and predicted locations (such as a location1020) of the crossing vehicle1012. The occupancy grid1016also illustrates that a smoothed speed plan1022has been identified for the AV1004and that, as indicated by the group of locations1024, the AV1004is planned to be static for a period of time (e.g., more than 1 second) inside the intersection while awaiting the crossing lead vehicle1012to pass so that the AV1004can proceed behind it. The occupancy grid1016also includes lines1026and1028demarcating the start and the end of the intersection1007, as identified based on a map.

As such, a virtual stop line1030is created at the beginning of the intersection therewith preventing the AV from entering the intersection1007until a clear path from the start to end of the intersection can be identified. The OSP tool312updates the occupancy grid1016to obtain an occupancy grid1016′ adding static locations, such as a location1032, that reflect the placement of the virtual stop line1030. Accordingly, a new smoothed speed plan1034can be obtained (e.g., searched or recalculated) based on the updated occupancy grid.

FIGS.11A-11Billustrate an example1100of identifying speed plans based on occupancy grids. The example1100illustrates how occupancy grids are changed as a scene1102evolves.

At a first time, the scene1102includes an AV1104that is traveling along a lane1106and is planned (based on a strategic speed plan) to turn westward onto a lane1108thereby crossing an intersection1110. The scene1102includes a first along path vehicle1112and a second along path vehicle1114that are predicted to be traveling westward on the lane1108. The scene1102also includes a first crossing vehicle1116and a second crossing vehicle1118that are predicted to be traveling eastward on a lane1120. Accordingly, the OSP tool312ofFIG.3generates an occupancy grid1122.

The occupancy grid1122includes the planned locations of the AV1104based on the strategic speed plan, such as indicated by a vertical bar1124; predicted locations (such a location1126) of the first along path vehicle1112and distance buffers associated therewith; predicted locations (such a location1128) of the second along path vehicle1114and distance buffers associated therewith; predicted locations (such a location1130) of the first crossing vehicle1116and distance buffers associated therewith; and predicted locations (such a location1132) of the second crossing vehicle1118and distance buffers associated therewith.

The occupancy grid1122also illustrates that a tactical speed plan that includes locations (such as a location1134) has been identified (using A*, as described above) and smoothed (as illustrated by a smoothed speed plan1136).

As the (smoothed) speed plan results in the AV1104having to stop (not explicitly shown) inside the intersection1110, a virtual stop line1138causing the AV1104to stop at the beginning of the intersection1110is added and a new speed plan is generated. The updated occupancy grid1122′ illustrates the locations (such as a location1140) of the stop line and locations (such a location1142) of an updated speed plan.

At a second time, and as shown inFIG.11B, the scene1102has evolved such that the first along path vehicle1112and the first crossing vehicle1116are no longer in the scene; that the second along path vehicle1114has progressed to the location shown inFIG.11B; that the second crossing vehicle1118has already passed the intersection1110; and that a third along path vehicle1144has entered the scene. A new (updated) occupancy grid1122″ is generated. The occupancy grid1122″ includes new predicted locations (such as a location1146) of the second along path vehicle1114, predicted locations (such as a location1148) of the third along path vehicle1144, and distance buffers associated therewith. The occupancy grid1122″ also illustrates that a tactical speed plan that includes locations (such as a location1150) has been identified (using A*, as described above) and smoothed (as illustrated by a smoothed speed plan1152).

FIG.12is an example of an occupancy grid1200that illustrates upper and lower bounds used as constraints when searching for a speed plan. At every time step, the upper bound constraint (such as an upper bound constraint illustrated by an upward pointing arrow1202) is determined by searching upward from the estimated distance along the path of the AV, and the lower bound constraint (such as a lower bound constraint illustrated by a downward pointing arrow1204) is determined by searching downward from each estimated distance along the path of the AV. It is noted that the occupancy grid1200is the same as the occupancy grid612and corresponds to the scene described with respect toFIG.6. As already mentioned, an optimization problem is set up and solved to obtain the speed plan from the estimated speed plan (which, for brevity, may be the smoothed estimated speed plan).

The optimization problem can be separated into two parts: a cost function that is to be minimized and constraints on the optimization solution (e.g., that the optimization solution must adhere to). The result of the optimization problem is the estimated distances as a function of time along the path for the AV to follow in order to safely interact with the relevant world objects. Said another way, the solution to the optimization problem is a set of optimal velocities and optimal acceleration for the AV to follow in order to create a speed plan to adjust for world objects on the road.

The optimization problem performs a minimization optimization of a cost function, as shown in (3). The optimization problem of (3) is subject to the constraints (3a1)-(3d). In the following, stdenotes the distance along the path at a time step t, vtdenotes the speed of the AV at the time step t, atdenotes the acceleration of the AV the time step t, and Stare slack values.

The constraints (3a1)-(3a2) are kinematic constraints that define (e.g., set) a motion model for the optimization. The motion model (e.g., the constraints (3a1)-(3a2) ensure that the solved optimization values are physically possible for the AV to perform. The constraint (3a1) constrains the possible location of the AV at a next time step t+1 based on the location st, speed vt, and acceleration at atthe time step t. The constraint (3a2) constrains the speed at the next time t+1 step given the AV's speed and acceleration at the time step t.

The constraint (3b), which may be referred to as the vehicle following constraint, sets a lead vehicle following constraint and ensures that the AV maintains a safe following distance from the lead vehicle (LV) (e.g., the leading vehicle606ofFIG.6) and is always ahead of the behind vehicle (BV) (e.g., the trailing vehicle608ofFIG.6). That is, the constraint (3b) sets the keeping distance away from a lead vehicle. The variables sBV, sLV, vLV, aLV (which indicate, respectively, a location of the behind vehicle, a location of the lead vehicle a speed of the lead vehicle, and an acceleration of the lead vehicle) are obtained (as constraint formulations) from the occupancy grid. The locations of the behind vehicle (sBV) set a lower bound on the location of the AV; and the locations of the lead world object set the upper bound. The locations of the lead world object is set/defined by the second equation of motion. The locations of the AV are defined by st+vtτ+St, where τ is the desired THW, which is obtained as described above, and Stare slack values. Thus, the locations of the AV are such that safe distances away from the lead world object are maintained. Upper and lower bounds are illustrated with respect toFIG.12.

The constraint (3c) indicates that the AV is to be moving forward or is static (stopped) at all times. That is, the AV should not be moving backwards. That is, the future locations of the AV are always positive definite. Constraints (3d) set general variable constraints for the AV's velocity (the velocity vtshould always be between 0 and a constant upper limit velocity value vUL); extent of acceptable acceleration or deceleration of the AV at a given time (i.e., −2≤at≤2); extent of acceptable jerk of the AV at a given time (i.e., −0.5≤jt≤1.5); and the range of the slack value at any given time step (i.e., −1.5≤St≤1.5). The constraints (3d) ensure that the optimized values are within the safety limits set by the constraints (3d). While certain limits are described herein, others are possible.

The cost function J(vt, at, St) can be as shown equation (4). The cost function essentially turns a desired experience (e.g., comfort) of occupants of the AV into equation form.

The expression ωdV(vt−vt-1)2results in a minimization of the change in velocity of the AV for a smooth speed plan. The expression ωdA(at−at-1)2results in the minimization of the change in acceleration of the AV for a smooth speed plan. The expression ωV(vt−vdes,t)2results in the minimization of the differences between the velocities of the strategic speed plan and the optimized (e.g., determined) velocities. The expression ωA(at−ades,t)2results in the minimization of the differences from strategic speed plan acceleration and the optimized (e.g., determined) accelerations. The expression ωS(St)2minimizes the slack therewith resulting in a minimizing error. The values ωirepresent respective weights of each term in the cost function. The weights ωiare positive definite numbers that define how heavily to value the respective terms in the cost function. For example, a higher weight for minimizing change in velocity/acceleration compared to the weight in following the speed plan results in prioritizing a smooth speed plan more than following the strategic speed plan.

To further describe some implementations in greater detail, reference is next made to examples of techniques which may be performed by or using a system for constraint-based speed profile.FIG.13is a flowchart of an example of a technique1300for generating a speed profile of for an autonomous vehicle. The technique1300can be executed using devices, such as the systems, hardware, and software described with respect toFIGS.1-12. The technique1300can be performed, for example, by executing a machine-readable program or other computer-executable instructions, such as routines, instructions, programs, or other code. The steps, or operations, of the technique1300, or another technique, method, process, or algorithm described in connection with the implementations disclosed herein can be implemented directly in hardware, firmware, software executed by hardware, circuitry, or a combination thereof.

At1302, planned locations of the AV are placed in an occupancy grid. As described above, the planned locations are based on a strategic speed plan that is determined without taking world objects into account. As described above, the world objects are those that may interfere with the path of the AV. The planned locations that are added to the occupancy grid correspond to locations at future time steps in a planning window.

At1304, predicted locations of the world objects are placed in the occupancy grid, as described above. The predicted locations are those that correspond to at least some of the future time steps. At1306, respective buffer distances corresponding to the predicted locations are added to the occupancy grid. At1308, an estimated speed plan is obtained for the AV based on the occupancy grid. The estimated speed plan may be a smooth speed plan. The estimated speed plan can be obtained using the A* search algorithm. At1310, the speed plan (i.e., the short-term or tactical speed plan) is obtained from the estimated speed plan. As described above, the speed plan can be obtained (e.g., solved for) by formulating an optimization problem that minimizes a change in velocity of the AV, minimizes an acceleration change of the AV, and minimizes differences from the strategic speed plan. The optimization problem can use, as constraints, kinematic constraints of the AV and distance constraints related to distances between the AV and relevant other road users.

At1312, the AV is controlled according to the speed plan. Controlling the AV according to the speed plan can include, and as described above with respect to a virtual stop line, in response to determining that the speed plan causes the AV to stop in an intersection, causing the AV to stop at a virtual stop line that is added to the occupancy grid; continually updating the occupancy grid and identifying an updated speed plan until the updated speed plan is such that the AV does not stop in the intersection; and controlling the AV according to the updated speed plan.

While not specifically shown inFIG.13, the technique1300can be continuously performed (such as at every time step) while the AV is being autonomously controlled.

A world object of the world objects may be identified as an along path world object. In such a case, the respective buffer distances corresponding to the world object can be based on a time headway to the world object. A world object may be identified as a crossing world object. In such a case, the respective buffer distances corresponding to the world object can be based on a time to collision between the AV and the world object.

As described above, in response to determining that a crossing lane is obstructed to sensors of the AV, the technique1300may place a virtual along path vehicle in the occupancy grid.

For simplicity of explanation, each technique herein is depicted and described as a series of operations. However, the operations in accordance with this disclosure can occur in various orders and/or concurrently. Additionally, other steps or operations not presented and described herein may be used. Furthermore, not all illustrated operations may be required to implement a technique in accordance with the disclosed subject matter.

As used herein, the terminology “driver” or “operator” may be used interchangeably. As used herein, the terminology “brake” or “decelerate” may be used interchangeably. As used herein, the terminology “computer” or “computing device” includes any unit, or combination of units, capable of performing any method, or any portion or portions thereof, disclosed herein.

As used herein, the terminology “example,” “embodiment,” “implementation,” “aspect,” “feature,” or “element” indicate serving as an example, instance, or illustration. Unless expressly indicated otherwise, any example, embodiment, implementation, aspect, feature, or element is independent of each other example, embodiment, implementation, aspect, feature, or element and may be used in combination with any other example, embodiment, implementation, aspect, feature, or element.

As used herein, the terminology “determine” and “identify,” or any variations thereof, includes selecting, ascertaining, computing, looking up, receiving, determining, establishing, obtaining, or otherwise identifying or determining in any manner whatsoever using one or more of the devices shown and described herein.