Motion planning using a spatiotemporal convex corridor

Abstract

The present invention provides a system, method and computer readable medium for path planning for an autonomous vehicle. The method comprises: receiving data, wherein the data defines an upper bound of a spatiotemporal convex corridor and a lower bound of the spatiotemporal convex corridor; calculating a plurality of control point times within a corridor start time and a corridor end time, wherein the plurality of control point times comprises at least a start control point time, an end control point time, and an intermediate control point time between the start control point time and the end control point time; determining a plurality of control point maximums and a plurality of control point minimums for the plurality of control point times; calculating a plurality of control point values for the plurality of control point times from the plurality of control point maximums and the plurality of control point minimums, wherein the plurality of control point values define a curve segment within the upper bound and the lower bound of the spatiotemporal convex corridor.

Description

Technical Field

[0001] This invention relates to systems, methods, and computer-readable media for motion planning using spatiotemporal convex corridors. Background Technology

[0002] An autonomous vehicle (e.g., an autonomous car) is a vehicle that includes various types of sensors and is capable of operating safely and autonomously without human intervention. These sensors are used to sense the vehicle's surrounding environment (e.g., the presence and state of stationary and moving objects near the vehicle) and vehicle operating parameters (e.g., speed, acceleration, pose, etc.). Autonomous vehicles typically include various software systems for perception and prediction, localization and mapping, and planning and control. The planning software system (often called the planning system) plans the vehicle's trajectory based on the target destination, the vehicle's surrounding environment, and the vehicle's physical parameters (e.g., wheelbase, width, length, etc.). The vehicle control software system (e.g., the vehicle control system) receives the trajectory from the planning system and generates control commands to control the vehicle to follow the trajectory.

[0003] A planning system can include multiple planners (also called planning units, planning subsystems, planning modules, etc.) arranged in a hierarchical structure. A planning system typically includes a task planner, a behavior planner, and a motion planner. The motion planner receives behavioral decisions generated by the behavior planner for the autonomous vehicle, vehicle state information (including sensed environmental data and vehicle operation data), and road network information where the vehicle is traveling as input, and performs motion planning to generate the trajectory of the autonomous vehicle. In this invention, the trajectory includes a sequence of the autonomous vehicle's position in a spatiotemporal coordinate system across multiple time steps. Other parameters can be associated with the trajectory, including vehicle orientation, vehicle speed, vehicle acceleration, vehicle bumps, or any combination thereof.

[0004] Motion planning systems generate trajectories that meet criteria such as safety, comfort, and mobility within a spatiotemporal search space corresponding to vehicle states, behavioral decisions, and the road network in which the vehicle travels. Motion planning systems may face challenges in achieving safe local motion planning for autonomous vehicles when temporally dependent constraints are present. These temporally dependent constraints are typically generated by dynamic objects such as other vehicles, pedestrians, and cyclists, which may alter the available spatiotemporal search space over time. More broadly, any changes in the surrounding environment (due to moving objects, changes in traffic light states, etc.) and / or changes in behavioral decisions can lead to temporally dependent constraints. To formulate this problem as an optimization problem, a common approach is to decompose the available search space into multiple time-spanning corridors. Each corridor defines an available range within the spatiotemporal search space. Successive corridors overlap at some point in time to guarantee the existence of a solution. Considering these corridors, the motion planning problem simplifies to finding the trajectory segments for each corridor and connecting these segments at each overlap point, thereby generating a continuous trajectory from the vehicle's initial position to its target position within the available search space. The vehicle's initial position can be its current position, a point on the trajectory at a previous time, or a point that is far away from the current position.

[0005] For example, Figure 1 An exemplary driving scenario on a road is illustrated. Dashed areas represent occupied or non-drivable areas 11 (e.g., outside road boundary areas), drivable areas are marked by outlined diamond patterns 13, and corridors are shown by white rectangles 12. For simplicity, this scenario is assumed to be static (all objects are stationary). Therefore, the time axis is not considered in this particular scenario, and the search space is projected onto the SL plane, where S and L refer to spatial coordinates in the Frenet coordinate system. The S coordinate is the longitudinal distance along the road tangent vector. The L coordinate is the lateral distance along the road normal vector to the road. The origin of the Frenet coordinate system is a moving reference, typically placed on the centerline of the driving area (e.g., the road or a lane on the road) at the point closest to the vehicle. The position of the origin 15 varies depending on the vehicle's position in each planning cycle. Figure 1 In the scenario, trajectory segments 16 are generated within each corridor rectangle 12. The first trajectory segment 16 starts from the vehicle's current position 15 and ends at a point on the overlapping edge between the first (current) corridor and the second (next) corridor. This process is repeated for each corridor to obtain a trajectory from the starting position 14 to the target position 17. This process is similar in dynamic environments, except that the search space is a 3D SLT space (where T refers to the time coordinate), while... Figure 1In the static environment shown, the search space is a 2D SLT space. To handle planning in dynamic environments, existing methods project the SLT search space onto the ST and LT planes. A common practice is to decompose the drivable area / free space (hereinafter referred to as the available area) into rectangular ST and / or LT corridors (cubic corridors in SLT space). Since the rectangular corridors only cover a portion of the available area, the resulting trajectory is always a suboptimal solution. That is, if each corridor covered a larger portion of the available area, a more optimized trajectory might be found. To address this, Li, Jialun et al., in their paper "Speed ​​Planning Using Bezier Polynomials with Trapezoidal Corridors" published in arXiv preprint: 2104.11655 (2021), describe a state-of-the-art method using trapezoidal corridors, with a static example as shown below. Figure 2A The projection shown is on the SL plane. Compared to rectangular corridors, trapezoidal corridors, while still having limitations, may cover a larger area in some driving scenarios. Another approach is to maximize coverage by considering smaller corridors and increasing the number of corridors. Figure 2B An example illustrates this approach. However, increasing the number of corridors can negatively impact optimization execution time because it increases the number of trajectory segments and may result in a non-real-time solution. Furthermore, given the simple shape (geometry) of the corridors, there is no guarantee that increasing the number of rectangular or trapezoidal corridors will expand the search space coverage.

