A trajectory obstacle avoidance method for autonomous vehicles based on closed scenes

Abstract

The present invention belongs to the field of unmanned driving path planning, and specifically relates to a closed scene-based autonomous driving vehicle trajectory obstacle avoidance method ("RtdsHRT"). The algorithm of the present invention has excellent convergence and universality in closed scenes, which is mainly based on the idea of ​​"global sampling + local sweep". Through experimental comparison, compared with the RRT algorithm, which is also a sampling algorithm, it has a significant improvement in the number of iterations, sampling efficiency, and solution quality. It also has relevant advantages compared with other types of algorithms. Scene verification was carried out in a two-dimensional virtual environment and a three-dimensional virtual reality environment, respectively, with good results.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving path planning, and specifically relates to a closed scene based autonomous driving vehicle trajectory obstacle avoidance method. Background Art

[0002] Currently, trajectory planning algorithms widely used in autonomous vehicles fall into three main categories: 1) traditional planning algorithms; 2) intelligent planning algorithms; and 3) sampling-based planning algorithms. Traditional planning algorithms include the APF algorithm, the Bug algorithm, the VFH algorithm, and the GM algorithm; intelligent planning algorithms include trajectory algorithms based on artificial neural network architectures, the GA algorithm, and the ACO algorithm; and sampling-based planning algorithms include the PRM algorithm and the RRT algorithm. Each of these algorithms has its own advantages and disadvantages, and while they can achieve certain results in their respective scenarios, they lack universal applicability. Summary of the Invention

[0003] This paper addresses the shortcomings of existing trajectory planning algorithms and proposes a novel obstacle avoidance algorithm for autonomous driving. This algorithm is suitable for high-dimensional environments, exhibits good computational distribution and convergence, and offers robust fault tolerance for vehicles with diverse technical architectures. Multiple validation methods have demonstrated the algorithm's effectiveness and its applicability to practical engineering applications.

[0004] The specific technical solutions are as follows:

[0005] A closed scene based autonomous vehicle trajectory obstacle avoidance method (called " RtdS ”), the steps are as follows:

[0006] Step 1: Real-time traffic scene acquisition and 3D mapping to initialize the map space;

[0007] Step 2: perform global sampling based on the Hammersley sampling sequence;

[0008] Step 3: weight the sampling point set;

[0009] Step 4: Perform a local flat scan of the sector circular area of ​​the sampling point set;

[0010] Step 5: Generate a search tree, and generate a decision tree through collision detection, pruning and result judgment.

[0011] Furthermore, the step one is specifically to transmit the camera, lidar, CAN acquisition module, and GNSS sensor signals to the corresponding signal processing unit and then to the acquisition host. The collected signals are processed by the acquisition software and the relevant target object information is output in real time through the screen.

[0012] Furthermore, in step 2, the global sampling method based on the Hammersley sampling sequence is as follows:

[0013] Represent any m+1-digit integer x as a combination of digits, where x0, x1, x2, ... xm-1, xm are the values ​​of the integer x from the units digit to the m+1 digit, i.e. x = xmxm-1 ... x2x1x0. After introducing a base n, it can be expressed as:

[0014] x=x0+x1n+x2n 2 +…+x m n m

[0015]

[0016] Where: m is an integer;

[0017] Reverse the order of the digits in x and construct a unique fn(x) in the specified interval (interval [0,1]) based on the reciprocal of the base n, expressed as:

[0018] f n (x)=x0x1x2...x m =x0n -1 +x1n -2 +…+x m n -m-1

[0019] The Q i-dimensional Hammersley sequence point sets Pi(x) are expressed as: Pi(x)=1-Zi(x)x=1,2,3…Q

[0020]

[0021] Where: Zi(x) is a set of Q sampling points randomly distributed in i dimensions; n1, n2, …ni-1 are all prime numbers.

[0022] Furthermore, the weighting method in step three is as follows:

[0023] λ N =Anglecalculation(X.start_goal,X.sample_goal)

[0024] D N =Distance|Xsample(i)-Xgoak|

[0025] Weight=β|sinλN| / DN

[0026] Where: Anglecalculation is the angle calculation function, Distance is the distance calculation function, DN is the distance between the sampling point set and the target point, λN is the angle between the line connecting the starting point and the target point and the line connecting the sampling point and the target point, Weight is the weighting formula for a certain sampling point, and β is the point set perturbation coefficient.

[0027] Furthermore, the specific method of step 4 is as follows:

[0028] During the vehicle tracking phase, a semicircular arc with a central angle of 45° is set in front of the vehicle. A set of points with higher weights is extracted as the "alternative branches" of the exploration number. Collision detection is performed on the alternative branches. When performing collision detection on the path, collision detection is performed on "points" and "edges":

[0029] Case 1: When the algorithm selects a point that is inside an obstacle, after Hammersley global sampling, the points inside the obstacle are removed by using the cross product of the vectors.

[0030] Case 2: The "edge" selected by the algorithm is within the obstacle. The edge formed by the two points will pass through the obstacle. The cross product method is used to determine whether the trajectory line and the obstacle contour line intersect.

[0031] Furthermore, the specific method of step 5 is as follows:

[0032] First, a conflict-free path tree with Hammersley sampling and gradual expansion is constructed to search for the target point from the starting point; the path tree is based on the starting point x init As the starting node, in each iteration, a node x with the largest value is selected from the fan-shaped sweep space X according to the weighted value. rand , search all nodes within the fan-shaped sweep range in the tree and find the distance x rand The nearest node is denoted as x nearest , and record its weight, then, with x nearest Starting point along x nearest and x rand The fan-shaped sweep continues in the direction of a specific step size to find a new node x new ; if x new And x new and x nearest After the collision detection is correct, the x new Add to the random tree and add x nearestAs its parent node; calculate the distance between each new node and the target point. If it is less than the set value and the line connecting the two points does not collide with any obstacles, the path is considered to have been found. The search path is determined by comparing the search results of multiple search trees. Finally, a decision tree is generated to connect the new node with the target point. Finally, the path is drawn by tracing back from the target point to the starting point. Otherwise, the above search process is repeated;

[0033] Pruning: If in the point set T, as the search tree gradually grows, the weight of the search point set gradually decreases, when the reduction value is greater than K, the new node search branch can be filtered out and not expanded, and the search branch end will be pruned.

[0034] The trajectory algorithm proposed in this invention, tailored to the applicable environment, achieves excellent results in both high- and low-dimensional spaces. It can promptly escape local optima when trapped in complex environments or blind spots. To address the complexities of debugging NN networks, this algorithm simplifies the existing model to a certain extent, significantly improving its operational efficiency. In terms of distribution and convergence, this algorithm employs a "local sweep + global point sampling" approach to ensure the algorithm's inherent distribution and convergence. Scenario verification was conducted in both a two-dimensional virtual environment and a three-dimensional virtual reality environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 Schematic diagram of the process of the present invention;

[0036] Figure 2 Schematic diagram of sector scanning area;

[0037] Figure 3 Schematic diagram of an edge passing through an obstacle;

[0038] Figure 4 Schematic diagram of the cross product algorithm;

[0039] Figure 5 Two-dimensional environmental verification map;

[0040] Figure 6 Map (1) algorithm calculates convergence results;

[0041] Figure 7 Map (2) algorithm calculates convergence results;

[0042] Figure 8 Map (3) algorithm calculates convergence results;

[0043] Figure 9 Map (1) Path length result statistics;

[0044] Figure 10 Map (1) Statistical graph of the actual number of sampling nodes;

[0045] Figure 11 Map (1) Iterative calculation time result statistics;

[0046] Figure 12 Map (2) Path length result statistics;

[0047] Figure 13 Map (2) Statistical graph of the actual number of sampling nodes;

[0048] Figure 14 Map (2) Iterative calculation time result statistics;

[0049] Figure 15 Map (3) Path length result statistics;

[0050] Figure 16 Map (3) Statistical graph of the actual number of sampling nodes;

[0051] Figure 17 Map (3) Iterative calculation time result statistics;

[0052] Figure 18 Signal acquisition schematic diagram;

[0053] Figure 19 Test vehicle bus data interface diagram;

[0054] Figure 20 Laser point cloud acquisition interface diagram;

[0055] Figure 21 Video information and target information map;

[0056] Figure 22 3D simulation operation diagram of obstacle avoidance facing fixed obstacles;

[0057] Figure 23 3D simulation operation diagram of obstacle avoidance for stationary vehicles;

[0058] Figure 24 3D simulation operation diagram of obstacle avoidance for moving vehicles. DETAILED DESCRIPTION

[0059] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0060] The new trajectory obstacle avoidance algorithm proposed in this invention has the following algorithm flow: Figure 1 , the specific steps are as follows:

[0061] Step 1: Real-time traffic scene acquisition and 3D mapping to initialize the map space;

[0062] Step 2: perform global sampling based on the Hammersley sampling sequence;

[0063] Step 3: weight the sampling point set;

[0064] Step 4: Perform a local flat scan of the sector circular area of ​​the sampling point set;

[0065] Step 5: Generate a search tree, and generate a decision tree through collision detection, pruning and result judgment.

[0066] Step 2: The global sampling method based on the Hammersley sampling sequence is as follows:

[0067] Represent any m+1-digit integer x as a combination of digits, where x0, x1, x2, ... xm-1, xm are the values ​​of the integer x from the units digit to the m+1 digit, i.e. x = xmxm-1 ... x2x1x0. After introducing a base n, it can be expressed as:

[0068] x=x0+x1n+x2n 2 +…+x m n m

[0069]

[0070] Where: m is an integer;

[0071] Reverse the order of the digits in x and construct a unique fn(x) in the specified interval (interval [0,1]) based on the reciprocal of the base n, expressed as:

[0072] f n (x)=x0x1x2...x m =x0n -1 +x1n -2 +…+x m n -m-1

[0073] The Q i-dimensional Hammersley sequence point sets Pi(x) are expressed as: Pi(x)=1-Zi(x)x=1,2,3…Q

[0074]

[0075] Where: Zi(x) is a set of Q sampling points randomly distributed in i dimensions; n1, n2, …ni-1 are all prime numbers.

[0076] Furthermore, the weighting method in step three is as follows:

[0077] λ N =Anglecalculation(X.start_goal,X.sample_goal)

[0078] D N =Distance|Xsample(i)-Xgoak|

[0079] Weight=β|sinλN| / DN

[0080] Where: Anglecalculation is the angle calculation function, Distance is the distance calculation function, DN is the distance between the sampling point set and the target point, λN is the angle between the line connecting the starting point and the target point and the line connecting the sampling point and the target point, Weight is the weighting formula for a certain sampling point, and β is the point set perturbation coefficient.

[0081] The specific method of step 4 is as follows:

[0082] During the vehicle tracking operation phase, a semicircular arc with a central angle of 45° is set in front of the vehicle, such as Figure 2 As shown in the figure, a set of points with higher weights is extracted as the "alternative branches" for exploration, and collision detection is required for the alternative branches. When performing collision detection on the path, collision detection is performed on "points" and "edges":

[0083] Case 1: When the algorithm selects a point that is inside an obstacle, after Hammersley global sampling, the points inside the obstacle are removed by using the cross product of the vectors.

