Local path planning method and dynamic planning method for unstructured road

Abstract

The present application relates to the technical field of intelligent traffic control, in particular to a local path planning method and a dynamic planning method for unstructured roads. The present application firstly performs real-time obstacle screening around the controlled vehicle to cover the multi-obstacle scene, then introduces a finite state machine, and combines the finite state machine with the dynamic planning with adjusted cost function when performing path planning, which can stably switch different driving behaviors under the unstructured road scene. Among them, the finite state machine can accurately judge the type of obstacle, facilitate selective obstacle inflation, and provide reliable obstacle avoidance basis for path planning. The dynamic planning with adjusted cost function can ensure that the controlled vehicle is not prone to instability when making high-frequency decisions, and also improves the smoothness of the initial planning trajectory. The present application solves the problems that the existing path planning of unstructured roads does not consider the coexistence of multiple obstacles, and is prone to instability when making high-frequency decisions.

Description

Technical Field

[0001] This invention relates to the field of intelligent traffic control technology, and more specifically, to: 1. a dynamic planning method for unstructured roads; 2. a dynamic planning method for unstructured roads. Background Technology

[0002] The application scenarios for autonomous vehicles can be divided into two categories: structured scenarios and unstructured scenarios. Compared with structured scenarios, decision-making and planning in unstructured scenarios is one of the challenges in autonomous driving technology research.

[0003] Unstructured scenarios refer to unstructured roads, which often occur in urban non-main roads, rural roads, and parking lots. In these scenarios, there is no relatively stable reference line information, no clear lane divisions or traffic rules, so traffic participants have a higher degree of freedom, their motion modes are difficult to predict accurately, and road conditions are complex and changeable, which poses a great challenge to the decision-making and planning of autonomous vehicles.

[0004] Current research on unstructured autonomous driving is limited, but its goal is similar to that of structured autonomous driving—to perform trajectory planning to obtain the optimal trajectory. Trajectory planning involves both path planning and velocity planning, and then the results of these two planning aspects are combined.

[0005] However, applying existing planning and decision-making methods to unstructured scenarios presents the following limitations when determining path trajectories:

[0006] 1. Generally, only a single obstacle is considered, while the study of scenarios where multiple obstacles coexist is ignored.

[0007] 2. Often, analysis and experiments are only conducted in a single, independent scenario, with less attention paid to situations where scenarios change repeatedly during driving, and less attention paid to high-frequency decision instability phenomena that occur in extremely special circumstances. For example, due to the short planning cycle, there may be multiple minima during decision-making, making the coarse solution of the local path not unique, resulting in wavering decisions and frequent changes in decisions. It can also cause repeated fluctuations in planning and poor smoothness, which can seriously affect driving safety. Summary of the Invention

[0008] Therefore, it is necessary to provide a dynamic planning method and device for unstructured roads to address the problems of existing unstructured road path planning not considering the coexistence of multiple obstacles and the instability that easily occurs during high-frequency decision-making.

[0009] This invention is achieved using the following technical solution:

[0010] In a first aspect, the present invention discloses a dynamic programming method for unstructured roads, comprising:

[0011] Step 1: Obtain the location of the controlled vehicle Car0 on the unstructured road within the current planning period, and perform real-time obstacle screening within a preset range around Car0 to identify the first obstacle Ob1 and the second obstacle Ob2 that affect Car0's planning.

[0012] Step 2: Ob1 and Ob2's real-time driving information and compare it with the set desired speed V. espected The system uses a finite state machine with pre-defined processing rules to process the data and obtain the real-time output of the controlled vehicle and the real-time output of the obstacle.

[0013] Among them, V espected This represents the speed at which Car0 travels at a constant speed on an unstructured road in the absence of obstacles.

[0014] The real-time output of the controlled vehicle includes: the real-time state of Car0; the real-time state of Car0 is used to verify whether the finite state machine is operating normally.

[0015] The real-time obstacle output includes: the obstacle type of Ob1 and the obstacle type of Ob2;

[0016] Step 3: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the path planning graph to obtain the selectively expanded path planning graph.

[0017] Step 4: Use dynamic programming algorithm to process the path planning graph after selective dilation to obtain the path convex space and coarse local path solutions.

[0018] In step four, the dynamic programming algorithm uses a cost function C. total_path ;

[0019]

[0020] In the formula, C obstacle ω represents the collision risk cost under path dynamic programming. obstacle Indicate C obstacle The weight of C; reference ω represents the path similarity cost. reference Indicate C reference The weight of C; smoothness ω represents the smoothness cost. smoothness Indicate C smoothness The weight of C; HistoricalRoute ω represents the cost of historical trajectory. HistoricalRoute Indicate C HistoricalRoute The weight.

[0021] This dynamic planning method for unstructured roads implements the method or process according to embodiments of this disclosure.

[0022] Secondly, this invention discloses a dynamic programming method for unstructured roads, which is used to dynamically plan the optimal trajectory of a controlled vehicle traveling on an unstructured road.

[0023] Dynamic programming methods for unstructured roads include:

[0024] Such as the local path planning method for unstructured roads disclosed in the first aspect;

[0025] In step two, the real-time output of the controlled vehicle also includes: the real-time reference speed V of Car0. ref V ref Used to calculate the cost of the expected speed deviation C velocity ;

[0026] If Ob1 or Ob2 is the first possible type, it means that this obstacle does not exist and will not appear in the speed planning.

[0027] If Ob1 or Ob2 is a third possible type, then expansion is performed only in the velocity planning graph;

[0028] The dynamic programming method for unstructured roads also includes:

[0029] Step 5: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the velocity planning graph to obtain the selectively expanded velocity planning graph.

[0030] Step 6: Use dynamic programming algorithm to process the velocity planning graph after selective dilation to obtain the velocity convex space and local velocity coarse solution;

[0031] In step six, the dynamic programming algorithm uses a cost function C. total_speed ;

[0032]

[0033] In the formula, C′ obstacle ω′ represents the collision risk cost under velocity dynamic programming. obstacle Represents C′ obstacle Weighting coefficients; C velocity ω represents the cost of the expected speed deviation. velocity Indicate C velocity The weight of C; accelerate ω represents the cost of comfort. accelerate Indicate C accelerate The weight of C; jerk ω represents the cost of acceleration. jerkRepresented as C jerk The weights;