[0006] Figure 2A The trapezoidal corridor in the image is an example of Bézier curve trajectory generation. Figure 2A In the example, multiple control points 20 are drawn within the safety corridor (corridor 21), and a Bézier curve 22 is generated based on these points 20. Due to the "convex hull property" of the Bézier curve, curve 22 does not extend beyond the outer boundary of corridor 21. Note that both axes (SL) are spatial dimensions. However, in scenarios with temporal dependencies, i.e., when planning in spatiotemporal space (e.g., ST and LT spatiotemporal planes), calculating Bézier curves within the safety corridor can present problems.

[0007] Therefore, there is a need for systems and methods capable of generating curved trajectories within safe corridors to optimize the available search space for trajectories. Summary of the Invention

[0008] According to a first exemplary aspect of the present invention, a computer implementation method for motion planning of an autonomous vehicle is provided. The method includes: receiving data, wherein the data (i) defines an upper bound of a first dimension of a spatiotemporal convex corridor in a first spatial dimension as a concave function of time between a corridor start time and a corridor end time, and (ii) defines a lower bound of the spatiotemporal convex corridor in the first spatial dimension as a convex function of time between the corridor start time and the corridor end time, the first spatial dimension corresponding to a first spatiotemporal plane; calculating a plurality of control point times within the corridor start time and the corridor end time, wherein the plurality of control point times includes at least a start control point time, an end control point time, and an intermediate control point time between the start control point time and the end control point time; and calculating the plurality of control point times for the... For each control point time among the multiple control point times, a first control point maximum value corresponding to the value of the first dimension upper bound at that control point time is determined; a first control point minimum value corresponding to the value of the first dimension lower bound at that control point time is determined for each control point time among the multiple control point times; based on the respective first control point maximum value and the respective first control point minimum value, a first dimension control point value is calculated for each control point time among the multiple control point times, wherein the first dimension control point value is limited to a first curve segment within the first dimension upper bound and the first dimension lower bound of the spatiotemporal convex corridor; based on the first curve segment, a planned trajectory for controlling the operation of the autonomous vehicle is output.

[0009] In at least some planning applications, using more complex spatiotemporal corridor shapes can expand the search space coverage provided by the corridors. This can minimize the computational costs required to find a trajectory solution (e.g., optimizing CPU usage, execution time, and power consumption), and can also make the solution potentially more efficient and comfortable from the perspective of vehicle operation by using trajectories that require lower acceleration and / or bumps.

[0010] In some exemplary aspects, the calculation of the first dimension control point value is for the purpose of uniformly optimizing a first defined cost function for the first curve segment passing through the spatiotemporal convex corridor on the first spatiotemporal plane.

[0011] In one or more of the above aspects, the first spatial dimension corresponds to one of the following: (i) the longitudinal distance of the tangent vector along the road and (ii) the lateral distance of the normal vector along the road to the road.

[0012] In one or more of the above aspects, the first spatial dimension corresponds to the distance axis in a three-dimensional Cartesian coordinate system with time as the other axis.

[0013] In one or more of the above aspects, the received data further (iii) defines the upper bound of the second dimension of the spatiotemporal convex corridor in the second spatial dimension as a concave function of time between the start time and the end time of the corridor, and (iv) defines the lower bound of the second dimension of the spatiotemporal convex corridor in the second spatial dimension as a convex function of time between the start time and the end time of the corridor, wherein the second spatial dimension corresponds in time to a second spatiotemporal plane that coincides with the first spatiotemporal plane. The method further includes: determining a respective second control point maximum value corresponding to the value of the second dimension upper bound at the control point time for each of the plurality of control point times; determining a respective second control point minimum value corresponding to the value of the second dimension lower bound at the control point time for each of the plurality of control point times; calculating a respective second dimension control point value for each of the plurality of control point times based on the respective second control point maximum value and the respective second control point minimum value, wherein the second dimension control point value is limited to a second curve segment within the second dimension upper bound and the second dimension lower bound of the spatiotemporal convex corridor; and outputting the planned trajectory based on the first curve segment and the second curve segment.

[0014] In one or more of the above aspects, calculating the second dimension control point value is for the purpose of uniformly optimizing the second defined cost function of the second curve segment passing through the spatiotemporal convex corridor on the second spatiotemporal plane.

[0015] In one or more of the above aspects, the first spatial dimension corresponds to the longitudinal distance of the tangent vector along the road, and the second spatial dimension corresponds to the lateral distance of the normal vector along the road to the road.

[0016] In one or more of the above aspects, the first spatial dimension and the second spatial dimension correspond to the first distance axis and the second distance axis in a three-dimensional Cartesian coordinate system with time as the third axis, respectively.

[0017] In one or more of the above aspects, each first curve segment is an nth-order parameterized curve, and the plurality of control point times include n+1 control point times that are evenly spaced in time.

[0018] In one or more of the above aspects, each first curve segment is an nth-order Bezier curve.

[0019] In one or more of the above aspects, each first curve segment is an nth-order B-spline curve.

[0020] In one or more of the above aspects, the received data defines a first upper bound and a first lower bound for a plurality of continuous spatiotemporal convex corridors; generates a first curve segment for each of the plurality of continuous spatiotemporal convex corridors; and the planned trajectory of the autonomous vehicle passes through the plurality of continuous spatiotemporal convex corridors and is calculated based on the respective first curve segments.

[0021] In one or more of the above aspects, the method includes: calculating constraints on a plurality of first spatial dimension control point values ​​for each of the plurality of consecutive spatiotemporal convex corridors to achieve continuity of a trajectory path traversing the plurality of consecutive spatiotemporal convex corridors, wherein the plurality of first spatial dimension control point values ​​for each of the plurality of consecutive spatiotemporal convex corridors are also calculated according to the constraints.

[0022] In some aspects, the present invention provides a system for trajectory planning of autonomous vehicles. The system includes a processing system configured by instructions to perform any aspect of the methods described above.

[0023] In some aspects, the present invention provides a computer-readable medium storing instructions. These instructions are executable by a processing system for trajectory planning of an autonomous vehicle. When executed, the instructions cause the system to perform any aspect of the methods described above. Attached Figure Description

[0024] The following figures illustrate exemplary embodiments of this application by way of example.

[0025] Figure 1 An exemplary static driving scenario based on a set of continuous corridors is shown.