[0084] Case 2: The edge selected by the algorithm is within the obstacle, and the edge formed by the two points will pass through the obstacle (such as Figure 3 As shown in Figure 2, the cross product method is used to determine whether the trajectory line and the obstacle contour line intersect.

[0085] Assume that line segment AB intersects line segment CD, then they must satisfy the following two conditions:

[0086] Vertices C and D are located at the two ends of line segment AB;

[0087] Or vertices A and B are located at the two ends of line segment CD respectively.

[0088] Suppose we want to prove that vertices C and D are located on both sides of line segment AB, then we can connect AC and AD and calculate the cross product of vector AB with vectors AC and AD respectively. As long as the cross product results of the two have different signs, it means that they are located at the two ends of line segment AB. Figure 4 shown.

[0089]

[0090] If m×n>0, it means that the two have the same sign, and vertices C and D are on the same side of AB;

[0091] If m×n<0, it means that the two are of different signs, and vertices C and D are on opposite sides of AB.

[0092] This allows for the two types of collision detection mentioned above.

[0093] The specific method of step five is as follows:

[0094] First, a conflict-free path tree with Hammersley sampling and gradual expansion is constructed to search for the target point from the starting point; the path tree is based on the starting point x init As the starting node, in each iteration, a node x with the largest value is selected from the fan-shaped sweep space X according to the weighted value. rand , search all nodes within the fan-shaped sweep range in the tree and find the distance x rand The nearest node is denoted as x nearest , and record its weight, then, with x nearest Starting point along x nearest and x rand The fan-shaped sweep continues in the direction of a specific step size to find a new node x new ; if x new And x new and x nearest After the collision detection is correct, the x new Add to the random tree and add x nearest As its parent node; calculate the distance between each new node and the target point. If it is less than the set value and the line connecting the two points does not collide with any obstacles, the path is considered to have been found. The search path is determined by comparing the search results of multiple search trees. Finally, a decision tree is generated to connect the new node with the target point. Finally, the path is drawn by tracing back from the target point to the starting point. Otherwise, the above search process is repeated;

[0095] Pruning: If in the point set T, as the search tree gradually grows, the weight of the search point set gradually decreases, when the reduction value is greater than K, the new node search branch can be filtered out and not expanded, and the search branch end will be pruned.

[0096] Scene verification is carried out in two-dimensional virtual environment and three-dimensional virtual reality environment respectively to illustrate the advantages of this algorithm over other algorithms.

[0097] 1. Simulation results in a two-dimensional environment

[0098] In order to verify the effectiveness of the improved algorithm, the patented algorithm is compared with the original RRT algorithm based on the same sampling idea and the H-RRT algorithm with the same improvement in the following examples: Figure 5Comparative simulation experiments were conducted under the three map environments shown, and the relevant algorithm settings were kept consistent. Each algorithm was run independently 50 times in the three environments. The algorithm running environment is 64-bit Windows 10, processor Intel (R) Core (TM) i7-7700HQ, and memory 8GB. Since they are all sampling algorithms, in order to evaluate the performance of the RtdsHRT algorithm under the above three maps, this paper runs the above three maps 50 times each. The following evaluation indicators are selected from the running results: a) path length; b) actual number of sampling nodes; c) iterative calculation time, and the statistical results are as follows: Figure 9-17 The comparison of each algorithm is shown in Figure 1-3 The calculation results statistics are shown below:

[0099]

[0100]

[0101] To illustrate its superiority, after comparing it with other different types of algorithms, the algorithm has the following advantages, as judged by experimental data:

[0102] Comparing Algorithms Advantages Comparison with the APF algorithm It overcomes the oscillation problem in high-dimensional environments and is not prone to falling into local optimality. Compared with Dijkstra's algorithm The planning efficiency is greatly improved, and the actual number of sampling nodes is greatly controlled. Compared with ACO Improved the universality of the algorithm in high-dimensional targets and improved the solution quality Compared with artificial neural networks The amount of calculation and the difficulty of model adjustment are reduced.

[0103] 2. Algorithm Verification in a 3D Environment Based on Virtual Mapping

[0104] In order to verify the obstacle avoidance performance of the RtdsHRT algorithm in the simulation scenario, a three-dimensional simulation environment based on real traffic scene mapping is proposed for verification.

[0105] First, real-time traffic scene acquisition and 3D mapping are carried out. The principle is as shown in the figure, which is to transmit the camera, lidar, CAN acquisition module, and GNSS sensor signals to the corresponding signal processing unit and then to the acquisition host. The collected signals are processed by a certain brand of acquisition software and then output relevant target information and other information on the screen in real time. Figure 18 In order to obtain complete scene data around the test vehicle in actual traffic scenarios, the collection vehicle uses a four-line laser radar, an intelligent camera, two sets of high-definition cameras, two sets of ordinary cameras, a collection engineering machine, a data collection system, a touch screen, an integrated navigation system, and CAN channel equipment. Through the combination of the above sensor data, the vehicle bus data (such as Figure 19 As shown), the laser point cloud in front of the vehicle (as shown Figure 20 as shown) and video information and target information (as shown) Figure 21It is planned to select the dynamics and other technical parameters of a domestic autonomous driving vehicle and bring them into the digital model, adjust and improve the relevant dynamic parameters, and then substitute the algorithm proposed in this patent into the technical architecture to conduct path planning simulation tests. The test shows that in a closed test scenario, the algorithm is effective in facing fixed obstacles (such as Figure 22 As shown)), stationary vehicles (such as Figure 23 As shown)), sports vehicles (such as Figure 24 As shown)) can successfully avoid obstacles and finally reach the predetermined destination.

[0106] The above describes in detail the preferred embodiments of this patent, but this patent is not limited to the above embodiments. Various changes can be made within the scope of knowledge possessed by ordinary technicians in this field without departing from the purpose of this patent.

