Local path planning method and dynamic planning method for unstructured road

Abstract

The invention relates to the technical field of intelligent traffic control, in particular to a local path planning method and a dynamic planning method for an unstructured road. According to the method, firstly, real-time obstacle screening is carried out around the controlled vehicle to cover a multi-obstacle scene, then the finite-state machine is introduced, the finite-state machine is combined with dynamic planning with a cost function adjusted during path planning, and stable switching of different driving behaviors can be carried out in an unstructured road scene. Wherein the finite-state machine can accurately judge the type of the obstacle, selective obstacle expansion is facilitated, and a reliable obstacle avoidance basis is provided for path planning. The dynamic planning of the cost function is adjusted, so that the controlled vehicle is not easy to be unstable during high-frequency decision making, and the smoothness of an initial planning track is also improved. According to the method, the problems that the condition of coexistence of multiple obstacles is not considered in the path planning of the existing unstructured road and instability is easy to occur during high-frequency decision making are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent traffic control, and more specifically, to: 1. A dynamic programming method for unstructured roads; 2. A dynamic programming method for unstructured roads. Background Art

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

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

[0004] Currently, there are few studies on unstructured autonomous driving, but its goal is similar to that of structured autonomous driving - both require trajectory planning to obtain the optimal trajectory. Trajectory planning is carried out from two aspects: path planning and speed planning, and then the planning results of these two aspects are combined.

[0005] However, if the existing planning and decision-making methods are applied to unstructured scenarios, there will be the following limitations in path trajectories:

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

[0007] 2. They often only consider single independent scenarios for analysis and testing, and pay less attention to the repeated switching of continuous scenarios during driving, and pay less attention to the high-frequency decision-making instability phenomenon that occurs under extremely special circumstances. For example, since a single planning cycle is very short, there may be multiple minimum values ​​when making decisions, making the local path rough solution non-unique, resulting in vacillating decisions and frequent changes in decisions. At the same time, it will also cause repeated fluctuations in planning and poor smoothness, which will seriously affect driving safety. Summary of the invention

[0008] Based on this, it is necessary to provide a dynamic planning method and device for unstructured roads, in order to address the problem that the existing path planning of unstructured roads does not take into account the coexistence of multiple obstacles and is prone to instability during high-frequency decision-making.

[0009] The present invention is implemented by the following technical solutions:

[0010] In a first aspect, the present invention discloses a dynamic planning method for an unstructured road, comprising:

[0011] Step 1: Obtain the position of the controlled vehicle Car0 on the unstructured road in the current planning cycle, and perform real-time obstacle screening within a preset range around Car0 to determine obstacle No. 1 Ob1 and obstacle No. 2 Ob2 that affect the planning of Car0;

[0012] Step 2: Obtain the real-time driving information of Ob1 and Ob2, and compare it with the set expected speed V espected A finite state machine with preset processing rules is used for processing to obtain real-time output of controlled vehicles and real-time output of obstacles;

[0013] Among them, V espected represents the speed of Car0 traveling at a constant speed on an unstructured road without any obstacles;

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

[0015] The real-time obstacle output includes: obstacle type to which Ob1 belongs, obstacle type to which Ob2 belongs;

[0016] Step 3: selectively expand Ob1 and Ob2 in the path planning graph according to the real-time obstacle output to obtain a selectively expanded path planning graph;

[0017] Step 4: Use a dynamic programming algorithm to process the selectively expanded path planning graph to obtain a path convex space and a local path rough solution;

[0018] Among them, the dynamic programming algorithm in step 4 adopts the cost function C total_path ;

[0019]

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

[0021] This dynamic planning method for unstructured roads implements the method or process according to an embodiment of the present disclosure.

[0022] In a second aspect, the present invention discloses a dynamic planning method for unstructured roads, which is used to dynamically plan the optimal trajectory of a controlled vehicle traveling on the unstructured road.

[0023] Dynamic programming methods for unstructured roads include:

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

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

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

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

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

[0029] Step 5: selectively expand Ob1 and Ob2 in the speed planning map according to the real-time obstacle output to obtain a speed planning map after selective expansion;

[0030] Step 6, using a dynamic programming algorithm to process the selectively expanded velocity planning graph to obtain a velocity convex space and a local velocity rough solution;

[0031] Among them, the dynamic programming algorithm in step six adopts the cost function C total_speed ;

[0032]

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

[0034] Step 7, using standard quadratic programming to solve the optimal local path based on the path convex space and the local path rough solution;

[0035] Standard quadratic programming is used to solve the optimal local speed based on the speed convex space and the local speed rough solution;

[0036] Assemble the optimal local path and the optimal local speed together to obtain the optimal local trajectory;

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

[0038] This dynamic planning method for unstructured roads implements the method or process according to an embodiment of the present disclosure.

[0039] In a third aspect, the present invention discloses a computer program product, including a computer program. When the computer program is executed by a processor, the steps of the local path planning method for unstructured roads disclosed in the first aspect or / and the steps of the dynamic planning method for unstructured roads disclosed in the second aspect are implemented.

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

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