[0034] Step 7: Use standard quadratic programming to solve for the optimal local path based on the path convex space and local path coarse solution;

[0035] The optimal local velocity is obtained by using standard quadratic programming based on the velocity convex space and local velocity coarse solution.

[0036] The optimal local trajectory is obtained by combining the optimal local path and the optimal local velocity.

[0037] Step 8: As the current planning cycle is updated, repeat steps 1 through 7 to form the global optimal trajectory from the optimal local trajectories.

[0038] This dynamic planning method for unstructured roads implements the method or process according to embodiments of this disclosure.

[0039] Thirdly, the present invention discloses a computer program product, comprising a computer program. When executed by a processor, the computer program implements the steps of the local path planning method for unstructured roads as disclosed in the first aspect, and / or the steps of the dynamic planning method for unstructured roads as disclosed in the second aspect.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] 1. This invention performs real-time obstacle screening around the controlled vehicle to cover multi-obstacle scenarios and introduces a finite state machine, which can accurately determine the type of obstacle and facilitate selective obstacle expansion. It provides a reliable obstacle avoidance basis for path planning and speed planning, and can provide more accurate optimal local paths and optimal local speeds for subsequent standard secondary planning, thereby assembling the optimal local trajectory to form the global optimal trajectory.

[0042] 2. This invention combines finite state machines with dynamic programming with an adjusted cost function when performing path planning, enabling stable switching between different driving behaviors in unstructured road scenarios. The adjusted cost function ensures that the controlled vehicle is less prone to instability during high-frequency decision-making, while also improving the smoothness of the initial planned trajectory.

[0043] 3. This invention combines finite state machines with dynamic programming using conventional cost functions when performing speed planning, enabling balanced transitions in speed control in unstructured road scenarios. Attached Figure Description

[0044] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0045] Figure 1 A flowchart of a local path planning method for unstructured roads provided in Embodiment 1 of the present invention;

[0046] Figure 2 This is a schematic diagram of the controlled vehicle Car0 in Embodiment 1 of the present invention, in which there are two relatively close obstacles in front of it, obstacle1 and obstacle2, and the closest obstacle 3 behind it;

[0047] Figure 3 This is a schematic diagram illustrating the danger of obstacle expansion during overtaking in Embodiment 1 of the present invention;

[0048] Figure 4 This is a schematic diagram of the processing rules in Embodiment 1 of the present invention;

[0049] Figure 5 This is a schematic diagram of selective expansion in Embodiment 1 of the present invention;

[0050] Figure 6 This is a flowchart of a dynamic planning method for unstructured roads provided in Embodiment 2 of the present invention;

[0051] Figure 7 This is scenario one provided in embodiment 2 of the present invention;

[0052] Figure 8 In Embodiment 2 of the present invention, C is not added. HistoricalRoute The result of calculating the coarse solution of the local path using the cost function is shown in the figure.

[0053] Figure 9 In Embodiment 2 of the present invention, C was added. HistoricalRoute The result of calculating the coarse solution of the local path using the cost function is shown in the figure.

[0054] Figure 10 This is an unstructured road model provided in Embodiment 2 of the present invention;

[0055] Figure 11 This is the real-time state diagram of Car0 output by the wired state machine in Embodiment 2 of the present invention;

[0056] Figure 12 This is a longitudinal acceleration diagram of Car0 throughout its entire trajectory in Embodiment 2 of the present invention;

[0057] Figure 13 This is a longitudinal velocity diagram of Car0 throughout its entire trajectory in Embodiment 2 of the present invention;

[0058] Figure 14 This is a lateral acceleration diagram of Car0 throughout the entire process in Embodiment 2 of the present invention;

[0059] Figure 15 This is a diagram showing the steering wheel angle of Car0 throughout its rotation in Embodiment 2 of the present invention;

[0060] Figure 16 This is a heading angle diagram of Car0 throughout the entire journey in Embodiment 2 of the present invention;

[0061] Figure 17 This is a vehicle trajectory diagram of Car0 throughout the entire journey in Embodiment 2 of the present invention. Detailed Implementation

[0062] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0063] It should be noted that when a component is said to be "installed on" another component, it can be directly on the other component or it may be in a component that is centered on it. When a component is said to be "set on" another component, it can be directly set on the other component or it may also be in a component that is centered on it. When a component is said to be "fixed to" another component, it can be directly fixed to the other component or it may also be in a component that is centered on it.

[0064] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the specification of this invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "or / and" as used herein includes any and all combinations of one or more of the associated listed items.

[0065] Example 1

[0066] This embodiment 1 is an improvement on existing path planning, used to dynamically plan a coarse local path solution for a controlled vehicle Car0 traveling on an unstructured road.

[0067] See Figure 1 This document illustrates the main flowchart of a local path planning method for unstructured roads provided in Embodiment 1, which includes:

[0068] Step 1: Obtain the location of the controlled vehicle Car0 on the unstructured road within the current planning period, and perform real-time obstacle screening within a preset range around Car0 to identify the first obstacle Ob1 and the second obstacle Ob2 that affect Car0's planning.

[0069] For Car0, it can sense all obstacles within a preset range—the obstacle's speed, acceleration, and position relative to Car0. Generally, a corresponding detection device can be installed on Car0 to detect obstacle information; or a third-party detection device can be installed on unstructured roads to detect obstacle information.

[0070] Since this method is dynamic programming, the planning range is the area near Car0 (smaller than the preset range around Car0).

[0071] Generally, the preset range around Car0 is defined as the road segment formed by the distance behind Car0 along the s-axis being less than L1 and the distance in front of Car0 along the s-axis being less than L2. Here, L1 represents the rear distance threshold, and L2 represents the front distance threshold.

[0072] For road segments where the distance along the s-axis behind Car0 is ≥ L1 or the distance along the s-axis in front of Car0 is ≥ L2, even if there are obstacles, their impact on Car0 can be ignored due to the large distance. The values ​​of L1 and L2 can be set according to specific circumstances. In this embodiment 1, L1 is 20m and L2 is 60m.

[0073] The reason for selecting two obstacles in this step is to be able to handle complex situations, such as when following or overtaking other vehicles simultaneously. This requires real-time detection of two obstacles that are close to Car0 – it is difficult to handle such situations when only one obstacle is detected.

[0074] Specifically, real-time obstacle screening methods include:

[0075] Real-time detection of whether there are M obstacles within a preset range around Car0;