Claims

1. A method for avoiding obstacles in the trajectory of an autonomous vehicle based on a closed scene, characterized by: Here are the steps: Step 1: Real-time traffic scene acquisition and 3D mapping to initialize the map space; Step 2: Perform global sampling based on the Hammersley sampling sequence; express any m+1-bit integer x as a combination of digits, where x0, x1, x2, ... xm-1, xm are the values ​​of the integer x from the units digit to the m+1 digit, i.e. x = xmxm-1 ... x2x1x0. After introducing a base n, it can be expressed as: x=x0+x1n+x2n 2 +…+x m n m ; ； Where: m is an integer; Reverse the order of the digits in x and construct a unique fn(x) in the specified interval [0, 1] based on the reciprocal of the base n, expressed as: f n (x)=x0x1x2…x m =x0n -1 +x1n -2 +…+x m n -m-1 ; The Q i-dimensional Hammersley sequence point sets Pi(x) are expressed as: Pi(x) = 1-Zi(x) x = 1, 2, 3...Q; {Z}_{i}(x)=\left [ {\frac {x} {Q},{f}_{n1}\, (x),{f}_{n2}(x),…{f}_{ni-1}(x)} \right ] ; Where: Zi(x) is a set of Q sampling points randomly distributed in i-dimension; n1, n2, ...ni-1 are all prime numbers; Step 3: weight the sampling point set; D N =Distance|Xsample(i)-Xgoal|; λ N =Anglecalculation(X.start_goal,X.sample_goal); Weight=β|sinλN| / DN ; Where: Anglecalculation is the angle calculation function, Distance is the distance calculation function, DN is the distance between the sampling point set and the target point, λN is the angle between the line connecting the starting point and the target point and the line connecting the sampling point and the target point, Weight is the weighting formula for a certain sampling point, and β is the point set disturbance coefficient; Step 4: Perform a local flat scan of the sector circular area of ​​the sampling point set; Step 5: Generate a search tree, and generate a decision tree through collision detection, pruning and result judgment; First, a conflict-free path tree with Hammersley sampling and gradual expansion is constructed to search for the target point from the starting point; the path tree is based on the starting point x init As the starting node, in each iteration, a node x with the largest value is selected from the fan-shaped sweep space X according to the weighted value. rand , search all nodes within the fan-shaped sweep range in the tree and find the distance x rand The nearest node is denoted as x nearest , and record its weight, then, with x nearest Starting point along x nearest and x rand The fan-shaped sweep continues in the direction of a specific step size to find a new node x new ; if x new And x new and x nearest After the collision detection is correct, the x new Add to the random tree and add x nearest As its parent node; calculate the distance between each new node and the target point. If it is less than the set value and the line connecting the two points does not collide with any obstacles, the path is considered to have been found. The search path is determined by comparing the search results of multiple search trees. Finally, a decision tree is generated to connect the new node with the target point. Finally, the path is drawn by tracing back from the target point to the starting point. Otherwise, the search process is repeated to find the target point and new node again. Pruning: If in the point set T, as the search tree gradually grows, the weight of the search point set gradually decreases, when the reduction value is greater than K, the new node search branch is filtered out and not expanded, and the search branch end is pruned.

2. The closed-scene obstacle avoidance method for an autonomous driving vehicle according to claim 1, characterized in that: The step one is to transmit the camera, lidar, CAN acquisition module, and GNSS sensor signals to the corresponding signal processing unit and then to the acquisition host. The collected signals are processed by the acquisition software and the relevant target object information is output in real time on the screen.

3. The closed-scene obstacle avoidance method for an autonomous driving vehicle according to claim 1, characterized in that: The specific method of step 4 is as follows: During the vehicle tracking phase, a semicircular arc with a central angle of 45° is set in front of the vehicle, and a set of points with higher weights is extracted as the "alternative branches" of the exploration number. The alternative branches need to be collided with; when performing collision detection on the path, collision detection is performed on "points" and "edges": Case 1: When the algorithm selects a point within an obstacle, after Hammersley global sampling, the points within the obstacle are removed using the cross product of the vectors. Case 2: The edge selected by the algorithm is within the obstacle. The edge formed by the two points will pass through the obstacle. The cross product method is used to determine whether the trajectory line and the obstacle contour line intersect.

Images