[0042] 2. The present invention combines a finite state machine with dynamic programming with an adjusted cost function when performing path planning, and can perform stable switching between different driving behaviors in unstructured road scenarios; wherein, the dynamic programming with an adjusted cost function can ensure that the controlled vehicle is not prone to instability during high-frequency decision-making, while also improving the smoothness of the initial planned trajectory.

[0043] 3. The present invention combines a finite state machine with dynamic programming using a conventional cost function when performing speed planning, and can perform a balanced transition of speed control in unstructured road scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

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

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

[0047] Figure 3 This is a schematic diagram of the danger of overtaking without considering the expansion of obstacles in Embodiment 1 of the present invention;

[0048] Figure 4 Schematic diagram of processing rules in Example 1 of the present invention;

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

[0050] Figure 6 A flow chart of a dynamic planning method for unstructured roads provided in Embodiment 2 of the present invention;

[0051] Figure 7 Scenario 1 provided for Embodiment 2 of the present invention;

[0052] Figure 8 The embodiment 2 of the present invention adopts the method without increasing C HistoricalRoute The result graph of the local path rough solution calculated by the cost function;

[0053] Fig. 9 The embodiment 2 of the present invention adopts the addition of C HistoricalRoute The result graph of the local path rough solution calculated by the cost function;

[0054] Fig.10 The unstructured road model provided in Embodiment 2 of the present invention;

[0055] Fig.11 The real-time state diagram of Car0 output by the wired state machine in Embodiment 2 of the present invention;

[0056] Fig.12 is the longitudinal acceleration diagram of Car0 throughout the entire process in Example 2 of the present invention;

[0057] Fig.13 is a longitudinal speed diagram of Car0 throughout the entire journey in Example 2 of the present invention;

[0058] Fig.14 is a lateral acceleration diagram of Car0 throughout the entire process in Example 2 of the present invention;

[0059] Fig.15 is a steering wheel angle diagram of the entire journey of Car0 in Example 2 of the present invention;

[0060] Fig.16 is a heading angle diagram of the entire journey of Car0 in Example 2 of the present invention;

[0061] Fig.17 This is the vehicle trajectory diagram of the entire journey of Car0 in Example 2 of the present invention. DETAILED DESCRIPTION

[0062] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0063] It should be noted that when a component is referred to as being "mounted on" another component, it may be directly on the other component or there may be a central component. When a component is considered to be "set on" another component, it may be directly set on the other component or there may be a central component at the same time. When a component is considered to be "fixed to" another component, it may be directly fixed on the other component or there may be a central component at the same time.

[0064] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which the present invention belongs. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "or / and" used herein includes any and all combinations of one or more of the related listed items.

[0065] Example 1

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

[0067] See also Figure 1 , which shows a main flow chart of a local path planning method for unstructured roads provided in this embodiment 1, which includes:

[0068] Step 1: Obtain the position of the controlled vehicle Car0 on the unstructured road in the current planning cycle, and perform real-time obstacle screening within a preset range around Car0 to determine obstacle No. 1 Ob1 and obstacle No. 2 Ob2 that affect the planning of Car0.

[0069] For Car0, it can sense all obstacles within a preset range around it - obstacle 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 an unstructured road to detect obstacle information.

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

[0071] Generally speaking, the preset range around Car0 is set as a road section formed by a distance behind Car0 along the s-axis direction less than L1 and a distance in front of Car0 along the s-axis direction less than L2, where L1 represents a rear distance threshold and L2 represents a front distance threshold.

[0072] For the road section where the distance behind Car0 along the s-axis direction is ≥ L1 or the distance in front of Car0 along the s-axis direction is ≥ L2. Even if there is an obstacle, the impact on Car0 can be ignored due to the large distance from Car0. The values ​​of L1 and L2 can be set according to the specific situation. 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 the situation where following a car and overtaking exist at the same time. It is necessary to detect two obstacles close to Car0 in real time. It is difficult to handle such situations when only one obstacle is detected.

[0074] Specifically, the real-time obstacle screening method includes:

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

[0076] ①When M=0, Ob1 and Ob2 do not exist;

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

[0078] ③When M≥2:

[0079] If there is a nearest obstacle obstacle3 behind Car0, and the longitudinal speed of obstacle3 is greater than or equal to the longitudinal speed threshold V L, then obstacle3 is taken as Ob1; otherwise, obstacle1, the closest obstacle in front of Car0, is taken as Ob1, and obstacle2, the second closest obstacle in front of Car0, is taken as Ob2;

[0080] When obstacle3 is used as Ob1, if there is a nearest obstacle obstacle1 in front of Car0, obstacle1 is used as Ob2; otherwise, Ob2 does not exist.

[0081] For easier understanding, see Figure 2 , showing that there are two relatively close obstacles, obstacle1 and obstacle2, in front of Car0, and the closest obstacle, obstacle3, behind it:

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

[0083] Then, according to the provisions of ③:

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