[0076] ① When M = 0, neither Ob1 nor Ob2 exists;

[0077] ② When M=1, the obstacle is designated as Ob1, while Ob2 does not exist;

[0078] ③ When M≥2:

[0079] If there is a nearest obstacle, obstacle3, behind Car0, and the longitudinal velocity of obstacle3 is greater than or equal to the longitudinal velocity threshold V. LIf the condition is met, then obstacle3 is designated as Ob1; otherwise, the nearest obstacle in front of Car0, obstacle1, is designated as Ob1, and the second nearest obstacle in front of Car0, obstacle2, is designated as Ob2.

[0080] If obstacle3 is considered as Ob1, and there is a nearest obstacle obstacle1 in front of Car0, then obstacle1 is considered as Ob2; otherwise, Ob2 does not exist.

[0081] For easier understanding of ③, please refer to Figure 2 This shows that Car0 has two relatively close obstacles in front of it, obstacle1 and obstacle2, and the closest obstacle, obstacle3, behind it.

[0082] Among them, the relative distances of obstacle1, obstacle2, obstacle3 and Car0 in the s-axis direction are s1, s2, and s3, respectively; s1 < s2.

[0083] Therefore, according to rule ③:

[0084] (1) If the real-time longitudinal velocity of obstacle3 is ≥ V L If obstacle3 is a moving vehicle with longitudinal velocity, then obstacle3 is taken as Ob1 and obstacle1 is taken as Ob2.

[0085] (2) If the real-time longitudinal velocity of obstacle3 is < V L If we rule out the possibility that obstacle3 is a longitudinally moving vehicle, then obstacle1 is designated as Ob1, and obstacle2 is designated as Ob2.

[0086] The rules ①, ②, and ③ mentioned above are adopted to facilitate the expansion of obstacles in the future, so as to better handle overtaking scenarios.

[0087] See Figure 3 Taking overtaking as an example: If the obstacle screening rules mentioned above are not applied, when Car0 has just passed the vehicle that needs to be overtaken, the overtaken vehicle will be treated as a normal static obstacle. Therefore, the controlled vehicle will move towards the global reference line, such as... Figure 3 As shown in (a); if the overtaken vehicle's obstruction expansion is ignored, this could result in the two vehicles being too close together when completing the overtaking maneuver, which is quite dangerous. Figure 3 As shown in (b).

[0088] Step 2: Ob1 and Ob2's real-time driving information and compare it with the set desired speed V. espectedThe system uses a finite state machine with pre-defined processing rules to process the data and obtain the real-time output of the controlled vehicle and the real-time output of the obstacle.

[0089] A finite state machine (FSM) is a tool used to model the behavior of objects. Its main function is to describe the sequence of states that an object goes through during its life cycle and how it responds to various events from the outside world.

[0090] This step uses a finite state machine to describe the state and behavior of Car0, in order to more closely resemble the autonomous driving situation of Car0 on unstructured roads.

[0091] For a finite state machine, there are pre-defined processing rules—the processing rules are constructed based on the vehicle states (state1~state8), vehicle behaviors, and transition conditions (M1~M11):

[0092] The vehicle states include: cruising (state 1, indicating that there are no obstacles around and the controlled vehicle travels along the reference path at a certain speed), static obstacle avoidance (state 2, indicating that when there is a static obstacle in front, the vehicle speed is reduced to ensure low-speed obstacle avoidance, and the speed is increased after obstacle avoidance is completed), dynamic obstacle avoidance (state 3, indicating that when there is a dynamic obstacle in front, speed obstacle avoidance is performed, i.e., deciding to accelerate to overtake or decelerate to avoid), following (state 4, indicating that following the vehicle in front and maintaining a certain following distance), overtaking (state 5, indicating that overtaking is performed), meeting oncoming traffic (state 6, indicating that decelerating in advance to meet oncoming traffic), following and overtaking (state 7, indicating that following one vehicle while maintaining a certain following distance and overtaking another vehicle), and stopping (state 8, indicating that decelerating and stopping).

[0093] The vehicle behaviors include: cruise control transition to static obstacle avoidance (i.e., from state 1 to state 2), static obstacle avoidance transition to cruise control (i.e., from state 2 to state 1), cruise control transition to dynamic obstacle avoidance (i.e., from state 1 to state 3), dynamic obstacle avoidance transition to cruise control (i.e., from state 3 to state 1), cruise control transition to parking (i.e., from state 1 to state 8), cruise control, static obstacle avoidance, or dynamic obstacle avoidance transition to following (i.e., from state 1, state 2, or state 3 to state 4), following, overtaking, meeting oncoming traffic, or both following and overtaking transition to cruise control (i.e., from state 4, state 5, state 6, or state 7 to state 1), following to overtaking (i.e., from state 4 to state 5), following to meeting oncoming traffic (i.e., from state 4 to state 6), following to both following and overtaking (i.e., from state 4 to state 7), and both following and overtaking transition to following (i.e., from state 7 to state 4).

[0094] The transfer conditions include:

[0095] To transition from cruise control to static obstacle avoidance, the following conditions must be met: both Ob1 and Ob2 must exist, and V... Ob1-1 <V L V Ob1-2 <V H This condition corresponds to M1.

[0096] To switch from static obstacle avoidance to cruise, the following conditions must be met: neither Ob1 nor Ob2 exists; this condition corresponds to M2.

[0097] To transition from cruise control to dynamic obstacle avoidance, the following conditions must be met: Ob1 exists, V Ob1-2 ≥V H This condition corresponds to M3.

[0098] To switch from dynamic obstacle avoidance to cruise, the following conditions must be met: neither Ob1 nor Ob2 exists; this condition corresponds to M4.

[0099] To perform a cruise transfer to parking, the following conditions must be met: neither Ob1 nor Ob2 exists, and the distance between Car0 and the end of the unstructured road is less than L0; this condition corresponds to M5.

[0100] To perform cruise control, static obstacle avoidance, or dynamic obstacle avoidance transition to following, the following conditions must be met: Ob1 exists, V Ob1-2 <V H V Ob1-1 ≥V L This condition corresponds to M6.

[0101] To switch to cruise control for following, overtaking, passing, or both, the following conditions must be met: Ob1 exists, V Ob1-1 <V L This condition corresponds to M7.