[0026] Figure 2A An exemplary static driving scenario based on a trapezoidal corridor is shown.

[0027] Figure 2B An exemplary driving scenario is shown, in which multiple corridors are used instead of a single corridor.

[0028] Figure 3 An exemplary dynamic driving scenario is shown.

[0029] Figure 4 It shows the relationship with Figure 3 Examples of safe zones for the vehicle in the ST and LT spatiotemporal planes corresponding to the scenarios described.

[0030] Figure 5A , Figure 5B and Figure 5C Rectangular corridors, trapezoidal corridors, and general convex corridors are shown respectively, used to define... Figure 4The search space (represented by solid lines) and the unsearched space (represented by dashed lines) of the safe area in the diagram.

[0031] Figure 6A , Figure 6B and Figure 6C The figures on the spacetime plane and the plane are shown respectively. Figure 5A , Figure 5B and Figure 5C Solution for the trajectory curve segment corresponding to the corridor in the diagram.

[0032] Figure 7 Examples of parametric curves and convex corridors are shown, illustrating cases where the convex hull property is not satisfied.

[0033] Figure 8 This is a block diagram of some components in an exemplary autonomous vehicle.

[0034] Figure 9 The exemplary embodiments provided can be used to implement Figure 8 A block diagram of some components in the planning system of an autonomous vehicle.

[0035] Figure 10 This is a block diagram showing more details of an exemplary planning system.

[0036] Figure 11 An example of convex corridor data is shown.

[0037] Figure 12 yes Figure 10 A block diagram showing more details of the trajectory generator in an exemplary planning system.

[0038] Figure 13 An example of a convex corridor is shown.

[0039] Figure 14A and Figure 14B An example of a Bezier curve segment within a convex corridor is shown.

[0040] Similar reference numerals can be used to denote similar components in different accompanying drawings. Detailed Implementation

[0041] As described in this article, the term "within" as used in the context of a range or boundary includes the values ​​that define the outer range or boundary, as well as all intermediate values ​​between those values. For example, "within 1 to 10" includes the values ​​1 and 10, and all values ​​in between.

[0042] According to an exemplary aspect, motion planning uses spatiotemporal convex corridors to define the search space in order to generate trajectories for autonomous vehicles. A convex corridor is a geometry in which any straight line segment connecting two points on the boundary of the shape lies within the shape; that is, for any two points within the shape, the line segment between them never extends beyond the shape. In at least some planning applications, using spatiotemporal corridor shapes that are more complex than the rectangular or trapezoidal shapes previously used can cover a larger search space with fewer corridors. This can minimize the computational cost required to find a trajectory solution (e.g., optimize CPU usage, execution time, and power consumption), and can also make the solution potentially more efficient and comfortable from the perspective of vehicle operation by using trajectories that require lower acceleration.

[0043] To provide context for the following description, see reference. Figure 3 , Figure 4 , Figure 5A , Figure 5B , Figure 5C , Figure 6A , Figure 6B and Figure 6C Examples of driving scenarios are described, along with corresponding ST / LT graphs for different corridor shapes. Figure 3 A scenario is illustrated, showing the predicted trajectories of vehicle 30 and first and second other vehicles 32 and 34 traveling along a road over a duration of 5 seconds. At the start, vehicle 32 (dashed line) is positioned in front of vehicle 30, and vehicle 34 (solid line) is positioned to the left of vehicle 30. Over the next 5 seconds, vehicle 32 begins to decelerate, and vehicle 34 cuts into vehicle 30's lane and pushes vehicle 30 to the right. After projecting all traffic vehicles onto the ST and LT planes, Figure 4 The ST and LT charts corresponding to the vehicle's collision-free safety space 42 (e.g., the area below the plotted spatiotemporal safety space boundary curve 44, which corresponds to the search space of the safety trajectory curve) are shown.