[0085] (2) If the real-time longitudinal velocity of obstacle 3 is less than V L , then the possibility that obstacle3 is a longitudinally moving vehicle is ruled out, then obstacle1 is taken as Ob1 and obstacle2 is taken as Ob2.

[0086] The above rules ①, ②, and ③ are adopted for the subsequent obstacle expansion to better handle overtaking scenarios.

[0087] See also Figure 3 ,Take the overtaking situation as an example: if the obstacle screening of the above rules is not performed, when Car0 just overtakes the vehicle in front that needs to be overtaken, the overtaken vehicle will be regarded as an ordinary static obstacle, and the controlled vehicle will move closer to the global reference line, such as Figure 3 (a) If the expansion of the barrier of the vehicle being overtaken is ignored, the distance between the two vehicles may be too close when the overtaking is completed, which is more dangerous. Figure 3 (b) as shown.

[0088] Step 2: Obtain the real-time driving information of Ob1 and Ob2, and compare it with the set expected speed V espectedA finite state machine with preset processing rules is used for processing to obtain real-time output of the controlled vehicle and real-time output of the obstacle.

[0089] Finite state machine (FSM) is a tool used to model object behavior. Its main function is to describe the state sequence 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 to be closer to the autonomous driving situation of Car0 on unstructured roads.

[0091] For the finite state machine, it presets processing rules - the processing rules are constructed based on the vehicle state (state1~state8), vehicle behavior, and transfer conditions (M1~M11):

[0092] Among them, the vehicle states include: cruising (i.e. state1, indicating: there are no obstacles around, and the controlled vehicle travels along the reference path at a certain speed), static obstacle avoidance (i.e. state2, indicating: when there is a static obstacle in front, reduce the speed to ensure low-speed obstacle avoidance, and speed up when the obstacle avoidance is completed), dynamic obstacle avoidance (i.e. state3, indicating: when there is a dynamic obstacle in front, speed obstacle avoidance is performed, that is, it is decided to accelerate to overtake or decelerate to avoid), following the car (i.e. state4, indicating: following the car and keeping a certain following distance with the car in front), overtaking (i.e. state5, indicating: overtaking), meeting (i.e. state6, indicating: slowing down in advance to meet), following and overtaking (i.e. state7, indicating: following a car with a certain following distance and overtaking another car), and parking (i.e. state8, indicating: decelerating and stopping).

[0093] Among them, vehicle behaviors include: cruising transfer to static obstacle avoidance (i.e. from state1 to state2), static obstacle avoidance transfer to cruising (i.e. from state2 to state1), cruising transfer to dynamic obstacle avoidance (i.e. from state1 to state3), dynamic obstacle avoidance transfer to cruising (i.e. from state3 to state1), cruising transfer to parking (i.e. from state1 to state8), cruising or static obstacle avoidance or dynamic obstacle avoidance transfer to following car (i.e. from state1 or state2 or state3 to state4), following car or overtaking or meeting car or following car and overtaking transfer to cruising (i.e. from state4 or state5 or state6 or state7 to state1), following car transfer to overtaking (i.e. from state4 to state5), following car transfer to meeting car (i.e. from state4 to state6), following car transfer to following car and overtaking (i.e. from state4 to state7), following car and overtaking transfer to following car (i.e. from state7 to state4).

[0094] The transfer conditions include:

[0095] If you want to transfer from cruising to static obstacle avoidance, the following conditions must be met: Ob1 and Ob2 both exist, V Ob1-1 <V L , V Ob1-2 <V H ; This condition corresponds to M1.

[0096] If static obstacle avoidance is to be transferred to cruising, the following conditions must be met: Ob1 and Ob2 do not exist; this condition corresponds to M2.

[0097] If you want to transfer from cruising to dynamic obstacle avoidance, the following conditions must be met: Ob1 exists, V Ob1-2 ≥V H ; This condition corresponds to M3.

[0098] If dynamic obstacle avoidance is to be transferred to cruising, the following conditions must be met: Ob1 and Ob2 do not exist; this condition corresponds to M4.

[0099] If cruising is to be transferred to parking, the following conditions must be met: Ob1 and Ob2 do not exist, and the distance between Car0 and the end of the unstructured road is less than L0; this condition corresponds to M5.

[0100] If cruising, static obstacle avoidance, or dynamic obstacle avoidance is to be transferred to the following vehicle, the following conditions must be met: Ob1 exists, V Ob1-2 <V H , V Ob1-1 ≥V L ; This condition corresponds to M6.

[0101] If you want to switch to cruising from following, overtaking, meeting, or both following and overtaking, you must meet the following conditions: Ob1 exists, V Ob1-1 <V L ; This condition corresponds to M7.

[0102] If you want to transfer from following to overtaking, 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] If you want to transfer from following a car to meeting a car, the following conditions must be met: Ob1 exists, V Ob1-2 <V H , V Ob1-1 <-V L ; This condition corresponds to M9. It should be noted that -V L With V L In opposite direction.