[0102] To perform a follow-to-overtake maneuver, the following conditions must be met: Ob1 exists, V Ob1-2 <V H , α*V espected ≥V Ob1-1 ≥V L D Ob1-1 Or D Ob1-2 <D0; This condition corresponds to M8.

[0103] For a transfer from following another vehicle to meeting another vehicle to proceed, the following conditions must be met: Ob1 exists, V Ob1-2 <V H V Ob1-1 <-V L This condition corresponds to M9. Note that -V L With V L The directions are opposite.

[0104] To switch from following a vehicle to both following and overtaking, the following conditions must be met: Ob2 exists, V H <V Ob2-1 <α*V espected D Ob2-1 Or D Ob2-2 ≥D0; This condition corresponds to M10.

[0105] To switch from following / overtaking to following, the following conditions must be met: Ob2 does not exist, V Ob1-1 <α*V espected This condition corresponds to M11.

[0106] Among them, V L V represents the longitudinal velocity threshold. H L0 represents the lateral speed threshold, α represents the stopping distance threshold, and D0 represents the boundary distance threshold.

[0107] V L V H The values ​​of L0, α, and D0 are adjustable. In this embodiment 1, V L Take 1 m / s, V H Take 0.3 m / s, L0 as 65 m, α as 0.8, and D0 as 4.5 m.

[0108] The above processing rules can be expressed as follows: Figure 4 This is a schematic diagram showing the transformation of vehicle status and behavior under the corresponding transfer conditions.

[0109] Real-time driving information for Ob1 and Ob2, and Vespected As input to a finite state machine.

[0110] Among them, Ob1's real-time driving information includes: Ob1's real-time longitudinal speed V Ob1-1 Real-time lateral velocity V Ob1-2 Real-time distance D from the boundary of the unstructured road. Ob1-1 The real-time distance D from the other side boundary of the unstructured road Ob1-2 ;

[0111] Ob2's real-time driving information includes: Ob2's real-time longitudinal speed V Ob2-1 Real-time lateral velocity V Ob2-2 Real-time distance D from the boundary of the unstructured road. Ob2-1 The real-time distance D from the other side boundary of the unstructured road Ob2-2 .

[0112] Based on the results of step one:

[0113] If Ob1 exists, then V Ob1-1 D Ob1-1 D Ob1-2 V Ob1-2 Values ​​are taken based on the actual situation; if Ob1 does not exist, then V Ob1-1 D Ob1-1 D Ob1-2 Both are 0, V Ob1-2 This is the maximum value (1000 m / s in this embodiment 1);

[0114] If Ob2 exists, then V Ob2-1 D Ob2-1 D Ob2-2 V Ob2-2 Values ​​are taken based on the actual situation; if Ob2 does not exist, then V Ob2-1 D Ob2-1 D Ob2-2 Both are 0, V Ob2-2 It is the maximum value (1000 in this embodiment 1).

[0115] It should be noted that Ob1 and Ob2 are set to travel at a constant speed during a single planning cycle.

[0116] Among them, V espected This represents the speed at which Car0 travels at a constant speed on an unstructured road in the absence of obstacles; its value is adjustable. In this embodiment 1, V espected Take 10 m / s.

[0117] Therefore, the finite state machine with preset processing rules processes the real-time driving information of Ob1 and Ob2, as well as V. espectedThe process yields several outputs: real-time output of the controlled vehicle and real-time output of obstacles.

[0118] The real-time output of the controlled vehicle includes: the real-time status of Car0 and the real-time reference speed V of Car0. ref .

[0119] The real-time state of Car0 is used to verify whether the finite state machine is operating normally: if the real-time state of Car0 falls into the above 8 vehicle states, it means that the finite state machine is operating normally.

[0120] Car0's real-time reference speed V ref Used to calculate the cost of the expected speed deviation C velocity —It is used in speed planning. V ref Since it is not used in this embodiment 1, it will not be described in detail here.

[0121] The real-time obstacle output includes: the obstacle type of Ob1 and the obstacle type of Ob2.

[0122] Specifically, both Ob1 and Ob2 have four possible obstacle types:

[0123] The first possible type (code 0), the second possible type (code 1), the third possible type (code 2), and the fourth possible type (code 3).

[0124] Step 3: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the path planning graph (i.e., SL graph) to obtain the selectively expanded path planning graph.

[0125] It should be noted that the selective expansion here follows the principle of projecting different types of obstacles onto the SL and ST diagrams.

[0126] For details, please refer to Figure 5 :

[0127] 1. If Ob1 or Ob2 is the first possible type, it means that this obstacle does not exist and does not appear in the path planning diagram or the speed planning.

[0128] 2. If Ob1 or Ob2 is the second possible type, it indicates that this obstacle is a static obstacle and will not be expanded in the velocity planning graph (i.e., the st graph), but only in the path planning graph—expanding outwards from the center of this obstacle into a rectangle; the side length of this rectangle is roughly estimated based on the original size of the obstacle, such as... Figure 5 As shown in (a).

[0129] 3. If Ob1 or Ob2 is the third possible type, it indicates that this obstacle is a dynamic obstacle and will not expand in the path planning graph, but only in the velocity planning graph—expanding outwards from the center of this obstacle into a parallelogram; the longer side of this parallelogram is converted based on the speed of this obstacle, and the shorter side is roughly estimated based on the original size of this obstacle, such as... Figure 5 As shown in (b).

[0130] 4. If Ob1 or Ob2 is the fourth possible type, it indicates that the obstacle is an overtaking or passing obstacle. It will not be expanded in the speed planning graph, but only in the path planning graph—the length expands in the direction of movement from the obstacle's center, and the width expands symmetrically around the center to form a rectangle. The long side of this rectangle is calculated based on the obstacle's speed, and the short side is roughly estimated based on the obstacle's original size. Figure 5 As shown in (c).

[0131] It is important to emphasize that the same obstacle cannot appear in both the speed planning map and the path planning map simultaneously as different obstacle types.

[0132] It should be noted that selective expansion can be performed on Ob1 and Ob2 in the same path planning diagram; alternatively, selective expansion can be performed on Ob1 and Ob2 in two separate path planning diagrams, and then the two path planning diagrams can be merged into one.

[0133] Step four: Use dynamic programming algorithm to process the path planning graph after selective dilation to obtain the path convex space and local path coarse solution.

[0134] Dynamic programming (DP) is used to solve problems with optimal properties. In such problems, there may be many feasible solutions, each corresponding to a value. The goal of dynamic programming is to find the solution with the optimal value.