[0044] Figure 5A , Figure 5B and Figure 5C The corresponding security planning spatiotemporal corridors generated using the following corresponding methods are shown: (i) rectangular ST and LT search corridors 52 ( Figure 5A (ii) Trapezoidal ST and LT search corridor 54 ( Figure 5B (iii) General convex ST and LT search corridors 56 ( Figure 5C In all cases, the corridor serves as a safe passageway through which vehicles can maneuver without collisions. Corridor boundaries can also enforce other constraints, such as those imposed by behavioral decisions, vehicle kinematics, and dynamics.

[0045] It should be noted that, compared to other corridor shapes, Figure 5C The typical convex corridor provides a larger search space coverage for the available safe space below the safe space boundary curve 44, where, Figure 5A and Figure 5B The dashed shape 58 in the figure represents the unsearched space.

[0046] Figure 5C The convex corridors in the design offer an advantage by providing a larger search area. Figure 6A , Figure 6B and Figure 6C The ST plan diagram illustrates this, where the dashed line 60 shows the planned trajectory curve, which stops within the boundaries of the corresponding rectangular corridor, trapezoidal corridor, and general convex corridor. (See diagram for example.) Figure 6A , Figure 6B and Figure 6C As shown, the larger the search area covered by the corresponding corridor, the lower the deceleration required to plan the trajectory curve. This improves overall comfort and smoothness, enhances the vehicle's energy efficiency, and thus reduces power consumption. The same principle applies to the LT plane. The vehicle doesn't need to make as sharp turns, which also improves comfort.

[0047] As mentioned above, for Figure 6A In the case of the rectangular corridor shown, one solution is to divide the search space below the safety space boundary curve 44 into multiple consecutive rectangular corridors of different shapes, such as... Figure 2A and Figure 2B As shown. However, as the number of corridors increases, this solution increases the computational cost of the space under consideration. Furthermore, this solution produces a more conservative trajectory, i.e., a lower speed and faster deceleration when traversing corridors. In contrast, Figure 5C and Figure 6C The typical convex corridor in the design covers the same (or even larger) searchable area with as few corridors as possible.

[0048] Nevertheless, rectangular corridors (rather than trapezoidal or general convex corridors) are typically used for safe trajectory planning to ensure that the trajectory curve segments generated for the corridor satisfy the "convex hull property." Specifically, to ensure that the planned trajectory segments are safe, all points on the parametric curve (e.g., a Bézier curve) that defines the trajectory curve segment passing through the corridor must fall within the search space defined by the corridor. The curve is typically generated using a subset of control points bounded by the corridor. n A Bézier curve segment requires n+1 control points. However, for complex corridor shapes, it is difficult to ensure that the convex hull property is satisfied even if all control points fall within the corridor. The convex hull property (that the generated trajectory Bézier curve segment is also within the convex boundary as long as the control points are inside the convex boundary) only applies to rectangular corridors in spacetime corridors.

[0049] For example, Figure 7 An example of a convex boundary 72 on the SL plane and a convex boundary 74 on the ST spacetime plane is shown, along with the corresponding nth-order Bézier curve segments 76 and 78. Both curve segments have been generated based on the corresponding set of n+1 control points located within convex boundaries 72 and 74. Figure 7 As shown, even if no control point exceeds the boundary, the Bézier curve segment 78 generated on the ST spacetime plane still partially exceeds the convex boundary. Although the Bézier curve segment 76 on the spacetime SL plane satisfies the convex hull property, the Bézier curve segment 78 on the spacetime ST plane does not satisfy the convex hull property.

[0050] According to an exemplary aspect of the present invention, a motion planning system and method for generating safe trajectory curve segments in a general convex corridor is described. The time axis position of control points within the corridor strongly influences the shape of the generated Bézier curve segment traversing the corridor. In this regard, the control point time and the corresponding upper and lower corridor boundary values ​​are determined as conditions to maintain the generated Bézier curve within the spatiotemporal convex corridor, while optimizing the search region within the spatiotemporal convex corridor. As detailed below, calculating the control point time and upper and lower control point limits on the Bézier curve is to ensure that the general spatiotemporal convex corridor satisfies the convex hull property. Therefore, all points on the generated Bézier curve strictly remain within one or more corridors, which not only guarantees safety but also ensures the implementation of behavioral decisions and other constraints.

[0051] In some examples, this method can narrow down the unsearched safe region (i.e., region 58 in safe space 42 that is not included in the spatiotemporal convex corridor 56) to approximately The order of magnitude (where, O This is on the order of magnitude the difference between the unsearched region 58 and the searchable region (e.g., the safe space 42). (This is the number of control points), which is the most likely optimal result. Therefore, the square of the number of control points is inversely proportional to the unsearched area.

[0052] Exemplary embodiments will be described in the context of n-order Bézier curves, but the methods and systems described herein can be applied to other n-order parametric curves, including n-order B-spline curves, etc.

[0053] The following is for reference. Figure 8 , Figure 9 and Figure 10 This paper briefly introduces an autonomous vehicle to which the exemplary planning system and methods described herein can be applied.

[0054] Autonomous vehicles typically include various software systems for perception and prediction, localization and mapping, and planning and control. The planning software system (often referred to as the planning system) plans the vehicle's trajectory based on the target destination and vehicle physical parameters (e.g., wheelbase, width, length, etc.). The vehicle control software system (e.g., the vehicle control system) receives the trajectory from the planning system and generates control commands to control the vehicle to follow the trajectory. While the examples described herein may refer to automobiles as autonomous vehicles, the guidance of this invention can be implemented in other forms of autonomous (including semi-autonomous) vehicles, including trams, subways, trucks, buses, surface and underwater vessels and ships, aircraft, unmanned aerial vehicles (also known as unmanned aerial vehicles, UAVs), warehouse equipment, manufacturing facility equipment, construction equipment, farm equipment, mobile robots (e.g., vacuum cleaners and lawnmowers), and other robotic devices. Autonomous vehicles can include both passenger-carrying and passenger-carrying vehicles.

[0055] Figure 8 This is a block diagram of some components in an exemplary autonomous vehicle 100. Vehicle 100 includes a sensor system 110, a perception system 120, a state generator 125, a planning system 130, a vehicle control system 140, and an electromechanical system 150, etc. Vehicle 100 may also include other systems and components, but are not shown for illustration purposes. In this example, the perception system 120, the planning system 130, and the vehicle control system 140 are different software systems that include machine-readable instructions. For example, these instructions may be executed by one or more processors in the processing system of vehicle 100. The various systems and components in the vehicle can communicate with each other via wired or wireless communication, etc.

[0056] Sensor system 110 includes various sensing units, such as radar unit 112, LIDAR unit 114, and camera 116, which are used to collect information about the surrounding environment as the vehicle 100 operates. Sensor system 110 also includes a global positioning system (GPS) unit 118 for collecting the vehicle's position information in the environment. Sensor system 110 also includes one or more internal sensors 119 for collecting information about the physical operating conditions of the vehicle 100 itself, including sensors for sensing steering angle, linear velocity, linear acceleration and angular acceleration, pose (pitch, yaw, roll), compass direction of travel, vehicle vibration, throttle status, braking status, wheel traction, gear ratio, in-vehicle temperature and pressure, etc.

[0057] Information collected by each sensing unit in sensor system 110 is provided as sensor data to perception system 120. Perception system 120 processes the sensor data received from each sensing unit to generate vehicle data and surrounding environment data. For example, vehicle data includes one or more of the following: data representing the vehicle's position; data representing the vehicle's physical properties, such as width and length, mass, wheelbase, and slip angle; vehicle motion data, such as linear velocity and acceleration, direction of travel, angular acceleration, pose (e.g., pitch, yaw, roll) and vibration; and mechanical system operating parameters (e.g., engine RPM, throttle position, brake position, and gear ratio). For example, surrounding environment data may include information on stationary and moving objects detected around vehicle 100, weather and temperature conditions, road conditions, road configuration, and other information about the surrounding environment. For example, sensor data received from radar, LIDAR, and camera units 112, 114, and 116 can be used to determine the local operating environment of vehicle 100. Sensor data from GPS unit 118 and other sensors can be used to determine the vehicle's position, thereby defining the geographic location of vehicle 100. Sensor data from internal sensor 119 and other sensor units can be used to determine the vehicle's motion properties, including the vehicle 100's velocity and orientation (i.e., orientation) relative to a reference coordinate system.

[0058] The state generator 125 receives environmental data output from the perception system 120 and data from the vehicle 100. The state generator 125 processes the environmental data and vehicle 100 data to generate the state of the vehicle 100 (hereinafter referred to as the vehicle state). Although Figure 8 The state generator 125 is shown to be a standalone software system, but in some embodiments, the state generator 125 may be included in the sensing system 120 or the planning system 130.

[0059] The key aspect of this invention is the real-time output of vehicle status from the status generator 125 to the planning system 130, as detailed below. The vehicle control system 140 controls the operation of the vehicle 100 based on the trajectory output by the planning system 130. The vehicle control system 140 can generate control signals for the electromechanical components in the vehicle 100 to control the movement of the vehicle 100. The electromechanical system 150 receives control signals from the vehicle control system 140 to operate the electromechanical components in the vehicle 100, such as the engine, transmission system, steering system, and braking system.

[0060] Figure 9An example of a processing system 200 that can be implemented in vehicle 100 is shown. The processing system 200 includes one or more processors 210. The one or more processors 210 may include a central processing unit (CPU), a graphics processing unit (GPU), a tensor processing unit (TPU), a neural processing unit (NPU), a digital signal processor, and / or other computing elements. The one or more processors 210 are coupled to one or more electronic memories 220 and to one or more input and output (I / O) interfaces or devices 230, such as network interfaces, user output devices like displays, user input devices like touchscreens, and so on.

[0061] Electronic storage device 220 may include any suitable one or more volatile and / or non-volatile storage and retrieval devices, including flash memory, random access memory (RAM), read-only memory (ROM), hard disk, optical disk, subscriber identity module (SIM) card, memory stick, secure digital (SD) card, and other state storage devices, etc. Figure 8 In the example, the electronic memory 220 in the processing system 200 stores instructions (executable by one or more processors 210) for implementing the perception system 120 (instruction 120I), the state generator 125 (instruction 125I), the planning system 130 (instruction 130I), and the vehicle control system 140 (instruction 140I). In some embodiments, the electronic memory 220 also stores data 145, including sensor data provided by the sensor system 110, vehicle data and environmental data output by the perception system 120 used by the planning system 130 to generate at least one trajectory, and other data such as road network maps.

[0062] Figure 10 This is a block diagram showing more details about the planning system 130.

[0063] As shown in the figure, the planning system 130 can perform planning and decision-making operations at different levels: for example, the task level (e.g., task planner 310 performs task planning), the behavior level (e.g., behavior planner 320 performs behavior planning), and the motion level (e.g., motion planner 330 performs motion planning). Task planning is considered high-level (or global) planning, motion planning is considered low-level (or local) planning, and behavior planning is considered a level of planning between task planning and motion planning. Generally, the output of high-level planning and decision-making operations can constitute at least part of the input of low-level planning and decision-making.

[0064] Typically, the purpose of planning and decision-making operations is to determine the path and corresponding trajectory for vehicle 100 to travel from an initial position (e.g., the vehicle's current position and orientation, or its expected future position and orientation) to a target position (e.g., a user-defined final destination). As is known in the art, a path is a series of configurations arranged in a specific order (e.g., a path includes an ordered set of spatial coordinates), regardless of the temporal sequence of these configurations, while a trajectory focuses on when each part of the path must be reached, thus specifying the temporal sequence (e.g., a trajectory is a path with timestamped data, thus including a set of spatiotemporal coordinates). In some examples, the entire path can be processed and executed as a set of trajectories. When determining a suitable path and trajectory, planning system 130 takes into account various conditions, such as drivable ground (e.g., defined roads), obstacles (e.g., pedestrians and other vehicles), traffic regulations (e.g., obeying traffic signals), and user-defined preferences (e.g., avoiding toll roads).

[0065] The planning and decision-making operations performed by the planning system 130 can be dynamic, meaning they can be repeated as the environment changes. Therefore, for example, the planning system 130 can receive a new vehicle state output by the state generator 125 and repeat the planning and decision-making operations to generate new plans and trajectories based on the environmental changes reflected in the new vehicle state. Environmental changes may be due to vehicle 100 movement (e.g., vehicle 100 approaching a newly detected obstacle) or due to dynamic environmental properties (e.g., moving pedestrians and other moving vehicles).

[0066] Planning and decision-making operations performed at the task level (e.g., task planner 310 performing task planning) involve planning a route for vehicle 100 at a high or global level. The first location of vehicle 100 can be the starting point of the journey, and the target location of vehicle 100 can be the final destination. Drawing a route through a set of roads is an example of task planning. Generally, once the final destination is set (e.g., through user input), it remains unchanged throughout the journey. While the final destination may be constant, the route planned by task planning may change throughout the journey. For example, changing traffic conditions may require task planning to dynamically update the planned route to avoid congested roads.

[0067] In order to perform task planning, the input data received by the task planner 310 may include GPS data (e.g., for determining the starting point of vehicle 100), geographic map data (e.g., road networks from internal or external map databases), traffic data (e.g., from external traffic monitoring systems), final destination (e.g., limited to x and y coordinates, or limited to longitude and latitude coordinates), any user-defined preferences (e.g., preferences to avoid toll roads), and so on.

[0068] The planned path generated by the task planner 310, which executes the task planning process, defines the route to be traveled from the starting point to the final destination. The output may include data defining a set of intermediate target locations (or waypoints) along the route.

[0069] The behavior planner 320 receives a planned path from the task planner 310, including a set of intermediate target locations (if any). The behavior planner 320 also receives the vehicle state output by the state generator 125. Based on the planned path and vehicle state, the behavior planner 320 generates behavioral decisions to control the behavior of the vehicle 100 on a more local and short-term basis than the task planner 310. These behavioral decisions can serve as objectives or a set of constraints for the motion planner 330. The behavior planner 320 can generate behavioral decisions that conform to certain rules or driving preferences. These behavioral rules can be based on traffic rules or on smooth and efficient driving instructions (e.g., the vehicle should use the faster lane if possible). For example, the behavioral decisions output from the behavior planner 320 can be used as constraints for motion planning.

[0070] The motion planner 330 is used to iteratively find trajectories to achieve a planned path, which satisfies behavioral decision-making and navigates the environment encountered on the planned path in a relatively safe, comfortable and fast manner.

[0071] exist Figure 10In the example shown, the motion planner 330 includes a convex corridor generator 332 and a trajectory generator 334. While the input received by the convex corridor generator 332 may vary in different applications, in the example shown, the convex corridor generator 332 receives planned paths, time-series data, vehicle states, behavioral decisions, and road network data in a structured environment as input. In an exemplary embodiment, the convex corridor generator 332 generates corridor data 400, which represents a planned time interval. T This defines multiple consecutive general spatiotemporal convex corridors. For example, Figure 11 A simplified example of convex corridor data 400 for two consecutive convex corridors 402(1) and 402(2) is shown in the diagram. The planning path and time series data specify the time. t Vehicle starting position 15 and time at point 1 t Given the vehicle target location at 3 locations, with a data point of 17, the convex corridor data is 400 and... T=t 3 –t The planning time range intervals correspond to 1. Each convex corridor 402(1) and 402(2) (generally referred to herein as one or more convex corridors 402) defines a corresponding collision-free search space for the corresponding trajectory segment. Figure 11 As shown, the end of convex corridor 402(1) and the starting point of convex corridor 402(2) are at the midpoint of time. t There are two overlaps, thus providing a time frame. t Starting position 15 to time t The trajectory of vehicle target location 17 at point 3 provides a continuous corridor.

[0072] In an exemplary embodiment, the corridor data for each convex corridor 402 includes data defining the convex corridor 402 in a 3D SLT Fleury coordinate system space. In this respect, each convex corridor 402 is projected onto 2D ST and LT spatiotemporal planes. Therefore, the corridor data for each convex corridor 402 includes data defining the convex corridor 402 on the ST and LT planes. For the ST plane, the corridor data defines the upper bound 406 of the spatiotemporal convex corridor 402 in the S-space dimension as a concave function of time (e.g., ...). f ub (t) ), and the lower bound 408 of the spacetime corridor in the S-space dimension is restricted to a convex function of time (e.g., f lb (t) Similarly, for the LT plane, the corridor data defines the upper bound 410 of the spatiotemporal convex corridor 402 in the L spatial dimension as a concave function of time, while defining the lower bound 410 of the spatiotemporal corridor in the L spatial dimension as a convex function of time.

[0073] exist Figure 11 The image shows a set of continuous convex corridors. In some examples, among the multiple corridors generated for planning paths over time intervals, some corridors may not be convex, but rather rectangular or trapezoidal in shape.

[0074] refer to Figure 12 The trajectory generator 334 receives corridor data 400 generated by the corridor generator 332 over a planning time interval T as input and outputs a planned trajectory from the vehicle's starting position 15 to the target position 17. The trajectory generator 334 treats the calculation of the planned trajectory as an optimization problem, which may include the following operations (detailed below): a constraint generator 336 generates a set of constraints for the optimization problem; a curve segment generator 344 calculates trajectory curve segments for each corridor; and the planned trajectory generator 346 converts the generated curve segments into an output planned trajectory between the initial position 15 (e.g., the vehicle's starting position) and the target position 17. The output planned trajectory is then provided to the vehicle control system 140 for implementation.

[0075] As described above, each spatiotemporal convex corridor 402 includes data defining the upper and lower corridor boundaries on the ST and LT planes. In an exemplary embodiment, the curve segment generator 344 generates corresponding n-order Bézier curve segments for each spatiotemporal convex corridor 402 on the ST and LT planes. As a prerequisite for determining the Bézier curve segments, the control point boundary operation 340 is used to calculate the temporal position and control range (e.g., upper and lower bounds) for each of the n+1 control points on the corresponding Bézier curve segment, which will be generated by the curve segment generator 344 for the convex corridor 402. As described above, in an exemplary embodiment, the control points are calculated to constrain the Bézier curves within their respective spatiotemporal convex corridors 402. The control point boundary operation 340 calculates the corresponding temporal position and upper and lower bounds for the n+1 control points on each Bézier curve segment so that each convex corridor 402 satisfies the convex hull property. Therefore, all points on the generated ST and LT plane Bézier curve segments are strictly contained within their respective corridors 402.

[0076] The following is for reference. Figure 13 It describes how to calculate the temporal position and spatial dimension range of control points for a trajectory crossing a spatiotemporal convex corridor 402 by using control point boundary operation 340. Figure 13 A simplified example of the ST plane of the spatiotemporal convex corridor 402 spanning the planning time interval is shown. The following variable nomenclature is used: Variable naming conventions :time The left endpoint of the corridor's time interval (corridor start time). The right endpoint of the corridor's time interval (corridor end time). : Upper bound function of ST-plane corridor : Lower bound function of LT planar corridor The degree of the Bézier polynomial used for the nth-order Bézier curve segment : Index of control points =0, 1, 2, 3...

[0077] Time of the i-th control point As described above Figure 11 As stated and as Figure 13 As shown, the ST plane of the spatiotemporal convex corridor 402 is defined by an upper bound 406 and a lower bound 408. The S-space dimension values ​​of the upper bound 406 and the lower bound 408 are defined in the corridor data 400 and can be represented by the upper and lower bound time correlation functions, respectively. , Corridor 402 has a corridor start time. t 1 and the end time of the corridor t 2. Control point boundary operation 340 is used to calculate the corridor start time. t 1 and the end time of the corridor t The control point times are multiple evenly spaced points within a range of 2. The evenly spaced control point times include at least the start control point time (the corridor start time in the example shown). t 1) End control point time (the corridor end time in the example shown) t 2) and one or more intermediate control point times between the start control point time and the end control point time (e.g., the times in the example shown). t i ). Specifically, n The first-order Bézier curve segment needs to include n+1 control points, and the corresponding control points and the first-order Bézier curve segment need to include n+1 control points. i The control point time for each control point can be calculated using the control point boundary operation 340 according to the following equation: (Equation 1) Then, the control point boundary operation 340 determines the maximum control point value corresponding to the value of the upper bound 406 of the S-space dimension at the corresponding control point time for each uniformly spaced control point time, and the minimum control point value corresponding to the value of the lower bound 406 of the S-space dimension at the corresponding control point time. For example, the maximum control point value of the i-th control point on the S-axis is: (Equation 2) The minimum value of the i-th control point on the S-axis is: (Equation 2) Similarly, the control point boundary operation 340 also determines the maximum control point value on the LT plane corresponding to the value of the upper bound 410 of the L space dimension at the corresponding control point time, and the minimum control point value corresponding to the value of the lower bound 412 of the L space dimension at the corresponding control point time for each control point time.

[0078] Accordingly, the output of the control point boundary operation 340 for the spatiotemporal convex corridor 402 includes n+1 control point times, the corresponding maximum and minimum values ​​of the n+1 control points of the n-order Bézier curve segment on the ST plane, and the corresponding maximum and minimum values ​​of the n+1 control points of the n-order Bézier curve segment on the LT plane.

[0079] In at least some examples, constraint generator 336 also includes one or more operations 342 for generating additional constraints for the ST and LT plane Bézier curve segments. For example, additional constraints can be generated for each control point to address issues such as continuity of curve segments between consecutive corridors 402, behavioral decisions, and dynamic constraints of vehicle 100. For example, to maintain continuity, the initial control points of the trajectory curve segments are constrained by trajectory segments calculated for the previous corridor. Examples of various types of constraints that can be generated for control points can be found in the following reference [1]: Ding, Wenchao et al. published "Safe" in IEEE Robotics and Automation Communications 4.3 (2019): 2997-3004. trajectory generation for complex urban environments using spatio-temporal Semantic corridor (using spatiotemporal semantic corridors to generate safe trajectories in complex urban environments) .

[0080] Control point constraints (including the corresponding maximum and minimum values ​​and times generated by control point boundary operation 340) and any other constraints generated by operation 342 (e.g., constraints required to position the initial control point on a trajectory segment as the final control point on the previous trajectory segment) are provided to curve segment generator 344. Curve segment generator 344 is used to generate corresponding control point spatial dimension values ​​for each control point time based on the control point constraints, thereby providing optimized Bézier curve segments for the LT and ST planes of each spatiotemporally complex corridor 402. In an exemplary embodiment, curve segment generator 344 is used to determine, for each spatiotemporally complex corridor 402 defined in the corridor data within the planning time interval, the coordinates of control points on the ST plane that minimize a first predefined cost function within the planning time interval and the coordinates of control points on the LT plane that minimize a second predefined cost function within the planning time interval.

[0081] Examples of cost functions can be found in the following references: Reference [1]: Ding, Wenchao et al. "Safe trajectory" published in IEEE Robotics and Automation Communications 4.3 (2019): 2997-3004 generation for complex urban environments using spatio-temporal semantic corridor (using spatiotemporal semantic corridors to generate safe trajectories in complex urban environments); Reference [2] Li, Jialun "Speed ​​Planning Using Bezier" was published by [Authors' Name] in arXiv preprint: 2104.11655 (2021). Polynomials with Trapezoidal Corridors (using Bessel polynomials with trapezoidal corridors for velocity) planning)" .

[0082] Through explanation, Figure 14A An example of a Bézier curve segment 502 of order n=5 is shown, which is a curve segment that has been generated by curve segment generator 344 for... Figure 13 The six ST plane control points 504(0) to 504(5) generated by the spatiotemporal convex corridor 402 correspond to this. In many scenarios, the planning time range interval needs to include multiple consecutive corridors with overlapping start and end times. In this regard, Figure 14B Examples of first and second spatiotemporal convex corridors 402(1) and 402(2) and Bézier curve segments 502(1) and 502(2) are shown, these curve segments corresponding to ST plane control points already generated by curve segment generator 344 for spatiotemporal convex corridors 402(1) and 402(2). The corresponding Bézier curve segments 502(1) and 502(2) are connected proportionally, wherein the end time of Bézier curve segment 502(1) coincides temporally and spatially with the start time of Bézier curve segment 502(2). Bézier curve segments 502(1) and 502(2) can be used together to define the planned trajectory within a planning time interval. Figure 14A and Figure 14B As can be seen, both the control points and the generated curve segments satisfy the convex hull property.

[0083] Refer again Figure 12 The optimized Bézier curve segments defined by the control points of the ST and LT planes, calculated by the curve segment generator 344, are provided as input to the planned trajectory generator 346. The planned trajectory generator converts the format of the Bézier curve segments on the ST and LT planes into a planned trajectory that can be implemented by the vehicle control system 140. For example, the planned trajectory may include multiple coordinate points, each including a first dimension value, a second dimension value, and a timestamp of a 3D reference space coordinate system available to the vehicle control system 140.

[0084] Although the convex corridor has already been described above in the context of the Fleury coordinate system, where the S-space dimension corresponds to the longitudinal distance of the tangent vector along the road and the L-space dimension corresponds to the lateral distance from the normal vector along the road to the road, the above system and method can be applied to other spatiotemporal formats. For example, the spatial dimensions can correspond to the first and second distance axes in a three-dimensional Cartesian coordinate system with time as the third axis, respectively.

[0085] Furthermore, in some cases, control points may only need to be calculated on a single spatiotemporal plane. For example, when vehicles such as train locomotives travel along a defined track, the calculations may be limited to the ST plane in some scenarios. Additionally, when vehicles such as airborne drones can move in three spatial dimensions, a third spatiotemporal plane may need to be added to the calculations.

[0086] While examples have been described in the context of autonomous vehicles, it should be understood that the invention is not limited to autonomous vehicles. For example, any vehicle that includes an advanced driver assistance system (including a planning system) can benefit from a motion planner that performs the trajectory generation, trajectory evaluation, and trajectory selection operations of the present invention. Furthermore, any vehicle that includes an autonomous driving system capable of fully or semi-autonomous operation can also benefit from a motion planner that performs the trajectory generation, trajectory evaluation, and trajectory selection operations of the present invention. A planning system incorporating the motion planner of the present invention can help enable vehicles to navigate in structured or unstructured environments containing static and / or dynamic obstacles.

[0087] Although this invention describes methods and processes by describing operations performed in a specific order, one or more operations in the methods and processes may be omitted or modified as appropriate. Where appropriate, one or more operations may be performed in an order other than that described.

[0088] Although the invention has been described at least partially in terms of method, those skilled in the art will understand that the invention is also directed to various components for performing at least some aspects and features of the method, whether by hardware components, software, or any combination thereof. Accordingly, the technical solutions of the invention can be embodied in the form of a software product. Suitable software products can be stored in pre-recorded storage devices or other similar non-volatile or non-transitory computer-readable media, including DVDs, CD-ROMs, USB flash drives, removable hard drives, or other storage media. The software product includes instructions tangibly stored thereon, which enable a processing device (e.g., a personal computer, server, or network device) to perform examples of the methods disclosed herein.

[0089] The invention may be embodied in other specific forms without departing from the subject matter of the claims. The exemplary embodiments described are merely illustrative in all respects and not restrictive. Features selected from one or more of the foregoing embodiments may be combined to create alternative embodiments not explicitly described, and features suitable for such combinations will be understood within the scope of the invention.

[0090] All values ​​and sub-ranges within the scope of the disclosure are also disclosed. Furthermore, although the systems, devices, and processes disclosed and shown herein may include a specific number of elements / components, the systems, devices, and components may be modified to include more or fewer of such elements / components. For example, although any element / component disclosed may be referenced as a single quantity, embodiments disclosed herein may be modified to include multiple such elements / components. The subject matter described herein is intended to cover and encompass all appropriate technical changes.

Claims

1. A computer implementation method for motion planning of autonomous vehicles, characterized in that, The method includes: Receive data, wherein the data (i) defines the upper bound of the first dimension of the spatiotemporal convex corridor in the first spatial dimension as a concave function of time between the corridor start time and the corridor end time, and (ii) defines the lower bound of the first dimension of the spatiotemporal convex corridor in the first spatial dimension as a convex function of time between the corridor start time and the corridor end time, the first spatial dimension corresponding to the first spatiotemporal plane; Calculate multiple control point times within the corridor start time and corridor end time, wherein the multiple control point times include at least the start control point time, the end control point time, and the intermediate control point time between the start control point time and the end control point time; For each of the plurality of control point times, determine a respective first control point maximum value corresponding to the value of the upper bound of the first dimension at the control point time; For each of the plurality of control point times, determine a respective first control point minimum value corresponding to the value of the first dimension lower bound at the control point time; Based on their respective first control point maximum value and their respective first control point minimum value, calculate their respective first dimension control point value for each of the plurality of control point times, wherein the first dimension control point value is limited to a first curve segment within the first dimension upper bound and the first dimension lower bound of the spatiotemporal convex corridor. Based on the first curve segment, a planned trajectory for controlling the operation of the autonomous vehicle is output.

2. The method according to claim 1, characterized in that, The calculation of the first dimension control point value is to uniformly optimize the first limited cost function of the first curve segment passing through the spatiotemporal convex corridor on the first spatiotemporal plane.

3. The method according to claim 1 or 2, characterized in that, The first spatial dimension corresponds to one of the following: (i) the longitudinal distance of the tangent vector along the road and (ii) the lateral distance of the normal vector along the road to the road.

4. The method according to claim 1 or 2, characterized in that, The first spatial dimension corresponds to the distance axis in a three-dimensional Cartesian coordinate system with time as the other axis.

5. The computer implementation method according to claim 1 or 2, characterized in that, The received data further (iii) defines the upper bound of the second dimension of the spatiotemporal convex corridor in the second spatial dimension as a concave function of time between the start time and the end time of the corridor, and (iv) defines the lower bound of the second dimension of the spatiotemporal convex corridor in the second spatial dimension as a convex function of time between the start time and the end time of the corridor, wherein the second spatial dimension corresponds in time to a second spatiotemporal plane that coincides with the first spatiotemporal plane; The method includes: For each of the plurality of control point times, determine a respective second control point maximum value corresponding to the value of the upper bound of the second dimension at the control point time; For each of the plurality of control point times, determine a respective second control point minimum value corresponding to the value of the second dimension lower bound at the control point time; Based on their respective maximum and minimum values ​​of the second control point, calculate the second dimension control point value for each of the plurality of control point times, wherein the second dimension control point value is limited to the second curve segment within the second dimension upper bound and the second dimension lower bound of the spatiotemporal convex corridor. The planned trajectory is output based on the first curve segment and the second curve segment.

6. The method according to claim 5, characterized in that, The calculation of the second dimension control point value is to uniformly optimize the second limited cost function of the second curve segment passing through the spatiotemporal convex corridor on the second spatiotemporal plane.

7. The method according to claim 6, characterized in that, The first spatial dimension corresponds to the longitudinal distance of the tangent vector along the road, and the second spatial dimension corresponds to the lateral distance of the normal vector along the road to the road.

8. The method according to claim 6, characterized in that, The first spatial dimension and the second spatial dimension correspond to the first distance axis and the second distance axis in a three-dimensional Cartesian coordinate system with time as the third axis, respectively.

9. The method according to claim 1 or 2, characterized in that, Each first curve segment is an nth-order parametric curve, and the multiple control point times include n+1 control point times that are evenly spaced in time.

10. The method according to claim 9, characterized in that, Each first curve segment is an nth-order Bezier curve.

11. The method according to claim 9, characterized in that, Each first curve segment is an nth-order B-spline curve.

12. The method according to claim 1 or 2, characterized in that, The received data defines a first-dimensional upper bound and a first-dimensional lower bound for each of the multiple continuous spatiotemporal convex corridors; a first curve segment is generated for each of the continuous spatiotemporal convex corridors; the planned trajectory of the autonomous vehicle passes through the multiple continuous spatiotemporal convex corridors and is calculated based on the respective first curve segments.

13. The method according to claim 12, characterized in that, The method includes: calculating the constraint conditions of multiple first spatial dimension control point values ​​for each of the multiple continuous spatiotemporal convex corridors to achieve the continuity of the trajectory path passing through the multiple continuous spatiotemporal convex corridors, wherein the multiple first spatial dimension control point values ​​for each of the multiple continuous spatiotemporal convex corridors are also calculated according to the constraint conditions.

14. A system for path planning in autonomous vehicles, characterized in that, The system includes a processing system configured by instructions to cause the system to perform the method according to any one of claims 1 to 13.

15. A non-transitory computer-readable medium for storing instructions, characterized in that, When the instructions are executed by the processing system, the processing system performs the method according to any one of claims 1 to 13.

16. A computer program comprising computer-readable instructions, characterized in that, When executed by a processing device in a processing system, the computer-readable instructions cause the processing system to perform the method according to any one of claims 1 to 13.

Images