[0104] If you want to transfer from following to 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] If you want to transfer from following and overtaking to following, you must meet the following conditions: Ob2 does not exist, V Ob1-1 <α*V espected ; This condition corresponds to M11.

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

[0107] V L 、V H , L0, α, and D0 are adjustable. In this embodiment 1, V L Take 1m / s, V H Take 0.3m / s, L0 is 65m, α is 0.8, and D0 is 4.5m.

[0108] The above processing rules can be expressed as Figure 4 , which shows the conversion diagram of vehicle status and vehicle behavior when the corresponding transfer conditions are met.

[0109] Ob1, Ob2 real-time driving information and Vespected As the input of the finite state machine.

[0110] Among them, the real-time driving information of Ob1 includes: the real-time longitudinal speed V of Ob1 Ob1-1 , real-time lateral speed V Ob1-2 , the real-time distance D from the boundary of one side of the unstructured road Ob1-1 , the real-time distance D to the boundary on the other side 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 speed V Ob2-2 , the real-time distance D from the boundary of one side of the unstructured road Ob2-1 , the real-time distance D to the boundary on the other side 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 The values ​​are determined according to the actual situation; if Ob1 does not exist, V Ob1-1 , D Ob1-1 , D Ob1-2 Both are 0, V Ob1-2 is a 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 The values ​​are determined according to the actual situation; if Ob2 does not exist, V Ob2-1 , D Ob2-1 , D Ob2-2 Both are 0, V Ob2-2 is a maximum value (taken as 1000 in this embodiment 1).

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

[0116] Among them, V espected represents the speed of Car0 traveling at a constant speed on an unstructured road without obstacles, and its value is adjustable. espected Take 10m / s.

[0117] Then, the finite state machine with preset processing rules processes the real-time driving information of Ob1 and Ob2 and V espectedAfter processing, some outputs are obtained: real-time output of the controlled vehicle and real-time output of obstacles.

[0118] Among them, the real-time controlled vehicle output includes: the real-time status of Car0, 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 operates normally: if the real-time state of Car0 falls into the above 8 vehicle states, it means that the finite state machine operates normally.

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

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

[0122] Specifically, the obstacle type of Ob1 and the obstacle type of Ob2 both include four possible 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 three, selectively expand Ob1 and Ob2 in the path planning map (ie, sl map) according to the real-time obstacle output, and obtain a selectively expanded path planning map.

[0125] It should be noted that the selective expansion here follows the principle of projection of different types of obstacles on the sl map and st map.

[0126] For details, see Figure 5 :

[0127] 1. If Ob1 or Ob2 is the first possible type, it means that the 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 means that the obstacle is a static obstacle and is not expanded in the speed planning graph (i.e., st graph), but only in the path planning graph. The obstacle is expanded from the center to the surrounding areas to form a rectangle. The side length of the rectangle is roughly estimated based on the original size of the obstacle, such as Figure 5 (a) shown.

[0129] 3. If Ob1 or Ob2 is the third possible type, it means that the obstacle is a dynamic obstacle and is not expanded in the path planning graph, but only in the speed planning graph. The obstacle is expanded from the center to the surrounding areas to form a parallelogram. The longer side of the parallelogram is converted according to the speed of the obstacle, and the shorter side is roughly estimated according to the original size of the obstacle. For example, Figure 5 (b) as shown.

[0130] 4. If Ob1 or Ob2 is the fourth possible type, it means that the obstacle is an overtaking or meeting obstacle. It is not expanded in the speed planning map, but only in the path planning map. The length is expanded from the center of the obstacle to the direction of movement, and the width is expanded symmetrically from the center to form a rectangle. The long side of the rectangle is converted according to the speed of the obstacle, and the short side is roughly estimated according to the original size of the obstacle. For example, Figure 5 (c) as shown.

[0131] It should be emphasized that the same obstacle cannot appear in both the speed planning graph and the path planning graph in different obstacle types.

[0132] It should be noted that selective expansion can be performed on Ob1 and Ob2 in the same path planning graph; or selective expansion can be performed on Ob1 and Ob2 in two path planning graphs respectively, and then the two path planning graphs are merged into one.

[0133] Step 4: Use a dynamic programming algorithm to process the selectively expanded path planning graph to obtain a convex path space and a local path rough solution.

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

[0135] In step 4, the dynamic programming algorithm discretizes the path search space in the Frenet coordinate system in the selectively expanded path planning graph, and then uses sampling search in the horizontal and vertical directions to obtain the candidate position of the controlled vehicle at each moment; then uses a quintic polynomial to connect the adjacent path points of all possible candidate positions to obtain several candidate paths.

[0136] The dynamic programming algorithm in step 4 uses the cost function C total_path . C total_path It includes 4 parts: Collision risk cost C under path dynamic planning obstacle , path similarity cost C reference , smoothness cost C smoothness 、Historical trajectory cost C HistoricalRoute .

[0137] It should be emphasized that C total_path is the adjusted cost function, which increases C HistoricalRoute .

[0138] 1. C obstacle Characterizes the risk of collision of the controlled vehicle under path planning, and its expression is:

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

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