[0135] In step four, the dynamic programming algorithm discretizes the path search space in the Frenet coordinate system in the path planning graph after selective inflation. Then, it uses sampling search in the horizontal and vertical directions to obtain the candidate positions of the controlled vehicle at each time step. Finally, it uses a fifth-order polynomial to connect all possible candidate positions with adjacent path points to obtain several candidate paths.

[0136] The dynamic programming algorithm in step four uses a cost function C. total_path C total_path It includes four parts: Collision Risk Cost C under Path Dynamic Programming obstacle Path similarity cost C reference Smoothing cost C smoothness Historical trajectory cost C HistoricalRoute .

[0137] It needs to be emphasized that C total_path It is an adjusted cost function that increases C. HistoricalRoute .

[0138] 1. C obstacle The risk of collision for the controlled vehicle under path planning is represented by the following expression:

[0139] C obstacle =∑h[(s i -s) 2 +(l i -l) 2 ];

[0140] In the formula: (s i ,l i (i) represents the position of the controlled vehicle at time i in the path planning graph after selective dilation; (s,l) represents the position of the obstacles near the controlled vehicle in the path planning graph after selective dilation.

[0141] h(.) is a user-defined function;

[0142] In the formula, x represents the independent variable of h(.); k represents the collision gain coefficient; b represents the collision gain bonus; d1 represents the maximum impact distance of the obstacle; and d2 represents the safety threshold distance.

[0143] 2. C reference The expression representing that the controlled vehicle stays close to the global reference path and cannot deviate significantly from the reference line during driving is as follows:

[0144] C reference =∑l i .

[0145] 3. C smoothness The balance of the controlled vehicle's path is characterized by the following expression:

[0146] C smoothness =∑[f′(s) i )] 2 +∑[f″(s i )] 2 +∑[f″′(s i )] 2 ;

[0147] In the formula, f′(s i ) represents the first derivative of the candidate path, which is related to the heading deviation of the controlled vehicle; f″(s i ) represents the second derivative of the candidate path, which is related to the curvature of the path; f″′(s i) represents the third derivative of the candidate path, which is related to the rate of change of curvature of the candidate path.

[0148] 4. C HistoricalRoute The influence of historical trajectories is represented—introducing this ensures the uniqueness of the minimum value, improves decision-making instability without affecting obstacle avoidance capabilities, and thus avoids the high-frequency decision instability mentioned in the background technology.

[0149] C HistoricalRoute The expression is:

[0150] C HistoricalRoute =∑(l i -l i-T ) 2 ;

[0151] In the formula, l i Let l be the vertical distance of a column of the candidate path at time i; i-T The vertical distance in the same column of the candidate paths in the previous planning period; T represents the planning period, which is generally taken as 0.1s.

[0152] The sum of the squares of the differences between the two values ​​in each column, multiplied by the weight, is used as the historical trajectory cost term. The smaller the value, the smaller the deviation between the path in this planning cycle and the path in the previous planning cycle. If the controlled vehicle needs to avoid an obstacle, assuming it chose to bypass the obstacle on the left in the previous planning cycle, then when making a decision again in the next planning cycle, it will refer to the trajectory of bypassing the obstacle on the left in the previous planning cycle—the closer the distance, the smaller the cost. This eliminates the minimum value of bypassing the obstacle on the right.

[0153] In summary, C total_path Represented as:

[0154] In the formula, ω obstacle Indicate C obstacle The weights; ω reference Indicate C reference The weights; ω smoothness Indicate C smoothness The weights; ω HistoricalRoute Indicate C HistoricalRoute The weight.

[0155] It is important to note that ω HistoricalRoute The weight should be smaller than other weights to prevent over-referencing historical trajectories and affecting obstacle avoidance capabilities.

[0156] Based on the above design, C total_path According to C total_path Calculate the cost of all candidate paths;

[0157] The candidate path with the lowest value is replaced as the local coarse solution, and a path convex space is defined based on the obstacle avoidance direction. Specifically, the local coarse solution cannot be used as the actual path; its main purpose is to create a path convex space for subsequent standard quadratic programming: when an obstacle that needs to be avoided is detected, the decision to bypass it from the left or right is made based on the path given by dynamic programming. If the obstacle is bypassed from the left, then all solutions that bypass from the right are removed, and the remaining space is the path convex space that can be solved using standard quadratic programming. Other bypass cases are similar and will not be elaborated further.

[0158] Example 2

[0159] This embodiment 2 is an improvement on the overall planning and decision-making method, used to dynamically plan the globally optimal trajectory of the controlled vehicle traveling on unstructured roads.

[0160] In other words, this embodiment 2 is based on embodiment 1, with the addition of speed planning and subsequent standard quadratic planning.

[0161] See Figure 6 This document presents a main flowchart of a dynamic planning method for unstructured roads provided in Embodiment 2, which includes the following steps:

[0162] Step 1: Obtain the location of the controlled vehicle Car0 on the unstructured road within the current planning period, and perform real-time obstacle screening within a preset range around Car0 to identify the first obstacle Ob1 and the second obstacle Ob2 that affect Car0's planning.

[0163] Step 2: Ob1 and Ob2's real-time driving information and compare it with the set desired speed V. espected The system uses a finite state machine with pre-defined processing rules to process the data and obtain the real-time output of the controlled vehicle and the real-time output of the obstacle.

[0164] Step 3: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the path planning graph to obtain the selectively expanded path planning graph.

[0165] Step four: Use dynamic programming algorithm to process the path planning graph after selective dilation to obtain the path convex space and local path coarse solution.

[0166] Steps one through four can be found in the description of Example 1, and will not be repeated here.

[0167] Step 5: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the velocity planning graph to obtain the selectively expanded velocity planning graph.

[0168] Step 5 can also be found in the description of selective expansion of Ob1 and Ob2 in Example 1, and will not be repeated here.

[0169] It should be noted that selective expansion of Ob1 and Ob2 can be performed on the same velocity planning chart; alternatively, selective expansion of Ob1 and Ob2 can be performed on two separate velocity planning charts, and then the two velocity planning charts can be merged into one.

[0170] Step 6: Use dynamic programming algorithm to process the velocity planning graph after selective dilation to obtain the velocity convex space and local velocity coarse solution.

[0171] Similar to step four, this step also uses a dynamic programming algorithm.

