The present disclosure relates to systems and methods for path planning for autonomous vehicles. Specifically, the present disclosure relates to systems and method for path planning using occupancy grid maps.
An autonomous vehicle may use different sensors to sense its surrounding environment and vehicle operating parameters, a state estimation system to determine a state of the vehicle based on the sensed environment and vehicle operating parameters, a planning system to plan future vehicle actions based on target objectives and the vehicle state, a vehicle control system for translating the planned future action into control commands, and an electromechanical system for implementing the control commands. Of these systems, the planning system plays a crucial role in decision making, planning, and navigation.
The planning system conventionally includes a behavioral planner (BP) and a motion planner or motion planning module (MoP). Motion planning in autonomous vehicles, such as ground, aerial, surface, and underwater autonomous vehicles, is typically done by generating a trajectory based on state information for a controller to follow. Standard approaches use explicit cost functions as rules for ranking candidate trajectories for selection. The motion planning module is responsible for generating a safe, feasible, and smooth trajectory from the current state of the vehicle to a target state provided by upstream modules in the planning system (e.g. the BP). As an example, in the context of autonomous vehicles, the current state is the current position and orientation of the vehicle, and target state can be a center point within a lane a few meters ahead. The generated trajectory is then fed to a vehicle control system to control the vehicle to follow the given trajectory.
Information collected from the sensors is provided as sensor data by the various sensors to the planning system of the autonomous vehicle which uses the sensor data for path planning and navigation of the autonomous vehicle. In order to efficiently plan a path and safely navigate an autonomous vehicle in any environment, it is important to have information about the position of any objects (both static and moving objects) in the environment. Occupancy Grid Maps (OGMs) are commonly used to represent the environment surrounding an autonomous vehicles. Each OGM is generated based on sensor data received from the various different sensors included in or mounted on an autonomous vehicle.
For various reasons (e.g., sensor noise), sensor data from the various different sensors may include uncertainties or ambiguities, and the observations constructed based on these sensor data may not be fully clear. Therefore, the value contained in each cell is a value, typically between 0 and 1, that corresponds to how likely the cell is occupied by an object. The autonomous vehicle can use the OGM (e.g., as input for path planning) to determine a path for the autonomous vehicle to reach to a certain target or sweep a certain area inside the environment, for example. However, a single OGM may represent the environment at a given time (specifically, a time at which the observations were obtained by the sensors). Therefore, the path planning performed using the OGM may be valid only if it is assumed that the objects in the environment are static. This assumption does not hold for an environment in which objects are moving (referred to as “a dynamic environment”) and particularly in a dynamic environment where the moving objects do not necessarily follow a predefined trajectory (referred to as a “dynamic, unstructured environment”).
The environment in which an autonomous vehicle operates is usually a dynamic, unstructured environment. One way to cope with such an environment is to keep updating the current OGM each time sensor data is received from a sensor of the vehicle and then update the planned path accordingly. However, this approach will result in frequent modifications to the autonomous vehicles planned trajectory, which may be unsafe and/or may require many unnecessary steering corrections. Another approach is to enlarge representations of dynamical objects represented in the current OGM to account for their movement. In this approach, the cells surrounding objects are assumed to also be occupied, and path planning is performed based on the “enlarged” OGM. A difficulty with this approach is to how determine which of the cells surrounding objects are assumed to be occupied. Also, such an approach may not be suitable in a tight environment because otherwise free spaces that can be used would be blocked by the enlarged object representations in the OGM.
Efficient motion planning is a challenging task in autonomous vehicles. The complexity increases for dynamic environments. When the motion planning module is required to determine trajectories for an autonomous vehicle in such environments, the motion planning module, the motion planning module needs to perform a significant amount of computation when determining a suitable trajectory for the autonomous vehicle in such environments. A three-dimensional (3D) spatiotemporal planning method or a padding method may be utilized by a motion planning module to reduce the amount of computations required when determining a suitable trajectory for an autonomous vehicle. The basic idea behind the padding method is to expand the bounding boxes of moving objects to account for changes in the dynamic environment. Padding-based methods generally result in a conservative trajectory and may require velocity estimation for all of the objects present in the dynamic environment. In 3D spatiotemporal planning methods, a common approach is to augment a predicted environment at every timestamp to create a 3D spatiotemporal space. The trajectories are then generated in 3D space. Such methods typically require the planning system to handle a high computational complexity, which is required to perform time-constrained path planning in 3D space, dynamic (e.g., time-varying) optimization, and velocity-depended 3D space. Moreover, in these approaches, the path planning method often needs to be implemented and executed with 3D space generation methods to ensure casualty and feasibility, which limits the choice of path planning methods for dynamic environments.
There is a need for a method for path planning in autonomous vehicles that can efficiently address challenges in representing dynamic objects in a real-world environment for navigation of autonomous vehicles in such environments.
According to example aspects, the present disclosure provides methods and system for hierarchical planning in autonomous vehicles.
In accordance with one aspect, there is provided a processor-implemented method for generating a single predicted 2D occupancy grid map (OGM) for an autonomous vehicle is disclosed. The method includes the steps of: receiving a set of predicted occupancy grid map (OGMs), the set of predicted OGMs including a current predicted OGM and one or more future predicted OGMs, the current OGM associated with a current timestamp and each future predicted OGM associated with a future timestamp; generating a weight map associated with the current timestamp based on one or more kinodynamic parameters of the vehicle at the current time stamp, and one or more weight map associated with a future timestamp based on one or more kinodynamic parameters of the vehicle at the respective future timestamp; generating a set of filtered predicted OGMs by filtering the current predicted OGM with the weight map associated the current timestamp and filtering each respective future predicted OGM associated with a future timestamp with the weight map associated with the respective future timestamp; and sending a single predicted OGM representative of the set of filtered predicted OGMs to a trajectory generator, the single predicted OGM generated by combining the filtered predicted OGMs in the set of filtered OGMs.
In some aspects, filtering the respective predicted OGM in the set of predicted OGMs includes a Gaussian filtering operation or a Bayesian filtering operation.
In some aspects, the method includes generating a set of area of interest maps based on a behaviour command received from a behaviour planning module, the set of area of interest maps may include a current area of interest map associated with the current timestamp and one or more future area of interest maps, each future area of interest map associated with a respective future timestamp.
In some aspects, the method includes filtering the current predicted OGM associated the current timestamp with the current area of interest map and filtering each respective future predicted OGM associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, the method includes, prior to generating the set of filtered predicted OGMs, filtering the weight map associated with the current timestamp with the current area of interest map and filtering each respective weight map associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, the method includes filtering the filtered predicted OGM associated with the current timestamp with the current area of interest map and filtering each respective filtered predicted OGM associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, the method includes generating a trajectory based on the single predicted OGM.
In some aspects, each of the set of weight maps has a respective weight region associated with the corresponding timestamp, the weight region may be constructed to represent a maximum area the vehicle can reach with a margin of safety at the corresponding timestamp, and the weight region may be calculated based on a maximum feasible speed and acceleration of the vehicle.
In some aspects, the one or more kinodynamic parameters may include at least one of: a linear speed, an acceleration or deceleration, a travel direction, an angular acceleration, pitch, yaw, roll, vibration, an engine RPM, a throttle position, a brake position, a transmission gear ratio, a maximum rotation speed of the steering wheel of the vehicle, a kinematic model of the vehicle, a width of the vehicle, a length of the vehicle, and a minimum turning radius.
In accordance with another aspect, there is provided a system for generating a predictive occupancy grid map (OGM) for an autonomous vehicle is disclosed. The system includes a processing unit configured to execute instructions to: receive a set of predicted occupancy grid map (OGMs), the set of predicted OGMs including a current predicted OGM and one or more future predicted OGMs, the current OGM associated with a current timestamp and each future predicted OGM associated with a future timestamp; generate a weight map associated with the current timestamp based on one or more kinodynamic parameters of the vehicle at the current time stamp, and one or more weight map associated with a future timestamp based on one or more kinodynamic parameters of the vehicle at the respective future timestamp; generate a set of filtered predicted OGMs by filtering the current predicted OGM with the weight map associated the current timestamp and filtering each respective future predicted OGM associated with a future timestamp with the weight map associated with the respective future timestamp; and send a single predicted OGM representative of the set of filtered predicted OGMs to a trajectory generator, the single predicted OGM generated by combining the filtered predicted OGMs in the set of filtered OGMs.
In some aspects, filtering the respective predicted OGM in the set of predicted OGMs includes a Gaussian filtering operation or a Bayesian filtering operation.
In some aspects, the processing unit is configured to execute instructions to generate a set of area of interest maps based on a behaviour command received from a behaviour planning module, and the set of area of interest maps may include a current area of interest map associated with the current timestamp and one or more future area of interest maps, and each future area of interest map may be associated with a respective future timestamp.
In some aspects, the processing unit is configured to execute instructions to: filter the current predicted OGM with the current area of interest map associated the current timestamp and filter each respective future predicted OGM associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, the processing unit is configured to execute instructions to: prior to generation of the set of filtered predicted OGMs, filter the weight map associated with the current timestamp with the current area of interest map associated the current timestamp and filter each respective weight map associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some embodiments, the processing unit is configured to execute instructions to: filter the filtered predicted OGM associated with the current timestamp with the current area of interest map and filter each respective filtered predicted OGM associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some embodiments, each of the set of weight maps has a respective weight region associated with the corresponding timestamp, the weight region may be constructed to represent a maximum area the vehicle can reach with a margin of safety at the corresponding timestamp, and the weight region may be calculated based on a maximum feasible speed and acceleration of the vehicle.
In some embodiments, the one or more kinodynamic parameters may include at least one of: a linear speed, an acceleration or deceleration, a travel direction, an angular acceleration, pitch, yaw, roll, vibration, an engine RPM, a throttle position, a brake position, a transmission gear ratio, a maximum rotation speed of the steering wheel of the vehicle, a kinematic model of the vehicle, a width of the vehicle, a length of the vehicle, and a minimum turning radius.
In accordance with another aspect, there is provided a non-transitory computer-readable medium storing instructions is provided. The instructions, when executed by a processor of an autonomous vehicle, cause the processor of the vehicle to: receive a set of predicted occupancy grid map (OGMs), the set of predicted OGMs including a current predicted OGM and one or more future predicted OGMs, the current OGM associated with a current timestamp and each future predicted OGM associated with a future timestamp; generate a weight map associated with the current timestamp based on one or more kinodynamic parameters of the vehicle at the current time stamp, and one or more weight map associated with a future timestamp based on one or more kinodynamic parameters of the vehicle at the respective future timestamp; generate a set of filtered predicted OGMs by filtering the current predicted OGM with the weight map associated the current timestamp and filtering each respective future predicted OGM associated with a future timestamp with the weight map associated with the respective future timestamp; and send a single predicted OGM representative of the set of filtered predicted OGMs to a trajectory generator, the single predicted OGM generated by combining the filtered predicted OGMs in the set of filtered OGMs.
In some aspects, the instructions, when executed by a processor of an autonomous vehicle, may cause the processor of the vehicle to generate a set of area of interest maps based on a behaviour command received from a behaviour planning module, the set of area of interest maps may include a current area of interest map associated with the current timestamp and one or more future area of interest maps, each future area of interest map may be associated with a respective future timestamp.
In some aspects, the instructions, which when executed by a processor of an autonomous vehicle, may cause the processor of the vehicle to: filter the current predicted OGM associated the current timestamp with the current area of interest map and filter each respective future predicted OGM associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, the instructions, which when executed by a processor of an autonomous vehicle, may cause the processor of the vehicle to: filter the weight map associated with the current timestamp with the current area of interest map and filter each respective weight map associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, the instructions, which when executed by a processor of an autonomous vehicle, may cause the processor of the vehicle to: filter the filtered predicted OGM associated with the current timestamp with the current area of interest map and filter each respective filtered predicted OGM associated with a future timestamp with the area of interest map associated with the respective future timestamp.
In some aspects, each of the set of weight maps has a respective weight region associated with the corresponding timestamp, the weight region being constructed to represent a maximum area the vehicle can reach with a margin of safety at the corresponding timestamp, and the weight region is calculated based on a maximum feasible speed and acceleration of the vehicle.
Reference will now be made, by way of example, to the accompanying drawings which show example embodiments of the present application, and in which:
Similar reference numerals may have been used in different figures to denote similar components.
Some examples of the present disclosure are described in the context of autonomous vehicles. Although examples described herein may refer to a car as the autonomous vehicle, the teachings of the present disclosure may be implemented in other forms of autonomous or semi-autonomous vehicles including, for example, trams, subways, trucks, buses, surface and submersible watercraft and ships, aircraft, drones (also referred to as unmanned aerial vehicles (UAVs)), warehouse equipment, manufacturing facility equipment, construction equipment, farm equipment, autonomous service robots such as vacuum cleaners and lawn mowers, and other robotic devices. Autonomous vehicles may include vehicles that do not carry passengers as well as vehicles that do carry passengers.
Autonomous navigation for a vehicle such as an autonomous vehicle operating in a dynamic environment requires at least two steps: a) predicting the movements of dynamic and static objects; and b) generating collision-free trajectory or trajectories for the vehicle based on the predicted movements. Predicting movements of dynamic objects in an environment are often represented using a set of predicted occupancy grid maps (OGMs), which are generated by predicting future changes to a current OGM, over time.
The vehicle 100 includes a sensor system 110, a state estimation system 120, a planning system 130, a vehicle control system 140 and an electromechanical system 150, for example. Other systems and components may be included in the vehicle 100 as appropriate. The state estimation system 120, the planning system 130, and the vehicle control system 140 in this example are distinct software systems that include machine readable instructions may be executed by one or processors of a processing system of the vehicle. Alternatively, the state estimation system 120, the planning system 130, and the vehicle control system 140 may be distinct systems on chip (e.g., application-specific integrated circuit (ASIC), field-programmable gate array (FGPA), and/or other type of chip). For example, the state estimation system 120, the planning system 130, and the vehicle control system 140 may be implemented using one chip, two chips, or three distinct chips (using the same or different types of chips). Various systems and components of the vehicle may communicate with each other, for example through wired or wireless communication. For example, the sensor system 110 may communicate with the state calculation system 120, the planning system 130 and the vehicle control system 140; state estimation system 120 may communicate with the planning system 130 and the vehicle control system 140; the planning system 130 may communicate with the vehicle control system 140; and the vehicle control system 140 may communicate with the electromechanical system 150.
Referring again to
The sensor system 110 includes various sensing units, such as a radar unit 112, a LIDAR unit 114, and a camera 116, for collecting information about an environment surrounding the vehicle 100 as the vehicle 100 operates in the environment. The sensor system 110 also includes a global positioning system (GPS) unit 118 for collecting information about a location of the vehicle in the environment.
The sensor system 110 provides information collected about an environment surrounding the vehicle 100 and the information collected about the location of the vehicle 100 as sensor data to the state estimation system 120. The state estimation system 120 receives the sensor data and generates an estimated state of the vehicle based on the sensor data, which is an estimate of a current state of the vehicle. In example embodiments, the estimated vehicle state includes information including a vehicle location state, a vehicle environment state, and a vehicle operational state. Vehicle location state describes a position of the vehicle and may for example include absolute geographical longitude/latitudinal values and/or values that reference other frames of reference. Vehicle environment state describes the surrounding environment of the vehicle, including for example stationary and moving objects around the vehicle, weather and temperature conditions, road conditions, road configuration and other information about the physical environment that the vehicle operates in. Vehicle operational state describes physical operating conditions of the vehicle itself, including for example kinodynamic parameters such as linear speed and acceleration, travel direction, angular acceleration, pose (e.g. pitch, yaw, roll), and vibration, and mechanical system operating parameters such as engine RPM, throttle position, brake position, and transmission gear ratio, among other things. Generally, the term kinodynamics relates to a class of problems, in robotics and motion planning, in which velocity, acceleration, force/torque bounds must be satisfied and where kinematics constraints (e.g., obstacle avoidance) must also be satisfied. Kinodynamic parameters are those parameters, such as described above, that are relevant to this type of motion planning problem. In some embodiments, kinodynamic parameters may include one or more of: a maximum acceleration, a maximum deceleration, a maximum rotation speed of the steering wheel of the vehicle 100, a kinematic model of the vehicle 100, a width or length of the vehicle 100, a minimum turning radius, and so on.
In this regard, the sensor system 110 may also include vehicle sensors 119. Vehicle sensors 119 may include sensors for collecting information about the physical operating conditions of the vehicle 100, including for example sensors for sensing steering angle, linear speed, linear and angular acceleration, pose (pitch, yaw, roll), compass travel direction, vehicle vibration, throttle state, brake state, wheel traction, transmission gear ratio, cabin temperature and pressure, as well as external environment sensors for sensing things such as an external temperature and pressure, precipitation, and noise, among other possibilities. Vehicle sensors 119 provide the collected information about the physical operating conditions of the vehicle 100 as sensor data to the state estimation system 120, and may be used by the state estimation system 120 when generating an estimated state of the vehicle based on the sensor data.
The sensor system 110 also includes a map 117, which may be a reference map that represents known information about the surrounding environment. For example, the map 117 may be received from an external reference database (e.g., retrieved based on the vehicle's position, as determined using the GPS unit 118).
The state estimation system 120 receives sensor data from the sensor system 110 and uses the sensor data to generate the estimated vehicle state S={Sl,Se,So}, which as noted above includes a vehicle location state Sl, vehicle environment state Se, and vehicle operational state So. For example, sensor data received from the radar, LIDAR and camera units 112, 114, 116 and sensor data received from other sensors may be used to determine the local environment of the vehicle 100 (e.g., any immediately surrounding obstacles) as well as information from a wider vicinity (e.g., the radar unit 112 and LIDAR unit 114 may collect information from an area of up to 100 m radius or more around the vehicle 100). Sensor data from GPS unit 118 and other sensors may be used to determine vehicle location state, defining a geographic position of the vehicle 100. Sensor data from vehicle sensors 119 and GPS unit 118, as well as from other sensor units, may be used to determine vehicle operational state So, including speed and pose of the vehicle 100 relative to a frame of reference.
Estimated vehicle state S={Sl,Se,So} output from the state estimation system 120 based on sensor data received from the sensor system 110 is provided in real-time to the planning system 130, which is the focus of the current disclosure and will be described in greater detail below. The vehicle control system 140 serves to control operation of the vehicle 100 based on target objectives set by the planning system 130. The vehicle control system 140 may be used to provide full, partial or assistive control of the vehicle 100. The electromechanical system 150 receives control signals from the vehicle control system 140 to operate the electromechanical components of the vehicle 100 such as an engine, transmission, steering system and braking system.
The state estimation system 120, planning system 130 and the vehicle control system 140 may be implemented as software that includes machine readable instructions that are executable by one or more processing units of a processing system of the vehicle 100. By way of example,
Autonomous navigation for cars from point A to point B may include many driving sub-tasks and conditions that require different considerations such as: conformance to traffic rules; navigation in structured and unstructured roads; navigation in different types of roads; handling dynamic and static obstacles; dealing with different weather conditions, and so on. These myriad tasks and considerations can make it difficult to design an end-to-end planning system that generates a driving plan that deals with all types of scenarios, environments, and conditions. To this end, in example embodiments, planning system 130 is configured to provide a modular and extensible system that can deal with different driving sub-tasks and conditions, and is enabled to abstract road and driving area complexities to a more general state-action representation.
In the example of
The OGM prediction module 340 may be implemented in different ways. Other systems and components may be included in the vehicle 100 as appropriate. Various systems and components of the vehicle may communicate with each other, for example through wired or wireless communication. For example, the sensor system 110 may communicate with the OGM prediction module 340, path planning system 130 and the vehicle control system 140; the OGM prediction module 340 may communicate with the path planning system 130 and the vehicle control system 140; the path planning system 130 may communicate with the vehicle control system 140; and the vehicle control system 140 may communicate with the electromechanical system 150.
Occupancy grid maps (OGMs) are commonly used to represent the environment surrounding the vehicle 100. An OGM may be represented as a grid cell. Each cell represents a physical space in the environment, and each cell contains a value representing the probability of each cell being occupied by an object, based on information or observations regarding the vehicle 100 and its surrounding environment. For example, the path planning system 130 may include a module, such as the OGM prediction module 340, for generating a predicted OGM. Alternatively, the module for generating predicted OGMS may be included in processing system coupled to the path planning system 130.
The OGM prediction module 340 is configured to receive an input OGM (e.g., a current observed OGM), and to generate a set of predicted OGMS Mt={Mt0, Mt1, . . . Mtn} based on the input OGM. Each predicted OGM in the set of predicted OGMs {Mt0, Mt1, . . . Mtn} is an OGM associated with the vehicle 100 predicted for a specific point in time t. For example, predicted OGM Mt0 represents a predicted OGM generated for t=t0, where t0 is the present. Predicted OPM Mt1 represents an OGM predicted for t=t1, where t1 represents a second after t0, and so on. The OGM prediction module 340 is configured to generate a set of predicted OGMs Mt based on a current observed OGM. In some cases, an OGM can be generated based on sensor data collected from different sensors on an autonomous vehicle. For example, sensor data from the radar, LIDAR and camera units 112, 114, 116, GPS unit 118 and other sensors may be used to generate a current observed OGM from which the OGM prediction module 340 generates a set of predicted OGMs Mt={Mt0, Mt1, . . . Mtn}. Estimated vehicle state S may also be used to generate the current observed OGM. In some cases, a current observed OGM can be generated based on one or more states generated by a state estimation system 120. The state estimation system 120 can generate the one or more states based on the sensor data collected from different sensors on an autonomous vehicle.
The software of the OGM prediction module 340 and MoP module 330 may be executed using a using the one or more processing units of the processing system 200 of the vehicle 100. The OGM prediction module 340 may repeatedly (e.g., in regular intervals) receive a current observed OGM generated from sensor data received from the sensor system 110 and generate a set of predicted OGMs from the current observed OGM in real-time or near real-time. The OGM prediction module 340 may also transmit the set of predicted OGMs to an OGM processor module 350 in the MoP module 330 for generating a final predicted OGM and a trajectory.
Data from the sensor system 110, estimated vehicle state S from the state estimation system 120, input behavior command B from the BP module 320, and a set of predicted OGMs Mt from the OGM prediction module 340 may be provided to the MoP module 330. The MoP module 330 carries out path planning for the vehicle 100. For example, the MoP module 330 may plan a path or trajectory T for the vehicle 100 to travel from a starting point to a target destination. The MoP module 330 may be implemented as one or more software modules or control blocks comprising machine-readable instruction that are executed the one or more processing units of the processing system 200 of the vehicle 100. The output from the MoP module 330 may include data defining one or more planned paths for the vehicle 100 to travel. The path planning carried out by the planning system 130 is performed in real-time or near real-time, to enable the vehicle 100 to be responsive to real-time changes in the sensed environment. Output from the planning system 130 may be a trajectory T provided to the vehicle control system 140. In particular, MoP Module 330 plans and outputs the selected trajectory T required to implement the behavior command B in view of the estimated vehicle state S. The selected trajectory T is then used by the vehicle control system 140 to control vehicle operation by sending commands to the electromechanical system to implement the behavior, resulting in a new vehicle state, for which a new estimated state S is generated (using new sensed data) and fed back to the planning system 130.
As indicated in
The OGM processor module 350 is configured to merge the set of predicted OGMs Mt into a final 2D predicted OGM Mtf, which is then used by the trajectory generator module 360 to generate one or more trajectories T for the vehicle 100. The method and system of the present disclosure lowers the computational complexity by reducing three-dimensional (3D) and time-constrained path planning problem to an efficient 2D path planning problem. In addition, because the generation of a final predicted 2D OGM Mtf by the OGM processor module 350 is independent from the path planning algorithm implemented by the trajectory generator module 360, generation of the final predicted 2D OGM does not impose any limitations which path planning algorithm is utilized by the trajectory generator module 360.
In example embodiments, the OGM processor module 350 includes at least three sub-modules, namely, an OGM merger module 351, a weight map module 353, and an area of interest module 355. The OGM processor module 350 generates the final 2D OGM Mtf by filtering or updating the given set of predicted OGMs Mt based on a number of factors including one or more of: kinodynamic parameters of the vehicle 100, a behavior command B, various sensor data, estimated vehicle state S, and one or more trajectories T generated by the trajectory generator 360.
As shown in
In addition, the OGM merger module 351 receives a set of weight maps Wt={Wt0, Wt1, . . . , Wtn} from a weight map module 353. Each weight map Wtx is associated with a corresponding timestamp tx. As shown in
In some example embodiments, each of the weight map Wtx 415a, 415b, 415c is determined by three variables {x, y, p}, where x and y may be values based on coordinates of a vehicle 100 on a map, and p may be one or more variables, such as velocity, acceleration, jerk, curvature, orientation, and so on. In some cases, one or more generated trajectories T may be received from the trajectory generator 360 and used to calculate or refine a weight map.
In some embodiments, weight maps can be generated based on for example {x, y, v} or {x, y, v, a}, where v and a each represents the velocity and acceleration of the vehicle, respectively. In some embodiments, a trajectory may be used to generate the weight map Wtx {x, y, τ, ϵ}, where τ is the trajectory, and E is the trajectory following error distribution of the vehicle control system 140. The trajectory τ may be for example a generated trajectory T from the trajectory generator 360. A trajectory (i.e., path) τ contains target points with target velocity, acceleration, jerk, orientation, and curvature at each point. The vehicle control system 140 may control the vehicle 100 to follow the trajectory τ with some level of accuracy. Therefore, the trajectory τ can be used t0, based on some probability of controller accuracy, calculate the whereabouts of the vehicle over time, which can directly be used in generating weight maps. For example, in discrete weight maps, the trajectory can be used to find the boundaries of weight map Wtx for each timestamp tx.
At t=t1, xt1 and yt1 may be determined based on an updated set of coordinates (a, b), where a represents the furthest possible point along x-axis to which the vehicle 100 can travel at time t1 based on the maximum feasible velocity v, and b represents the furthest possible point along y-axis to which the vehicle 100 can travel at time t1 based on the maximum feasible velocity v. xt1 is the distance between xt0 and a, and yt1 is the distance between yt0 and b. A weight map Wt1 is then calculated based on rt1 at time t1, using the equation r2=x2+y2. Weight map Wt1 has a geographic weight region 470b which represents an area the vehicle 100 can possibly reach (or occupy) at time t=t1. As seen in
Similarly, Wt2, Wt3 . . . Wtn can be calculated at a respective timestamp tx, where x=2, 3, . . . n. The maximum feasible speed v can be obtained from the sensor system 110.
In some embodiments, the weight maps Wtx 415a, 415b, 415c are calculated at discrete points in time such as at t=t0, t1, and t2. In other embodiments, the weight maps may be calculated continuously and represented as a function of time t. One example is to extend
Let v be the current speed of the vehicle. The weight map is defined as follows:
W(t,x,y,v)=c0e−c
Where tk, k={0, 1, 2, . . . } are timestamps associated with the nominal (predefined) time horizon of predicted OGMs. The benefits of this is that if there are any latency in predicted OGMs, W(t,x,y,v) can be sampled based on the timestamp of the OGMs, and the sampled weight maps can be applied to its corresponding OGM. For example, the OGM prediction module 340 may be designed to generate predictive OGMs for tk={0.0,1.0,2.0,3.0}, but in practice the OGM processor module 350 may receive the OGMs with some delays due to communication latency or delay in processing in modules 340. In this case, the OGM processor module 350 may receive predictive OGMs with tk={0.1,1.05,2.2,3.15} timestamps, which can be used to sample W (t,x,y,v) and those samples can be applied to their corresponding OGM.
Referring back to
As shown in
An area of interest map 610 is a map that is determined based on at least a behavior decision or command B from the BP module 320. For example, if the behavior command B for the vehicle 100 is to switch to the left lane, the area of interest map 610 may have an area of interest region 612 representing an area into which the vehicle 100 is likely going, as shown in
In some example embodiments, the area of interest maps At={At0, At1, At2} 610 can first be applied to filter the set of predicted OGMs Mtx 410a, 410b, 410c, to increase the relative importance of occupied cells within the area of interest maps At={At0, At1, At2}. Then, weight maps Wt={Wt0, Wt1, Wt2} 415a, 415b, 415c are applied to the resulting filtered predicted OGMs, which are added together to generate a final predicted 2D OGM Mtf 450. The area of interest map Atx may differ from one task to another. For example, for a switch lane task, more weights can be assigned to cells on the adjacent lane that the vehicle is about to switch into. For another example, for a keep lane task, more weights can be assigned to the current lane.
Once generated by OGM merger 351, the final predicted 2D OGM Mtf 450 is then transmitted by the OGM processor module 350 to the trajectory generator 360, which generates or retrieves a feasible and collision-free trajectory T based on the final OGM Mtf 450. The trajectory generator 360 can use a number of methods to generate the final trajectory T. For example, the trajectory generator 360 can generate a set of pre-generated candidate trajectories based on the behavior command B and kinodynamic parameters of the vehicle 100, and select, from among the candidate trajectories, a single specific trajectory T evaluated to be good or otherwise appropriate for execution. For another example, the trajectory generator 360 can generate a single trajectory by searching over the final, single 2D predicted OGM Mtf 450. Some example OGM-based techniques to generate trajectory includes: skeleton method, A*, hybrid A*, RRT, PRM, DRT, Roadmaps, Voronoi diagrams, and so on. The resulting 2D OGM can also be used in non-OGM based methods for collision checks and filtering trajectories in sampling-based motion planning methods.
There are a number of advantages associated with the proposed methods as performed by OGM processor module 350. By processing multiple 3D predicted OGMs, which are associated with different timestamps including future timestamps, into a single 2D predicted OGM, the path planning method is a predictive 2D path planning method rather than a 3D path method, which is significantly less computational complex. The proposed OGM processor module 350 is capable of handling dynamic environments in an efficient and real-time manner, which has long been a challenge faced by traditional path planning methods. In addition, by using a single 2D predicted OGM to generate a trajectory, it is now possible to achieve a constant-time collision check as the processing time for generating the single trajectory does not depend on the number of objects present on the scene, and no further motion prediction for objects is required. In comparison, traditional methods for generating a path trajectory typically need to loop over objects and the associated processing time for generating the path trajectory is proportional to the number of objects in the scene, and thus the processing time in these traditional methods tends to be non-constant.
By decoupling OGM-processing from trajectory generation, the OGM processor module 350 is largely agnostic to the specific method used for path planning and trajectory generation. As such, it is now possible for the trajectory generator 360 to use any suitable path planning method(s) for trajectory generation, without having to depend on the OGM processor module 350 for every step along the way to generate a final trajectory.
The use of a predicted OGM such as the final 2D predicted OGM Mtf 450 provides a higher degree of reliability for path planning in dynamic environments, compared to padding-based approaches, as the estimated whereabouts and information of dynamic objects can be used in both trajectory generation and collision-check processes. Since a predicted OGM takes potential movements of dynamic objects into consideration, it provides a higher degree of robustness in path planning, compared to other methods that require regular and rapid re-planning in dynamic environments.
Some specific planning examples will now be described to facilitate an understating of possible applications of the planning system 130 and in particular how the OGM processor module 350 may improve system operation. One intended application of the planning system 130 is in multi-lane driving for driving in structured and lane-based scenarios such as highways or urban roads, where change lane and keep lane behaviors are expected to reach to some target goals determined by a mission planner.
Next, the pre-summation OGMs 675a, 675b, 675c are processed in a summation operation to generate a final predicted 2D OGM 650, which is then sent to the trajectory generator module 360 to generate a final predictive trajectory 695. Comparing the predictive trajectory 695 to the non-predictive trajectory 690, it is readily clear that trajectory 695 is safer and leaves room for object 680a to move ahead before merging into traffic.
One advantage of this embodiment is that the path planning system 130 of generates a safe trajectory when the vehicle 100 is operating in a multi-lane highway and the vehicle 100 is required to merge into another lane because the lane the vehicle 100 is operating in ends. This embodiment does not require 3D spatiotemporal path planning or conservative padding to deal with dynamic vehicles/obstacles in the current lane the vehicle 100 is operating in and adjacent lanes to the current lane. Also, the path planning system 130 can re-plan the trajectory less frequently because the estimated future location of other vehicles or objects are reflected in the final predicted 2D OGM 650.
Each respective final predicted 2D OGM 750, 760 is used to generate a respective trajectory 755, 765 for the left-turn behavior. Depending on the final predicted 2D OGM 750, 760, the trajectory generator module 360 may choose a shorter trajectory 755 to stop at the middle of road to give yield to the incoming traffic (top row in
This example provides an efficient and non-conservative approach to deal with left turn scenario. The OGM merger module 351 generates a final predicted 2D OGM, which is used by the trajectory generator illustrates that the path planning system 130 generates a trajectory 360 of the path planning system 130 to generate a trajectory that results in smoother and safer driving.
A weight map definition has the flexibility of being realized through a) exclusive sub-zones (
The one or more kinodynamic parameters can include at least one of: a linear speed, an acceleration or deceleration, a travel direction, an angular acceleration, pitch, yaw, roll, vibration, an engine RPM, a throttle position, a brake position, a transmission gear ratio, a maximum rotation speed of the steering wheel of the vehicle, a kinematic model of the vehicle, a width of the vehicle, a length of the vehicle, and a minimum turning radius.
The generated trajectory T may be outputted to control the vehicle 100. For example, the generated trajectory T may be provided to the vehicle control system 140, which in turn outputs control commands to the electromechanical system 150, to cause the vehicle 100 to execute the generated trajectory T. In some examples, the method 1000 may include the action to execute the generated trajectory T.
Although the present disclosure describes methods and processes with steps in a certain order, one or more steps of the methods and processes may be omitted or altered as appropriate. One or more steps may take place in an order other than that in which they are described, as appropriate.
Although the present disclosure is described, at least in part, in terms of methods, a person of ordinary skill in the art will understand that the present disclosure is also directed to the various components for performing at least some of the aspects and features of the described methods, be it by way of hardware components, software or any combination of the two. Accordingly, the technical solution of the present disclosure may be embodied in the form of a software product. A suitable software product may be stored in a pre-recorded storage device or other similar non-volatile or non-transitory computer readable medium, including DVDs, CD-ROMs, USB flash disk, a removable hard disk, or other storage media, for example. The software product includes instructions tangibly stored thereon that enable a processing device (e.g., a personal computer, a server, or a network device) to execute examples of the methods disclosed herein.
The present disclosure may be embodied in other specific forms without departing from the subject matter of the claims. The described example embodiments are to be considered in all respects as being only illustrative and not restrictive. Selected features from one or more of the above-described embodiments may be combined to create alternative embodiments not explicitly described, features suitable for such combinations being understood within the scope of this disclosure.
All values and sub-ranges within disclosed ranges are also disclosed. Also, although the systems, devices and processes disclosed and shown herein may comprise a specific number of elements/components, the systems, devices and assemblies could be modified to include additional or fewer of such elements/components. For example, although any of the elements/components disclosed may be referenced as being singular, the embodiments disclosed herein could be modified to include a plurality of such elements/components. The subject matter described herein intends to cover and embrace all suitable changes in technology.