[0141] h(.) is a custom function;

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

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

[0144] C reference =∑l i .

[0145] 3. C smoothness Characterize the balance of the controlled vehicle path, and its expression is:

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

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

[0148] 4. C HistoricalRoute Represents the influence of historical trajectories - after introducing it, the uniqueness of the minimum value can be ensured, and the instability of decision-making can be improved without affecting the obstacle avoidance ability, thereby avoiding the high-frequency decision-making 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 is the longitudinal distance of a column of the candidate path at time i; l i-T It is the longitudinal distance of the candidate path in the same column in the previous planning cycle; T represents the planning cycle, which is generally 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 item value. The smaller the value, the smaller the deviation between the path of this planning cycle and the path of the previous planning cycle. If the controlled vehicle needs to avoid obstacles, assuming that it chooses to bypass the obstacle on the left side in the previous planning cycle, then when making decisions again in the next planning cycle, it will refer to the trajectory bypassing from the left side in the previous planning cycle - the closer the distance, the smaller the cost value, and the minimum value of bypassing from the right side is eliminated.

[0153] In summary, C total_path It is expressed as:

[0154] In the formula, ω obstacle Represents C obstacle The weight of reference Represents C reference The weight of smoothness Represents C smoothness The weight of HistoricalRoute Represents C HistoricalRoute The weight of .

[0155] It should be noted that ω HistoricalRoute Compared with other weights, it should be smaller to prevent excessive reference to historical trajectories from 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 minimum value is replaced as the local path rough solution, and the path convex space is divided according to the obstacle avoidance direction. Specifically, the local path rough solution cannot be used as a real path, but is mainly used to open up a path convex space for subsequent standard quadratic programming: when an obstacle that needs to be avoided is detected, whether to bypass it from the left or the right is decided based on the path given by dynamic programming - if the obstacle is bypassed from the left, then all solution spaces for bypassing from the right are removed, and the remaining space is the path convex space that can be solved by standard quadratic programming; other bypassing situations are similar and will not be repeated.

[0158] Example 2

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

[0160] That is to say, this embodiment 2 is based on the embodiment 1, and speed planning and subsequent standard secondary planning are added.

[0161] See also Figure 6 , which shows a main flow chart of a dynamic planning method for unstructured roads provided in this embodiment 2, which includes the following steps:

[0162] Step 1: Obtain the position of the controlled vehicle Car0 on the unstructured road in the current planning cycle, and perform real-time obstacle screening within a preset range around Car0 to determine obstacle No. 1 Ob1 and obstacle No. 2 Ob2 that affect the planning of Car0.

[0163] Step 2: Obtain the real-time driving information of Ob1 and Ob2, and compare it with the set expected speed V espected A finite state machine with preset processing rules is used for processing to obtain real-time output of the controlled vehicle and real-time output of the obstacle.

[0164] Step three, selectively expand Ob1 and Ob2 in the path planning map according to the real-time obstacle output, and obtain a selectively expanded path planning map.

[0165] Step 4: Use a dynamic programming algorithm to process the selectively expanded path planning graph to obtain a convex path space and a local path rough solution.

[0166] Steps 1 to 4 can refer to the introduction of Example 1 and will not be repeated here.

[0167] Step 5: selectively expand Ob1 and Ob2 in the speed planning map according to the real-time obstacle output to obtain a speed planning map after selective expansion.

[0168] For step 5, please refer to the description of the selective expansion of Ob1 and Ob2 in Example 1, which will not be repeated here.

[0169] It should be noted that selective expansion can be performed on Ob1 and Ob2 in the same speed planning map; or selective expansion can be performed on Ob1 and Ob2 in two speed planning maps respectively, and then the two speed planning maps are merged into one.

[0170] Step six, use the dynamic programming algorithm to process the selectively expanded velocity planning graph to obtain the velocity convex space and the local velocity rough solution.

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

[0172] In step six, the dynamic programming algorithm discretizes the speed search space in the Frenet coordinate system in the selectively expanded speed planning diagram, and then uses sampling search in the horizontal and vertical directions to obtain the corresponding longitudinal displacement of the controlled vehicle at each moment; then uses simple straight lines to connect all adjacent points to obtain several candidate speed clusters.

[0173] The dynamic programming algorithm in step 6 uses the cost function C total_speed . C total_speed It consists of 4 parts: Collision risk cost C′ under speed 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 is the regular cost function and has not been adjusted.

[0175] 1. C′ obstacle Characterizes the risk of collision of the controlled vehicle under speed planning, and its expression is:

[0176]

[0177] Where d represents the straight-line distance between the controlled vehicle and the surrounding obstacles in the selectively expanded speed planning map; m represents the gain coefficient of the obstacle.

[0178] 2. C velocity Characterize that the speed obtained by speed planning should be as close to V as possible ref , whose expression is:

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

[0180] Where s′ represents the speed of the controlled vehicle obtained by speed planning.

[0181] 3. C accelerate Characterizes the acceleration of the controlled vehicle, and its expression is:

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