[0172] In step six, the dynamic programming algorithm discretizes the velocity search space in the Frenet coordinate system in the velocity planning graph after selective dilation. Then, it uses a sampling search method in the horizontal and vertical directions to obtain the longitudinal displacement of the controlled vehicle at each time step. Finally, it uses simple straight lines to connect all adjacent points to obtain several candidate velocity clusters.

[0173] The dynamic programming algorithm in step six uses a cost function C. total_speed C total_speed It includes four parts: Collision risk cost C′ under velocity dynamic programming. obstacle Expected speed deviation cost C velocity Comfort Cost C accelerate , acceleration cost C jerk .

[0174] It should be noted that, with C total_path Different, C total_speed It is a standard cost function and has not been adjusted.

[0175] 1. C′ obstacle The expression representing the risk of a collision for the controlled vehicle under speed planning is as follows:

[0176]

[0177] In the formula, d represents the straight-line distance between the controlled vehicle and surrounding obstacles in the speed programming diagram after selective expansion; m represents the gain coefficient of the obstacle.

[0178] 2. C velocity The velocity obtained by velocity programming should be as close as possible to V. ref Its expression is:

[0179] C velocity =(s′-V ref ) 2;

[0180] In the formula, s′ represents the speed of the controlled vehicle obtained from speed planning.

[0181] 3. C accelerate The acceleration of the controlled vehicle is expressed as:

[0182] C accelerate =(s″) 2 ;

[0183] In the formula, s″ represents the acceleration of the controlled vehicle obtained from speed planning.

[0184] 4. C jerk The instantaneous rate of change of acceleration, which characterizes the acceleration of the controlled vehicle, is expressed as:

[0185] C jerk =(s″′) 2 ;

[0186] In the formula, s″′ represents the acceleration of the controlled vehicle obtained from speed planning.

[0187] In summary, C total_speed Represented as:

[0188] In the formula, ω′ obstacle Represents C′ obstacle The weighting coefficient; ω velocity Indicate C velocity The weight; representing, ω accelerate Indicate C accelerate The weights; ω jerk Represented as C jerk The weight.

[0189] In addition, C total_speed It also satisfies constraints—including vehicle kinematic constraints and dynamic constraints—which can be expressed as: In the formula, v min v max These represent the minimum and maximum speeds of the controlled vehicle, respectively; a min Indicates the maximum acceleration of the controlled vehicle during deceleration; a max This indicates the maximum acceleration of the controlled vehicle when it decelerates.

[0190] Based on the above design, C total_speed According to C total_speed Calculate the cost of all candidate velocity clusters;

[0191] The candidate velocity cluster with the lowest value is replaced as the local coarse velocity solution, and a velocity convex space is defined based on the obstacle avoidance direction. Specifically, the local coarse velocity solution cannot be used as the actual control velocity; its main purpose is to open up the velocity convex space for subsequent standard quadratic programming: when an obstacle that needs to be avoided is detected, the decision to bypass it from the left or right is made based on the velocity given by dynamic programming. If the obstacle is bypassed from below, then all solutions that bypass from above are removed, and the remaining space is the velocity convex space that can be solved using standard quadratic programming. Other bypass cases are similar and will not be elaborated further.

[0192] Step 7: Use standard quadratic programming to solve for the optimal local path based on the path convex space and local path coarse solution;

[0193] The optimal local velocity is obtained by using standard quadratic programming based on the velocity convex space and local velocity coarse solution.

[0194] The optimal local trajectory is obtained by combining the optimal local path and the optimal local velocity.

[0195] Among them, standard quadratic programming (QP) is a special type of nonlinear programming and a commonly used quadratic programming method. It mainly includes: planning that considers obstacle avoidance factors and smoothing processing, so as to obtain the optimal local path and the optimal local speed, and then assemble the optimal local trajectory—that is, as the basis for decision control.

[0196] Step 8: As the current planning cycle is updated, repeat steps 1 through 7 to form the global optimal trajectory from the optimal local trajectories.

[0197] Car0 is controlled to travel according to the optimal local trajectory obtained in the current planning cycle. As time progresses, the current planning cycle is updated. Steps one through seven are then repeated to obtain the updated optimal local trajectory for the current planning cycle, which is then used to control Car0. This forms a loop, and the optimal local trajectories together constitute the globally optimal trajectory.

[0198] Simulation verification

[0199] This embodiment 2 also simulates the above-mentioned dynamic planning method for unstructured roads to verify the effectiveness of the method.

[0200] The simulation platform was built using simulation software: an unstructured road model was created in Prescan; planning and control were written in Simulink; and the controlled vehicle Car0 model was selected in Carsim.

[0201] 1. Verify the effectiveness of the dynamic programming method with the adjusted cost function:

[0202] See Figure 7 Scenario 1: There is a static obstacle in front of Car0; Car0's desired speed is 7 m / s; Dynamic programming of a coarse local path is started 50 meters away from the static obstacle; The shortest distance between the coarse local path and the static obstacle is dl, such as... Figure 7 As shown, there are two coarse local path solutions (bypassing from above and by bypassing from below). The distance between Car0 and the static obstacle is ds; the planning period is 0.1s.

[0203] Each adopted an additional C HistoricalRoute The cost function, without increasing C HistoricalRoute The cost function is used to calculate the coarse solution of the local path. See the comparison results. Figure 8 , Figure 9 : Figure 8 It demonstrates the use of C without adding C HistoricalRoute The case of the cost function; Figure 9 The demonstration showed the adoption of C. HistoricalRoute The case of the cost function.

[0204] It can be seen that the addition of C HistoricalRoute The cost function shows that the dynamic programming trajectory no longer swings left and right, the decision is more stable, and it always chooses to bypass obstacles from the left side throughout the obstacle avoidance process. Moreover, the overall fluctuation of dl has also decreased, indicating that the smoothness of the initial path planning has been improved.

[0205] 2. Verify the effectiveness of the dynamic programming method for unstructured roads:

[0206] Scenario 2: Car0 is in the reference position Figure 10 The unstructured road model is used, and several obstacles and behavior transfer conditions are set in it.

[0207] The unstructured road is 6 meters wide and 2328 meters long; the desired speed of Car0 is 10 m / s; the planning cycle is 0.1 s and the control cycle is 0.01 s; the entire simulation takes 280 s. See simulation results below. Figures 12-17 .