[0183] Where s″ represents the acceleration of the controlled vehicle obtained by speed planning.

[0184] 4. C jerk Characterizes the jerk of the controlled vehicle - the instantaneous rate of change of acceleration, and its expression is:

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

[0186] Where s″′ represents the jerk of the controlled vehicle obtained by speed planning.

[0187] In summary, C total_speed It is expressed as:

[0188] In the formula, ω′ obstacle Represents C′ obstacle The weight coefficient of velocity Represents C velocity The weight of accelerate Represents C accelerate The weight of jerk Represented as C jerk The weight of .

[0189] In addition, C total_speed It also satisfies the constraints, including vehicle kinematic constraints and dynamic constraints, which can be expressed as: In the formula, v min 、v max Respectively represent the minimum and maximum speeds of the controlled vehicle; a min Indicates the maximum acceleration of the controlled vehicle when it is decelerating; a max Indicates the maximum acceleration of the controlled vehicle when it is decelerating.

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

[0191] The candidate speed cluster with the minimum value is replaced as the local speed rough solution, and the speed convex space is divided according to the obstacle avoidance direction. Specifically, the local speed rough solution cannot be used as the real control speed, but is mainly used to open up the speed convex space for the subsequent standard quadratic programming: when an obstacle that needs to be avoided is detected, whether to bypass it from the left or right is decided based on the speed given by the dynamic programming - if the obstacle is bypassed from the bottom, then all the solution spaces bypassing from the top are removed, and the remaining space is the speed convex space that can be solved by the standard quadratic programming; other bypassing situations are similar and will not be repeated.

[0192] Step 7, using standard quadratic programming to solve the optimal local path based on the path convex space and the local path rough solution;

[0193] Standard quadratic programming is used to solve the optimal local speed based on the speed convex space and the local speed rough solution;

[0194] The optimal local path and the optimal local velocity are assembled together to obtain the optimal local trajectory.

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

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

[0197] Control Car0 to drive according to the optimal local trajectory obtained in the current planning cycle. As time goes by, the current planning cycle is also updated. Then repeat steps 1 to 7 to obtain the updated optimal local trajectory of the current planning cycle to control Car0. In this way, a loop is formed, and the optimal local trajectory constitutes the global 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] Use simulation software to build a simulation platform: establish an unstructured road model in prescan; write planning control in simulink; select the controlled vehicle Car0 model in Carsim.

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

[0202] See also Figure 7 , set scenario 1: there is a static obstacle in front of Car0; Car0's expected speed is 7m / s; Car0 starts to dynamically plan the local path rough solution 50 meters away from the static obstacle; the closest distance between the local path rough solution and the static obstacle is dl, such as Figure 7 As shown in Figure 1, there are two local path rough solutions (bypassing from above and bypassing from below). The distance between Car0 and the static obstacle is ds; the planning cycle is 0.1s.

[0203] The C HistoricalRoute The cost function of C is not increased HistoricalRoute The cost function is used to calculate the local path rough solution. For comparison results, see Figure 8 , Fig. 9 : Figure 8 Demonstrates the use of no increase in C HistoricalRoute The case of the cost function; Fig. 9 Demonstrated adoption increases C HistoricalRoute The case of the cost function.

[0204] It can be seen that the use of increased C HistoricalRoute The cost function of the dynamic planning trajectory no longer swings left and right, and the decision is more stable. During the whole process of obstacle avoidance, the choice is to bypass the obstacle from the left, and the overall fluctuation of dl is also smaller, indicating that the smoothness of the initial path planning is improved.

[0205] 2. Verify the effect of dynamic planning method for unstructured roads:

[0206] Setting scene 2: Car0 is in reference Fig.10 The unstructured road model is constructed and several obstacles and behavior transfer conditions are set in it.

[0207] The width of the unstructured road is 6 meters and the total length is 2328 meters. The expected speed of Car0 is 10m / s. The planning cycle is 0.1s and the control cycle is 0.01s. The simulation takes 280s. For the simulation results, see Figure 12-Figure 17 .

[0208] See also Fig.11 , which shows the real-time status of Car0 output by the wired state machine. It can be seen that the finite state machine can perform normal state transitions and realize 11 behavior transitions between 8 states.

[0209] Among them, at t=24.7s, the first group of static obstacles were avoided; at t=32.1s, the second group of static obstacles were avoided; at t=75.6s, the vehicle was followed; at t=92.3s, the vehicle was overtaken; at t=117.9s, one vehicle was followed and another vehicle was prepared to be overtaken; at t=140.2s, one vehicle was followed and another vehicle was overtaken; at t=172.8s, the vehicle was met; at t=209.2s, the dynamic obstacles were avoided.

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

[0211] Among them, the maximum longitudinal acceleration of the whole process does not exceed 4m / s 2 , the minimum value is not less than -6m / s 2 . When there are no obstacles, it can cruise at a speed of 10m / s, and the speed throughout the journey is basically no more than 10m / s. The speed curve is smooth overall, indicating that Car0 can make corresponding speed changes when switching between different states. The lateral acceleration throughout the journey is less than 0.2 times the acceleration of gravity, indicating that the tire side deviation is in the linear region and has high safety. The steering wheel angle throughout the journey is no more than 60 degrees, and the curve is smooth and the change frequency is within a certain range, which is more in line with the actual situation. The steering wheel change range throughout the journey is relatively stable, indicating stable operation.

[0212] See also Fig.17 , showing the vehicle trajectory of Car0 throughout the journey, which is Fig.11 The unstructured road model remains consistent.

[0213] Based on the above simulation results, it is shown that the dynamic planning method for unstructured roads can plan a good global optimal path, and it also shows 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, wherein a computer program is stored in the memory, and when the processor executes the computer program, 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 are implemented.

[0216] This embodiment 3 also discloses a readable storage medium, in which computer program instructions are stored. When the computer program instructions are read and executed by a processor, 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 are executed.

[0217] This embodiment 3 also discloses a computer program product, including a computer program. When the computer program is executed by a processor, 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 are implemented.

[0218] The above-mentioned embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A local path planning method for unstructured roads, characterized in that: include: Step 1: Obtain the position of the controlled vehicle Car0 on the unstructured road in the current planning cycle, and perform real-time obstacle screening within a preset range around Car0 to determine obstacle No. 1 Ob1 and obstacle No. 2 Ob2 that affect the planning of Car0; Step 2: Obtain the real-time driving information of Ob1 and Ob2 and compare it with the set expected speed V espected A finite state machine with preset processing rules is used for processing to obtain real-time output of controlled vehicles and real-time output of obstacles; Among them, V espected represents the speed of Car0 traveling at a constant speed on an unstructured road without any obstacles; The real-time controlled vehicle output includes: the real-time status of Car0; the real-time status of Car0 is used to verify whether the finite state machine operates normally; The real-time obstacle output includes: obstacle type to which Ob1 belongs, obstacle type to which Ob2 belongs; Step 3: selectively expand Ob1 and Ob2 in the path planning graph according to the real-time obstacle output to obtain a selectively expanded path planning graph; Step 4: Use a dynamic programming algorithm to process the selectively expanded path planning graph to obtain a path convex space and a local path rough solution; Among them, the dynamic programming algorithm in step 4 adopts the cost function C total_path ; In the formula, C obstacle represents the collision risk cost under path dynamic planning, ω obstacle Represents C obstacle The weight of C reference represents the path similarity cost, ω reference Represents C reference The weight of C smoothness represents the smoothness cost, ω smoothness Represents C smoothness The weight of C HistoricalRoute represents the historical trajectory cost, ω HistoricalRoute Represents C HistoricalRoute The weight of .

2. The local path planning method for unstructured roads according to claim 1, characterized in that: In step 1, the preset range around Car0 is: the road section formed by the distance behind Car0 along the s-axis direction being less than L1 and the distance in front of Car0 along the s-axis direction being less than L2; Among them, 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 1, the real-time obstacle screening method includes: Real-time detection of whether there are M obstacles within the preset range around Car0; When M=0, Ob1 and Ob2 do not exist; When M=1, the obstacle is taken as Ob1, and Ob2 does not exist; When M ≥ 2: If there is a nearest obstacle obstacle3 behind Car0, and the longitudinal speed of obstacle3 is greater than or equal to the longitudinal speed threshold V L , then obstacle3 is taken as Ob1; otherwise, obstacle1, the closest obstacle in front of Car0, is taken as Ob1, and obstacle2, the second closest obstacle in front of Car0, is taken as Ob2; When obstacle3 is used as Ob1, if there is a nearest obstacle obstacle1 in front of Car0, obstacle1 is used 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 2, Ob1's real-time driving information includes: Ob1's real-time longitudinal speed V Ob1-1 , real-time lateral speed V Ob1-2 , the real-time distance D from the boundary of one side of the unstructured road Ob1-1 , the real-time distance D to the boundary on the other side 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 speed V Ob2-2 , the real-time distance D from the boundary of one side of the unstructured road Ob2-1 , the real-time distance D to the boundary on the other side of the unstructured road Ob2-2 ; If Ob1 does not exist, then V Ob1-1 , D Ob1-1 , D Ob1-2 Both are 0, V Ob1-2 is the maximum value; If Ob2 does not exist, then V Ob2-1 , D Ob2-1 , D Ob2-2 Both are 0, V Ob2-2 is a maximum value.