[0208] See Figure 11 It shows the real-time state of Car0 output by the wired state machine, indicating that the finite state machine can perform normal state transitions and realizes 11 behavior transitions between 8 states.

[0209] Specifically, at t=24.7s, the vehicle avoided the first group of static obstacles; at t=32.1s, it avoided the second group of static obstacles; at t=75.6s, it followed another vehicle; at t=92.3s, it overtook another vehicle; at t=117.9s, it followed one vehicle while preparing to overtake another; at t=140.2s, it followed one vehicle while overtaking another; at t=172.8s, it passed another vehicle; and at t=209.2s, it avoided a dynamic obstacle.

[0210] See Figure 12 , 13 14, 15, and 16 respectively show the longitudinal acceleration, longitudinal velocity, lateral acceleration, steering wheel angle, and heading angle of Car0 throughout the entire process.

[0211] The maximum longitudinal acceleration throughout the entire process did not exceed 4 m / s². 2 The minimum value is not lower than -6m / s 2 When there are no obstacles, it can cruise at a speed of 10 m / s, and the speed is generally no greater than 10 m / s throughout the entire journey. The speed curve is smooth overall, indicating that Car0 can make corresponding speed changes when switching between different states. The lateral acceleration is less than 0.2 times the acceleration due to gravity throughout the journey, indicating that the tire lateral deviation is within the linear range, resulting in high safety. The steering wheel angle is no greater than 60 degrees throughout the journey, and the curve is smooth with a certain frequency of change, which is quite consistent with actual conditions. The steering wheel change amplitude is relatively stable throughout the journey, indicating stable operation.

[0212] See Figure 17 It displays the entire vehicle trajectory of Car0, which is compared with... Figure 11 The unstructured road model remains consistent.

[0213] The simulation results show that the dynamic programming method for unstructured roads can plan a globally optimal path with good results, and also demonstrate that the local path planning method for unstructured roads is reliable.

[0214] Example 3

[0215] This embodiment 3 discloses a computer device, including a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the steps of the local path planning method for unstructured roads disclosed in embodiment 1, and / or the steps of the dynamic planning method for unstructured roads disclosed in embodiment 2.

[0216] This embodiment 3 also discloses a readable storage medium storing computer program instructions. When the computer program instructions are read and executed by a processor, they perform the steps of the local path planning method for unstructured roads disclosed in embodiment 1, or / and the steps of the dynamic planning method for unstructured roads disclosed in embodiment 2.

[0217] This embodiment 3 also discloses a computer program product, including a computer program. When executed by a processor, the computer program implements the steps of the local path planning method for unstructured roads disclosed in embodiment 1, and / or the steps of the dynamic planning method for unstructured roads disclosed in embodiment 2.

[0218] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.

Claims

1. A local path planning method for unstructured roads, characterized in that, include: Step 1: Obtain the location of the controlled vehicle Car0 on the unstructured road within the current planning period, and perform real-time obstacle screening within a preset range around Car0 to identify the first obstacle Ob1 and the second obstacle Ob2 that affect Car0's planning. Step 2: Ob1 and Ob2's real-time driving information and compare it with the set desired speed V. espected The system uses a finite state machine with pre-defined processing rules to process the data and obtain the real-time output of the controlled vehicle and the real-time output of the obstacle. Among them, V espected This represents the speed at which Car0 travels at a constant speed on an unstructured road in the absence of obstacles. The real-time output of the controlled vehicle includes: the real-time state of Car0; the real-time state of Car0 is used to verify whether the finite state machine is operating normally. The real-time obstacle output includes: the obstacle type of Ob1 and the obstacle type of Ob2; Step 3: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the path planning graph to obtain the selectively expanded path planning graph. Step 4: Use dynamic programming algorithm to process the path planning graph after selective dilation to obtain the path convex space and coarse local path solutions. In step four, the dynamic programming algorithm uses a cost function C. total_path ; In the formula, C obstacle ω represents the collision risk cost under path dynamic programming. obstacle Indicate C obstacle The weight of C; reference ω represents the path similarity cost. reference C represents reference The weight of C; smoothness ω represents the smoothness cost. smoothness Indicate C smoothness The weight of C; HistoricalRoute ω represents the cost of historical trajectory. HistoricalRoute Indicate C HistoricalRoute The weight.

2. The local path planning method for unstructured roads according to claim 1, characterized in that, In step one, the preset range around Car0 is: the road segment formed by the distance from Car0 behind along the s-axis that is less than L1 and the distance from Car0 in front along the s-axis that is less than L2. Where L1 represents the rear distance threshold and L2 represents the front distance threshold.

3. The local path planning method for unstructured roads according to claim 2, characterized in that, In step one, the methods for real-time obstacle screening include: Real-time detection of whether there are M obstacles within a preset range around Car0; When M = 0, neither Ob1 nor Ob2 exists; When M=1, the obstacle is designated as Ob1, while Ob2 does not exist. When M≥2: If there is a nearest obstacle, obstacle3, behind Car0, and the longitudinal velocity of obstacle3 is greater than or equal to the longitudinal velocity threshold V. L If the condition is met, then obstacle3 is designated as Ob1; otherwise, the nearest obstacle in front of Car0, obstacle1, is designated as Ob1, and the second nearest obstacle in front of Car0, obstacle2, is designated as Ob2. If obstacle3 is considered as Ob1, and there is a nearest obstacle obstacle1 in front of Car0, then obstacle1 is considered as Ob2; otherwise, Ob2 does not exist.

4. The local path planning method for unstructured roads according to claim 3, characterized in that, In step two, Ob1's real-time driving information includes: Ob1's real-time longitudinal speed V Ob1-1 Real-time lateral velocity V Ob1-2 Real-time distance D from the boundary of the unstructured road. Ob1-1 The real-time distance D from the other side boundary of the unstructured road Ob1-2 ; Ob2's real-time driving information includes: Ob2's real-time longitudinal speed V Ob2-1 Real-time lateral velocity V Ob2-2 Real-time distance D from the boundary of the unstructured road. Ob2-1 The real-time distance D from the other side boundary of the unstructured road Ob2-2 ; If Ob1 does not exist, then V Ob1-1 D Ob1-1 D Ob1-2 All are 0, V Ob1-2 It is the maximum value; If Ob2 does not exist, then V Ob2-1 D Ob2-1 D Ob2-2 All are 0, V Ob2-2 It is a maximum value.