5. The local path planning method for unstructured roads according to claim 1, characterized in that: In step 2, processing rules are constructed based on vehicle status, vehicle behavior, and transfer conditions; The vehicle status includes: cruising, static obstacle avoidance, dynamic obstacle avoidance, following, overtaking, meeting, following and overtaking, and parking; The vehicle behaviors include: transfer from cruising to static obstacle avoidance, transfer from static obstacle avoidance to cruising, transfer from cruising to dynamic obstacle avoidance, transfer from dynamic obstacle avoidance to cruising, transfer from cruising to parking, transfer from cruising or static obstacle avoidance or dynamic obstacle avoidance to following, transfer from following or overtaking or meeting or following and overtaking to cruising, transfer from following to overtaking, transfer from following to meeting, transfer from following to following and overtaking, and transfer from following and overtaking to following; The transfer conditions include: If you want to transfer from cruising to static obstacle avoidance, the following conditions must be met: Ob1 and Ob2 both exist, V Ob1-1 <V L , V Ob1-2 <V H ; If static obstacle avoidance is to be transferred to cruising, the following conditions must be met: Ob1 and Ob2 do not exist; If you want to transfer from cruising to dynamic obstacle avoidance, the following conditions must be met: Ob1 exists, V Ob1-2 ≥V H ; If dynamic obstacle avoidance is to be transferred to cruising, the following conditions must be met: Ob1 and Ob2 do not exist; If cruising is to be transferred to parking, the following conditions must be met: Ob1 and Ob2 do not exist, and the distance between Car0 and the end of the unstructured road is less than L0; If cruising, static obstacle avoidance, or dynamic obstacle avoidance is to be transferred to the following vehicle, the following conditions must be met: Ob1 exists, V Ob1-2 <V H , V Ob1-1 ≥V L ; If you want to switch to cruising from following, overtaking, meeting, or both following and overtaking, you must meet the following conditions: Ob1 exists, V Ob1-1 <V L ; If you want to transfer from following to overtaking, 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; If you want to transfer from following a car to meeting a car, the following conditions must be met: Ob1 exists, V Ob1-2 <V H , V Ob1-1 <-V L ; If you want to transfer from following to 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; If you want to transfer from following and overtaking to following, you must meet the following conditions: Ob2 does not exist, V Ob1-1 <α*V espected ; Among them, V L Indicates the longitudinal velocity threshold, V H represents the lateral speed threshold, L0 represents the stopping distance threshold, α represents the threshold coefficient, 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 2, the obstacle type to which Ob1 belongs and the obstacle type to which Ob2 belongs both include four possible types; If Ob1 or Ob2 is the first possible type, it means that the 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 the 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 the obstacle is a dynamic obstacle and is not expanded in the path planning graph; If Ob1 or Ob2 is the fourth possible type, it means that the obstacle is an overtaking or meeting obstacle, and only expansion is performed in the path planning graph.

7. The local path planning method for unstructured roads according to claim 6, characterized in that: In step 4, the dynamic programming algorithm generates several candidate paths in the selectively expanded path planning graph and selects the candidate paths according to C total_path Calculate the cost of all candidate paths; The candidate path with the minimum value is replaced as the local path rough 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 global optimal trajectory of the controlled vehicle on unstructured roads; The dynamic planning method for unstructured roads includes: A local path planning method for unstructured roads as described in any one of claims 1 to 7; Among them, in step 2, the real-time controlled vehicle output also includes: the real-time reference speed V of Car0 ref ; V ref Used to calculate the expected speed deviation cost C velocity ; If Ob1 or Ob2 is the first possible type, it means that this obstacle does not exist and does not appear in the speed planning; If Ob1 or Ob2 is of the third possible type, then only expansion is performed in the speed planning graph; The dynamic planning method for unstructured roads also includes: Step 5: selectively expand Ob1 and Ob2 in the speed planning map according to the real-time obstacle output to obtain a speed planning map after selective expansion; Step 6, using a dynamic programming algorithm to process the selectively expanded velocity planning graph to obtain a velocity convex space and a local velocity rough solution; Among them, the dynamic programming algorithm in step six adopts the cost function C total_speed ; In the formula, C o ' bstacle represents the collision risk cost under speed dynamic programming, ω o ' bstacle Represents C o ' bstacle The weight coefficient of velocity represents the expected speed deviation cost, ω velocity Represents C velocity The weight of C accelerate represents the comfort cost, ω accelerate Represents C accelerate The weight of C jerk represents the acceleration cost, ω jerk Represented as C jerk The weight of Step 7, using standard quadratic programming to solve the optimal local path based on the path convex space and the local path rough solution; Standard quadratic programming is used to solve the optimal local speed based on the speed convex space and the local speed rough solution; Assemble the optimal local path and the optimal local speed together to obtain the optimal local trajectory; Step 8: As the current planning cycle is updated, steps 1 to 7 are repeated to form a global optimal trajectory from the optimal local trajectories.

9. The dynamic planning method for unstructured roads according to claim 8, characterized in that: In step 4, the dynamic programming algorithm generates several candidate speed clusters based on the selectively expanded speed planning graph and selects the candidate speed clusters according to C total_speed Calculate the cost values ​​of all candidate velocity clusters; The candidate velocity cluster with the smallest value is replaced as the local velocity rough solution, and the velocity convex space is divided according to the obstacle avoidance direction.

10. A computer program product, characterized in that The invention comprises a computer program; when the computer program is 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 to 7, or / and the steps of the dynamic planning method for unstructured roads as described in any one of claims 8 to 9.

Images