5. The local path planning method for unstructured roads according to claim 1, characterized in that, In step two, the processing rules are constructed based on vehicle status, vehicle behavior, and transfer conditions; The vehicle status includes: cruise, static obstacle avoidance, dynamic obstacle avoidance, following, overtaking, meeting oncoming traffic, following and overtaking, and parking. Among them, vehicle behaviors include: cruise control shift to static obstacle avoidance, static obstacle avoidance shift to cruise control, cruise control shift to dynamic obstacle avoidance, dynamic obstacle avoidance shift to cruise control, cruise control shift to parking, cruise control or static obstacle avoidance or dynamic obstacle avoidance shift to following, following or overtaking or meeting oncoming traffic or following and overtaking shift to cruise control, following to overtaking, following to meeting oncoming traffic, following to following and overtaking, following and overtaking shift to following; The transfer conditions include: To transition from cruise control to static obstacle avoidance, the following conditions must be met: both Ob1 and Ob2 must exist, and V... Ob1-1 <V L V Ob1-2 <V H ; To switch from static obstacle avoidance to cruise, the following conditions must be met: neither Ob1 nor Ob2 exists. To transition from cruise control to dynamic obstacle avoidance, the following conditions must be met: Ob1 exists, V Ob1-2 ≥V H ; For dynamic obstacle avoidance to transition to cruise, the following conditions must be met: neither Ob1 nor Ob2 exists. To perform a cruise transfer to parking, the following conditions must be met: neither Ob1 nor Ob2 exists, and the distance between Car0 and the end of the unstructured road is less than L0. To perform cruise control, static obstacle avoidance, or dynamic obstacle avoidance transition to following, the following conditions must be met: Ob1 exists, V Ob1-2 <V H V Ob1-1 ≥V L ; To switch to cruise control for following, overtaking, passing, or both, the following conditions must be met: Ob1 exists, V Ob1-1 <V L ; To perform a follow-to-overtake maneuver, the following conditions must be met: Ob1 exists, V Ob1-2 <V H , α*V espected ≥V Ob1-1 ≥V L D Ob1-1 Or D Ob1-2 <D0; For a transfer from following another vehicle to meeting another vehicle to proceed, the following conditions must be met: Ob1 exists, V Ob1-2 <V H V Ob1-1 <-V L ; To switch from following a vehicle to both following and overtaking, the following conditions must be met: Ob2 exists, V H <V Ob2-1 <α*V espected D Ob2-1 Or D Ob2-2 ≥D0; To switch from following / overtaking to following, the following conditions must be met: Ob2 does not exist, V Ob1-1 <α*V espected ; Among them, V L V represents the longitudinal velocity threshold. H L0 represents the lateral speed threshold, α represents the stopping distance threshold, and D0 represents the boundary distance threshold.

6. The local path planning method for unstructured roads according to claim 5, characterized in that, In step two, both Ob1 and Ob2 have four possible obstacle types. If Ob1 or Ob2 is the first possible type, it means that this obstacle does not exist and does not appear in the path planning graph; If Ob1 or Ob2 is the second possible type, it means that this obstacle is a static obstacle and is only expanded in the path planning graph; If Ob1 or Ob2 is the third possible type, it means that this obstacle is a dynamic obstacle and will not be expanded in the path planning graph; If Ob1 or Ob2 is the fourth possible type, it means that the obstacle is an overtaking or passing obstacle, and it is only expanded in the path planning map.

7. The local path planning method for unstructured roads according to claim 6, characterized in that, In step four, the dynamic programming algorithm generates several candidate paths from the selectively expanded path planning graph, and then selects the appropriate path based on C. total_path Calculate the cost of all candidate paths; The candidate path with the lowest value is replaced as the local coarse solution, and the path convex space is divided according to the obstacle avoidance direction.

8. A dynamic programming method for unstructured roads, characterized in that, It is used to dynamically plan the globally optimal trajectory of a controlled vehicle traveling on unstructured roads; The dynamic programming method for unstructured roads includes: The local path planning method for unstructured roads as described in any one of claims 1-7; In step two, the real-time output of the controlled vehicle also includes: the real-time reference speed V of Car0. ref V ref Used to calculate the cost of the expected speed deviation C velocity ; If Ob1 or Ob2 is the first possible type, it means that this obstacle does not exist and will not appear in the speed planning. If Ob1 or Ob2 is a third possible type, then expansion is performed only in the velocity planning graph; The dynamic programming method for unstructured roads also includes: Step 5: Based on the real-time obstacle output, selectively expand Ob1 and Ob2 in the velocity planning graph to obtain the selectively expanded velocity planning graph. Step 6: Use dynamic programming algorithm to process the velocity planning graph after selective dilation to obtain the velocity convex space and local velocity coarse solution; In step six, the dynamic programming algorithm uses a cost function C. total_speed ; In the formula, C o ′ bstacle ω represents the collision risk cost under velocity dynamic programming. o ′ bstacle Indicate C o ′ bstacle Weighting coefficients; C velocity ω represents the cost of the expected speed deviation. velocity Indicate C velocity The weight of C; accelerate ω represents the cost of comfort. accelerate Indicate C accelerate The weight of C; jerk ω represents the cost of acceleration. jerk Represented as C jerk The weights; Step 7: Use standard quadratic programming to solve for the optimal local path based on the path convex space and local path coarse solution; The optimal local velocity is obtained by using standard quadratic programming based on the velocity convex space and local velocity coarse solution. The optimal local trajectory is obtained by combining the optimal local path and the optimal local velocity. Step 8: As the current planning cycle is updated, repeat steps 1 through 7 to form the global optimal trajectory from the optimal local trajectories.

9. The dynamic programming method for unstructured roads according to claim 8, characterized in that, In step four, the dynamic programming algorithm generates several candidate velocity clusters from the selectively dilated velocity programming graph, and then, based on C... total_speed Calculate the cost of all candidate velocity clusters; The candidate velocity cluster with the lowest value is replaced as the local coarse velocity solution, and the velocity convex space is divided according to the obstacle avoidance direction.

10. A computer program product, characterized in that, Includes a computer program; when executed by a processor, the computer program implements the steps of the local path planning method for unstructured roads as described in any one of claims 1-7, and / or the steps of the dynamic planning method for unstructured roads as described in any one of claims 8-9.

Images