High-performance intelligent vehicle trajectory planning method based on flexible rasterization

Abstract

The invention discloses a high-performance intelligent vehicle trajectory planning method based on flexible rasterization, and the method comprises the steps: constructing a grid map in a certain range of a vehicle, and marking a grid where an obstacle falls as occupied; the adjacent occupied grids in each column are combined to obtain a new occupied grid, and all the combined occupied grids are stored in a container; starting to expand a driving corridor; on the basis of the expanded driving corridor, continuously expanding the corridor by adopting a stable method of gradually expanding along the clockwise direction, and ensuring that the corridor is fully expanded in four directions; and in the extension process, when collision occurs in a certain direction or the maximum extension increment is reached, the extension is returned, then the extension in the direction is not carried out any more, the process continues until the extension cannot be continued in the four directions, then the process is ended, final extension increments in the four directions are obtained, and a final driving corridor area is calculated. Compared with a traditional scheme, the method is short in obstacle avoidance time consumption and stable in obstacle avoidance time consumption.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent vehicle trajectory planning, and in particular to a high-performance intelligent vehicle trajectory planning method based on flexible rasterization. Background Art

[0002] The trajectory planning of an intelligent vehicle aims to generate a space-time trajectory that reflects the local path and speed of the vehicle, conforming to the vehicle's internal kinematic constraints and external environment obstacle avoidance constraints, and optimizing the established performance indicators as much as possible while strictly ensuring that the above constraints are met.

[0003] The intelligent vehicle trajectory planning method is generally divided into two steps: the first step is to model the trajectory planning problem as a constrained dynamic optimization problem; the second step is to numerically solve the dynamic optimization problem.

[0004] The modeling of trajectory planning problem, like the modeling of all trajectory planning problems, is constructed as a constrained dynamic optimization problem, which consists of decision variables, constraints and cost functions.

[0005] Decision variables: The decision variables in the trajectory planning problem model based on dynamic optimization include the following:

[0006] 1)(x(t),y(t)) represents the coordinates of a fixed reference point of the vehicle body in the earth's inertial coordinate system at time t;

[0007] 2) v(t) and a(t) represent the velocity and acceleration of the vehicle body reference point, respectively;

[0008] 3)φ(t) represents the deflection angle of the front wheel of the vehicle, ω(t) represents the angular velocity of the front wheel of the vehicle corresponding to the deflection angle, and θ(t) represents the attitude angle of the vehicle in the geoid inertial coordinate system.

[0009] Therefore, the decision variables include the above-mentioned x(t), y(t), v(t), a(t), φ(t), ω(t), θ(t).

[0010] Cost function: The cost function in the dynamic optimization problem model is generally used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,T], the cost function J is constructed as:

[0011]

[0012] where w a ,w w >0 is the weight specified by the user.

[0013] Constraints: The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints, and obstacle avoidance constraints.

[0014] a) Vehicle kinematic constraints:

[0015] The motion of the vehicle in the inertial coordinate system is subject to the following differential equations:

[0016]

[0017] Among them, L W Represents the vehicle's wheelbase.

[0018] At the same time, some decision variables have an allowable action range, so corresponding interval constraints should be imposed:

[0019]

[0020] Among them, a min with a max Represents the left and right boundary values ​​of acceleration, v min With v max Represents the left and right boundary values ​​of the velocity, Ω max represents the maximum amplitude of the front wheel angular velocity ω(t), Φ max Represents the maximum amplitude of the front wheel steering angle φ(t).

[0021] b) Two-point boundary value constraints:

[0022] At the starting time t=0 in the trajectory planning domain, the vehicle is in the current actual motion state. For example, at this time, the acceleration is a0, the speed is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0. Then the following constraints should be imposed:

[0023]

[0024] Similarly, at the end time t = T in the trajectory planning domain, the vehicle is expected to be in a specified motion state, for example, the acceleration is af, the velocity is vf, the attitude angle is thetaf, the front wheel angle is phif, the front wheel angular velocity is omegaf, the horizontal coordinate position is xf, and the vertical coordinate position is yf, then the following constraints should be imposed:

[0025]

[0026] c) Obstacle avoidance constraints:

[0027] Obstacle avoidance constraints mean that the driving trajectory of the smart car needs to ensure driving safety, that is, to ensure that the vehicle does not collide with obstacles in the environment. Obstacle avoidance constraints are the most computationally complex and time-consuming part of the entire trajectory problem; the complexity of this constraint stems from the number, shape, and dynamic changes of obstacles in the environment, especially in complex urban environments, where obstacles are of many types and irregularly distributed, which increases the difficulty of calculation.

[0028] In traditional trajectory planning methods, the system needs to monitor obstacles around the vehicle in real time and dynamically adjust the vehicle's trajectory to avoid collisions. Although this method is flexible, as the complexity of the scene increases, especially when there are many obstacles or the environment changes rapidly, the complexity of collision detection and obstacle avoidance calculations will increase significantly, thereby increasing the system's computational burden and the difficulty of solving the problem, and reducing planning efficiency. To solve this problem, the current common technology is to lay a driving corridor along the initial trajectory, thereby completely isolating the free space where the vehicle can drive from obstacles; in this way, the vehicle obstacle avoidance constraint can be converted into a constraint that "the vehicle must travel within a specific sub-area of ​​the safe driving corridor within a specific time period."

[0029] Before constructing the driving corridor, the vehicle body is usually covered with N discs, such as Figure 1 As shown in the figure, two disks are used to represent the vehicle body, and “the vehicle body does not collide with obstacles” means “neither disk can collide with obstacles”. If the two disks are shrunk to the center of each circle and the obstacles in the environment are expanded to the radius R, then “neither disk can collide with obstacles” means “the center of the front and rear disks P f ,P r Do not collide with the expanded obstacle. Then start to construct the center P of the front and rear discs. f ,P r The drivable rectangular area, i.e. the driving corridor; and ensure that P f ,P r Being within the corresponding driving corridor area at a specific moment.

[0030] Current methods of constructing driving corridors, such as Figure 2 As shown in the figure, the method of gradual expansion is mainly adopted. The specific idea is: starting from the center point (x 0 ,y 0 ) and gradually expands outward along the four sides of the rectangle in a predetermined direction (clockwise in the figure). During this process, the expanded rectangle will perform collision detection with obstacles within a certain range around the vehicle. Based on the detection results, it is decided whether to merge the expanded part into the existing corridor rectangle and whether to continue expanding in this direction.

[0031] The generation and expansion process of rectangular blocks is as follows:

[0032] 1. Expand the starting point: First, from the center point (x 0 ,y 0 ) starts from the center point (x 0 ,y 0 ) can be considered as a rectangle with a length of 0 and a width of 0.

[0033] 2. Expansion direction: Select the expansion direction, which can be clockwise or counterclockwise (the figure shows four directions {π / 2, 0, -π / 2, π}, that is, clockwise). In each expansion step, the step length d will be expanded outward along one edge of the rectangle.

[0034] 3. Collision detection: After each expansion, collision detection is performed on the newly generated rectangular block and obstacles within a certain range around the vehicle. This usually involves calculating the spatial relationship between the rectangular block and the obstacle to determine whether there is overlap or intersection.

[0035] 4. Merge and return: If the collision detection shows that the newly expanded rectangle does not collide with the obstacle, it will be merged into the existing corridor rectangle. If a collision occurs or the preset maximum expansion increment is reached, the expansion will be returned and no expansion in this direction will be performed in the subsequent expansion process. The maximum expansion increment is set to avoid generating a rectangular area that is too large and wastes computing time.

[0036] The formation of the final driving corridor: When the four directions cannot be expanded any further, the resulting rectangular area is the driving corridor area required for the current position point. This area ensures that the vehicle can avoid obstacles at this position to ensure safety. In summary, the obstacle avoidance constraint can be expressed as the following linear inequality:

[0037]

[0038] Where (x f (t),y f (t)) is the center point position of the front disk of the vehicle at time t, (x r (t),y r (t)) is the center point position of the rear disc of the vehicle at time t. min ,xf max ,yf min ,yf max ] is the range of the drivable rectangular area of ​​the disk center point on the x-axis and y-axis before time t; [xr min ,xr max ,yr min ,yr max ] is the value range of the drivable rectangular area of ​​the center point of the disk on the x-axis and y-axis after time t.

[0039] At this point, the complete trajectory planning problem is constructed as the following dynamic optimization problem:

[0040]

[0041] However, constructing driving corridors using existing methods is a relatively time-consuming task.

[0042] 1. In actual operation, each time the corridor is expanded, it is necessary to perform collision detection between the expanded rectangular area and obstacles in the environment. Since the construction process of the corridor usually involves multiple expansions, this will lead to an increase in the number of collision detections, thereby increasing the overall time consumption of collision detection.

[0043] 2. When there are many obstacles in the environment and their complexity is high, the time required to construct the driving corridor will increase significantly, which will not only have a negative impact on the overall efficiency of trajectory planning, but may also seriously affect the real-time performance of the system.

[0044] If the construction process of the driving corridor can be accelerated and the time consumption of collision detection involved in the corridor construction process can be reduced, the efficiency of the driving corridor construction can be significantly improved, the time consumption of the obstacle avoidance module can be reduced, and the performance of the trajectory planning system can be improved.

[0045] In summary, it is necessary to propose a high-performance intelligent vehicle trajectory planning method based on flexible rasterization to solve the above problems. Summary of the invention

[0046] The present invention aims at the problem of how to optimize and accelerate the construction process of driving corridors and reduce the time consumption of collision detection involved in the construction process of driving corridors, and proposes a high-performance intelligent vehicle trajectory planning method based on flexible rasterization, comprising the following steps:

[0047] S1. Build a grid map within a certain range of the vehicle and mark the grids where obstacles fall as occupied;

[0048] S2, processing the occupied grid map obtained in S1, merging adjacent occupied grids in each column to obtain new occupied grids, and storing all the merged occupied grids in a container for subsequent collision detection;

[0049] S3, start the expansion of the driving corridor. In the early stage of the construction of the driving corridor, expand it in all directions at the same time, and perform collision detection on the driving corridor obtained after each expansion and the merged occupied grid obtained in S2. If a collision occurs or the maximum expansion increment is reached, return to this expansion, output the expansion increments in four directions and proceed to the next step;

[0050] S4. Based on the extended corridor in S3, the corridor is further extended in a clockwise direction to ensure that the four directions of upward, rightward, downward and leftward are fully extended. During the extension process, if a collision occurs in a certain direction or the maximum extension increment is reached, the extension is returned and no further extension is performed in that direction. This process is continued until the four directions cannot be extended any further. The final extension increments in the four directions are obtained and the final corridor area is calculated according to formula (8):

[0051]

[0052] In formula (8), (x, y) is the starting point of the corridor extension, and length 1 is the upward expansion increment (π / 2 direction), length 2 The extension increment for the right side of the vehicle (direction 0), length 3 is the extension increment below the vehicle (-π / 2 direction), length 4 The extension increment for the left side of the vehicle (π direction);

[0053] The overall process is executed as the following function: Boxs = GenerateCorridor (traj, map). The input list of this function is the vehicle trajectory traj, which contains N ordered trajectory points. Each trajectory point contains the vehicle location information (x, y). Map represents a map composed of the scattered coordinates of obstacles. The return value of this function is the set Boxs of the drivable corridor area corresponding to each trajectory point in this trajectory:

[0054] function Boxs = GenerateCorridor(traj,map);

[0055] Input: traj and map;

[0056] Output: Boxes;

[0057] Take out the first point (x,y) in the trajectory;

[0058] Execute the function FunA(x,y,map) and get its return value occupying the grid map Gmap;

[0059] Execute the function FunB(Gmap) to obtain the Rmap composed of the occupied grids after the return value is merged, which is used for collision detection in the corridor construction process;

[0060] For each i in {1,2,…,N}, do

[0061] Extract the coordinates of the i-th trajectory point (x i ,yi );

[0062] According to the trajectory point (x i ,y i ) and vehicle parameters to calculate the center point (x io ,y io )

[0063] Execute the function FunC(x io ,y io ,Rmap) gets its return value - the expansion increment lengthC in four directions;

[0064] Execute the function FunD(x io ,y io ,Rmap,lengthC), and get its return value—the final expansion increment lengthD in four directions;

[0065] According to (x io ,y io ) and length D are calculated by formula (8) to obtain the driving corridor area box = [x min ,x max ,y min ,y max ];

[0066] Boxes.push_back(box);

[0067] End for

[0068] Return Boxes;

[0069] Among them, the input list of the execution function FunB (Gmap) is the occupied grid map Gmap obtained by running FunA, and the output list is the merged occupied grid map Rmap; the execution function FunB (Gmap) processes the M×M occupied grid map Gmap generated by the function FunA, merges the adjacent occupied grids in each column to form a new occupied grid, and finally stores all the occupied grids in Rmap for collision detection in the subsequent driving corridor construction process. The specific execution process is:

[0070] For each i in {1,2,…,M-1}, do

[0071] For each j in {1,2,…,M-1}, do

[0072] If(Gmap[i][j].is_occupy∩!Gmap[i][j].is_used),then

[0073] Gmap[i][j].is_used = true;

[0074] Execute the raster merging function, Dfs_expand(i,j,Gmap), to get the merged rectange;

[0075] Rmap.push_back(rectangle);

[0076] End if

[0077] End for

[0078] End for

[0079] Return Rmap;

[0080] If Gmap[i][j].is_used is true, it means that the grid has been merged, if it is false, it means that the grid has not been merged;

[0081] The function Dfs_expand starts from the currently occupied grid and merges the adjacent, unmerged occupied grids one by one along the Gmap column direction upwards; if the grid above is occupied and has not been merged, it continues to merge upwards; if it encounters an unoccupied grid or a merged grid or there is no grid above, the merging process stops; after stopping the merging, it outputs the rectangle formed by the currently merged grid area; the specific execution process is as follows:

[0082] function rectangle = Dfs_expand(i,j,&Gmap)

[0083] Input: currently occupied grid row index i, column index j, reference to the occupied grid map Gmap

[0084] Output: The merged grid area rectangle

[0085] row_start = i;

[0086] row_current=i,col=j;

[0087] While(row_current <M-1∩Gmap[row_current+1][col].is_occupy∩!Gmap[row_current+1][col].is_used),do

[0088] Gmap[row_current+1][col].is_used=true;

[0089] row_current←row_current+1;

[0090] End while

[0091] row_end = row_current;

[0092] According to row_start and row_end, calculate the x, y, length and width of the merged grid using formula (9);

[0093] rectangle=[x,y,length,width];

[0094] Return rectangle;

[0095] Formula (9) is as follows:

[0096]

[0097] Where (x, y) is the center coordinate of the merged grid area, length is the length, and width is the width.

[0098] In a preferred embodiment of the present invention, the execution function FunA(x, y, map) is to construct a grid map within a certain range of the vehicle, and mark the grids into which obstacles in the environment fall as occupied;

[0099] The input list of FunA is the vehicle position (x, y) and the map map composed of scattered obstacle points, and the output is the occupied grid map Gmap; the specific implementation steps are as follows:

[0100] The first step is to determine the grid map range x min ,x max ,y min ,y max , create an occupied grid map Gmap of size M×M, and initialize all grids in the map to be unoccupied;

[0101] The second step is to select the left rear of the vehicle's current position as the map starting point and the right front as the end point, that is, x_start = xx min ,y_start=yy min ,x_end=x+x max ,y_end=y+y max ;

[0102] The third step is to determine the grid map resolution x_resolution = (x max -x min) / M,y_resolution=(y max -y min ) / M;

[0103] Step 4: traverse the obstacles in the map;

[0104] Step 5: Return the processed occupied grid map Gmap.

[0105] Furthermore, the obstacles in the map traversed in the fourth step include:

[0106] First, the obstacles within the range of the grid map are screened out;

[0107] Next, calculate the positions of these obstacles in the grid map Gmap and mark these positions as occupied. The specific execution process of this step is as follows:

[0108] For each (xo, yo) in map, do

[0109] If(x_start≤xo <x_end∩y_start≤yo<y_end),then

[0110] Calculate the position of the obstacle in the grid map: row = floor((yo-y_start) / y_resolution), col = floor((xo-x_start) / x_resolution);

[0111] Gmap[row][col].is_occupy = true, marked as occupied;

[0112] End if

[0113] End for.

[0114] In a preferred embodiment of the present invention, the function FunC(x io ,y io ,Rmap) is to expand the corridor area by expanding all around the rectangle at the beginning of the corridor construction; execute the function FunC(x io ,y io , Rmap) is the vehicle position (x, y) and the merged occupancy grid Rmap for collision detection, and the output is the expansion increment lengthC in four directions; the specific implementation steps of the function are as follows:

[0115] Step 1, preparation: First, define the four directions of rectangle expansion direction = {π / 2, 0, -π / 2, π}, and initialize the expansion length vector lengthC = [0, 0, 0, 0] corresponding to the four directions, and initialize the current rectangular area X 0 is the current center point (x, y), the initial expansion times i is 1, and the single expansion step is Δs;

[0116] The second step is to expand the rectangle in four directions at the same time: each time the existing rectangular area is expanded in four directions at the same time by Δs, that is, lengthC = [i*Δs,i*Δs,i*Δs,i*Δs]; the expanded rectangle is defined as X i Perform collision detection with the merged occupied grid RMAP obtained by running the previous function: If a collision occurs, the current extension is invalid and the extension is returned, that is, lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], and exit the function to return the current extension length; If no collision occurs, the current extension is considered valid and the number of extensions i is increased by 1;

[0117] Step 3: End condition: Repeat the expansion process in step 2 until the cumulative expansion increment is greater than the upper limit of the expansion increment, that is, i*Δs≥L limit , ends the extension and returns the final extension length lengthC; the specific execution process of the function is as follows:

[0118] Function lengthC = FunC(x,y,Rmap)

[0119] Input: center point (x, y), merged occupied grid Rmap

[0120] Output: the expanded length vector lengthC

[0121] Initialize the expansion length vector lengthC = [0,0,0,0];

[0122] Define the current rectangular area X 0 , initialized to the center point (x,y);

[0123] Initialize the extension times i=1;

[0124] Set the single expansion step size to Δs;

[0125] While i*Δs <L limit , do

[0126] Expand Δs in four directions simultaneously, i.e. lengthC = [i*Δs, i*Δs, i*Δs, i*Δs];

[0127] If the extended rectangular area X i Collision with the merged occupied grid Rmap occurs, then

[0128] lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], i.e., this extension is returned;

[0129] Return lengthC;

[0130] Else

[0131] i←i+1;

[0132] End if

[0133] End while

[0134] Return lengthC.

[0135] In a preferred embodiment of the present invention, the function FunD(x io ,y io ,Rmap,lengthC) is to further expand the corridor area by gradually expanding in four directions according to the expansion increment obtained after running the FunC function, so as to ensure that the corridor is fully expanded in each direction; the input list of the FunD function is the vehicle position (x, y), the merged occupancy grid Rmap for collision detection, and the expansion increment lengthC in four directions; the output is the final expansion increment lengthD in four directions. The specific implementation steps are as follows:

[0136] Step 1, preparation: First, define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, construct the expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction; and initialize the expansion length vector lengthD corresponding to the four directions to the expansion length vector lengthC obtained by running the FunC function, and set the single expansion step length to Δs;

[0137] The second step is to expand the rectangle in each direction according to the defined expansion direction direction: traverse the expandable direction set Y one by one, increase the length in the current direction i by the unit step length Δs, that is, lengthi+=Δs, to obtain a new rectangular area, check whether the expanded rectangular area collides with the merged occupied grid Rmap and the expansion length Length in this direction i Whether the maximum expansion limit L is exceeded limit ; If a collision occurs or the limit is exceeded: reduce the extension length in direction i by Lengthi - = Δs, that is, the current expansion is returned; and direction i is removed from the expandable direction set Y, and the direction will not be expanded in the future;

[0138] Step 3, end condition: repeat the expansion process in step 2 until the set Y is empty, that is, there is no direction to expand, and return the final expansion length lengthD;

[0139] The specific execution process of the function is as follows:

[0140] Function lengthD = FunD(x,y,RMAP,lengthC)

[0141] Input: vehicle position (x, y), merged occupied grid Rmap and extended length increment lengthC

[0142] Output: lengthD

[0143] Define the expansion direction set direction = {π / 2, 0, -π / 2, π} (clockwise);

[0144] Construct an expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction;

[0145] Initialize the expansion increments in four directions lengthD=lengthC, and define the single expansion step length as Δs;

[0146] While Y , do

[0147] for each i∈Y, do

[0148] LengthDi←LengthDi+Δs, expand the step length Δs in the i direction;

[0149] if (the expanded rectangular area collides with the merged occupied grid RMAP)∪(lengthi>Llimit), then

[0150] LengthDi←LengthDi-Δs, retract the expansion in this direction;

[0151] Move i out of the expandable direction set Y, and no further expansion will be performed in this direction;

[0152] End if

[0153] End for

[0154] End while

[0155] Return length D.

[0156] Compared with the traditional solution, the solution proposed by the present invention has the following beneficial effects:

[0157] 1. Short obstacle avoidance time: The corridor construction process is optimized to reduce the number of collision detections during the construction process; the grid map is optimized to reduce the number of collision detection objects; thus reducing the time consumption of collision detection and improving the real-time performance of the trajectory planning system;

[0158] 2. Obstacle avoidance is time-consuming and stable: Even in an environment with many obstacles and high complexity, the time required for collision detection will not increase significantly, which improves the stability of the trajectory planning system. BRIEF DESCRIPTION OF THE DRAWINGS

[0159] 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 work.

[0160] Figure 1 It is a schematic diagram of a disc covering a car body;

[0161] Figure 2 Schematic diagram of the driving corridor constructed for the traditional method;

[0162] Figure 3 The grid map constructed to execute the function FunA;

[0163] Figure 4 is the grid map of the function FunB;

[0164] Figure 5 Optimize schematics for grids;

[0165] Figure 6 This is a schematic diagram of the extension method of FunC;

[0166] Figure 7 It is a schematic diagram comparing the present invention with the traditional method. DETAILED DESCRIPTION

[0167] 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.

[0168] The high-performance intelligent vehicle trajectory planning method based on flexible rasterization includes the following steps:

[0169] S1. Construct a grid map within a certain range of the vehicle and mark the grids where obstacles fall as occupied.

[0170] S2. Process the occupied grid map obtained in S1, merge adjacent occupied grids in each column to obtain new occupied grids, and store all the merged occupied grids in a container for subsequent collision detection.

[0171] S3. Start expanding the driving corridor. Usually in the early stage of the construction of the driving corridor, the expandable space is usually large. Therefore, the original method of gradually expanding the driving corridor in all directions is abandoned here, and it is changed to expand in all directions at the same time. The driving corridor obtained after each expansion is subjected to collision detection with the merged occupied grid obtained in S2. If a collision occurs or the maximum expansion increment is reached, the current expansion is returned, and the expansion increments in four directions are output to proceed to the next step.

[0172] S4. Based on the extended corridor in S3, the corridor is further extended in a clockwise direction to ensure that the four directions of upward, rightward, downward and leftward are fully extended. During the extension process, if a collision occurs in a certain direction or the maximum extension increment is reached, the extension is returned and no further extension is performed in that direction. This process is continued until the four directions cannot be extended any further. The final extension increments in the four directions are obtained and the final corridor area is calculated according to formula (8):

[0173]

[0174] It should be noted that the expansion direction of the driving corridor is not fixed. In the present invention, the expansion direction adopted is {π / 2, 0, -π / 2, π}, which respectively represent the four directions of upward, rightward, downward and leftward.

[0175] In formula (8), (x, y) is the starting point of the corridor extension, and length 1 is the upward expansion increment (π / 2 direction), length 2 The extension increment for the right side of the vehicle (direction 0), length 3 is the extension increment below the vehicle (-π / 2 direction), length 4 The extension increment for the left side of the vehicle (π direction);

[0176] The overall process is executed as the following function: Boxs = GenerateCorridor (traj, map). The input list of this function is the vehicle trajectory traj, which contains N ordered trajectory points. Each trajectory point contains the vehicle location information (x, y). Map represents a map composed of the scattered coordinates of obstacles. The return value of this function is the set Boxs of the drivable corridor area corresponding to each trajectory point in this trajectory:

[0177] function Boxs = GenerateCorridor(traj,map);

[0178] Input: traj and map;

[0179] Output: Boxes;

[0180] Take out the first point (x,y) in the trajectory;

[0181] Execute the function FunA(x,y,map) and get its return value occupying the grid map Gmap;

[0182] Execute the function FunB(Gmap) to obtain the Rmap composed of the occupied grids after the return value is merged, which is used for collision detection in the corridor construction process;

[0183] For each i in {1,2,…,N}, do

[0184] Extract the coordinates of the i-th trajectory point (x i ,y i );

[0185] According to the trajectory point (x i ,y i ) and vehicle parameters to calculate the center point (x io ,y io )

[0186] Execute the function FunC(x io ,y io ,Rmap) gets its return value - the expansion increment lengthC in four directions;

[0187] Execute the function FunD(x io ,y io ,Rmap,lengthC), and get its return value—the final expansion increment lengthD in four directions;

[0188] According to (x io ,y io ) and length D are calculated by formula (8) to obtain the driving corridor area box = [xmin ,x max ,y min ,y max ];

[0189] Boxes.push_back(box);

[0190] End for

[0191] Return Boxes.

[0192] The following will explain in detail the implementation process of the four functions FunA, FunB, FunC and FunD:

[0193] 1. Execute the function FunA(x,y,map)

[0194] The main function of executing the function FunA(x,y,map) is to construct a grid map within a certain range of the vehicle and mark the grids where obstacles fall into the environment as occupied. The constructed grid map effect is as follows Figure 3 shown.

[0195] Figure 3 Where (x, y) is the position of the vehicle. The area within a certain range of the vehicle's position is selected to construct a grid map. This can effectively remove obstacles far away from the vehicle. This will not only not affect the safety of the vehicle, because distant obstacles usually do not pose a threat to the driving path, but also reduce unnecessary collision detection to improve computing efficiency.

[0196] The input list of FunA is the vehicle position (x, y) and the map map composed of scattered obstacle points, and the output is the occupied grid map Gmap; the specific implementation steps are as follows:

[0197] The first step is to determine the grid map range x min ,x max ,y min ,y max , create an occupied grid map Gmap of size M×M, and initialize all grids in the map to be unoccupied;

[0198] The second step is to select the left rear of the vehicle's current position as the map starting point and the right front as the end point, that is, x_start = xx min ,y_start=yy min ,x_end=x+x max ,y_end=y+y max ;

[0199] The third step is to determine the grid map resolution x_resolution = (x max -x min) / M,y_resolution=(y max -y min ) / M;

[0200] The fourth step is to traverse the obstacles in the map. First, filter out the obstacles within the range of the grid map. Then, calculate the positions of these obstacles in the grid map Gmap and mark these positions as occupied. The specific execution process of this step is as follows:

[0201] First, the obstacles within the range of the grid map are screened out;

[0202] Next, calculate the positions of these obstacles in the grid map Gmap and mark these positions as occupied. The specific execution process of this step is as follows:

[0203] For each (xo, yo) in map, do

[0204] If(x_start≤xo <x_end∩y_start≤yo<y_end),then

[0205] Calculate the position of the obstacle in the grid map: row = floor((yo-y_start) / y_resolution), col = floor((xo-x_start) / x_resolution);

[0206] Gmap[row][col].is_occupy = true, marked as occupied;

[0207] End if

[0208] End for;

[0209] Step 5: Return the processed occupied grid map Gmap.

[0210] By introducing the grid map for collision detection, the object of collision detection is no longer the obstacles in the environment, but the occupied grids in the grid map; the main advantage of this method is that no matter how many or how complex the obstacles around the vehicle are, the calculation amount of collision detection is always fixed to the collision detection between the driving corridor and at most M×M regular rectangular grids. This method effectively avoids the problem of significantly increased collision detection time due to the increase in the number or complexity of obstacles in complex environments.

[0211] 2. Execute function FunB(Gmap)

[0212] The input list of the execution function FunB(Gmap) is the occupied grid map Gmap obtained by running FunA, and the output list is the merged occupied grid map Rmap. The execution function FunB(Gmap) processes the M×M occupied grid map Gmap generated by the function FunA, merges the adjacent occupied grids in each column to form a new occupied grid, and finally stores all the occupied grids in Rmap for collision detection in the subsequent corridor construction process. Please refer to Figure 4 ,exist Figure 4 Middle: Rmap is composed of 5 merged occupancy grids.

[0213] The specific implementation process of this step is:

[0214] For each i in {1,2,…,M-1}, do

[0215] For each j in {1,2,…,M-1}, do

[0216] If(Gmap[i][j].is_occupy∩!Gmap[i][j].is_used),then

[0217] Gmap[i][j].is_used = true;

[0218] Execute the raster merging function, Dfs_expand(i,j,Gmap), to get the merged rectange;

[0219] Rmap.push_back(rectangle);

[0220] End if

[0221] End for

[0222] End for

[0223] Return Rmap;

[0224] If Gmap[i][j].is_used is true, it means that the raster has been merged. If it is false, it means that the raster has not been merged.

[0225] The function Dfs_expand starts from the currently occupied grid and merges the adjacent, unmerged occupied grids one by one upward along the Gmap column direction; if the grid above is occupied and has not been merged, it continues to merge upward; if it encounters an unoccupied grid or a merged grid or there is no grid above, the merging process is stopped; after stopping the merging, the rectangle formed by the current merged grid area is output.

[0226] The specific implementation process is as follows:

[0227] function rectangle = Dfs_expand(i,j,&Gmap)

[0228] Input: currently occupied grid row index i, column index j, reference to the occupied grid map Gmap

[0229] Output: The merged grid area rectangle

[0230] row_start = i;

[0231] row_current=i,col=j;

[0232] While(row_current <M-1∩Gmap[row_current+1][col].is_occupy∩!Gmap[row_current+1][col].is_used),do

[0233] Gmap[row_current+1][col].is_used=true;

[0234] row_current←row_current+1;

[0235] End while

[0236] row_end = row_current;

[0237] According to row_start and row_end, calculate the x, y, length and width of the merged grid using formula (9);

[0238] rectangle=[x,y,length,width];

[0239] Return rectangle;

[0240] Formula (9) is as follows:

[0241]

[0242] Where (x, y) is the center coordinate of the merged grid area, length is the length, and width is the width.

[0243] A major innovation of the present invention is that FunB merges adjacent occupied grids in each column to form a new occupied grid for collision detection. With this method, there is no need to perform collision detection on the expanded corridor with the occupied grids in the grid map one by one. Instead, it only needs to perform collision detection on a relatively small number of merged occupied grids. This reduces the number of objects that need to be collided, reduces the computational complexity, and thus reduces the time consumption of collision detection. Figure 4 As shown in the figure, originally the extended corridor needed to be collided with 14 occupied grids, but after processing, only the extended corridor needed to be collided with the 5 occupied grids after merging. It is worth noting that the grids before and after merging are regular rectangles, which means that the computational complexity will not increase due to the merging of grids.

[0244] In addition, the occupied grid can be further optimized, such as Figure 5 As shown: on the basis of merging adjacent occupied grids in each column, two merged occupied grids with the same height and center points at the same horizontal position in adjacent columns are further merged again. In this way, the number of collision detection objects can be further reduced, thereby improving the real-time performance of collision detection. Figure 5 In the figure, the part shown in the red box is where we further merge the three equal-height occupancy grids in the original adjacent columns to form a new occupancy grid.

[0245] 3. Execute the function FunC(x io ,y io ,Rmap)

[0246] Execute the function FunC(x io ,y io ,Rmap) is to expand the corridor area by expanding all four sides of the rectangle at the beginning of the corridor construction. Figure 6 ,like Figure 6 As shown, through this method, the work that originally required four expansions can be completed in one go, which can greatly speed up the construction process of the corridor.

[0247] Execute the function FunC(x io ,y io , Rmap) is the vehicle position (x, y) and the merged occupancy grid Rmap for collision detection, and the output is the expansion increment lengthC in four directions; the specific implementation steps of the function are as follows:

[0248] Step 1, preparation: First, define the four directions of rectangle expansion direction = {π / 2, 0, -π / 2, π}, and initialize the expansion length vector lengthC = [0, 0, 0, 0] corresponding to the four directions, and initialize the current rectangular area X 0 is the current center point (x, y), the initial expansion times i is 1, and the single expansion step is Δs;

[0249] The second step is to expand the rectangle in four directions at the same time: each time the existing rectangular area is expanded in four directions at the same time by Δs, that is, lengthC = [i*Δs,i*Δs,i*Δs,i*Δs]; the expanded rectangle is defined as X i Perform collision detection with the merged occupied grid RMAP obtained by running the previous function: If a collision occurs, the current extension is invalid and the extension is returned, that is, lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], and exit the function to return the current extension length; If no collision occurs, the current extension is considered valid and the number of extensions i is increased by 1;

[0250] Step 3: End condition: Repeat the expansion process in step 2 until the cumulative expansion increment is greater than the upper limit of the expansion increment, that is, i*Δs≥L limit , ends the extension and returns the final extension length lengthC; the specific execution process of the function is as follows:

[0251] Function lengthC = FunC(x,y,Rmap)

[0252] Input: center point (x, y), merged occupied grid Rmap

[0253] Output: the expanded length vector lengthC

[0254] Initialize the expansion length vector lengthC = [0,0,0,0];

[0255] Define the current rectangular area X 0 , initialized to the center point (x,y);

[0256] Initialize the extension times i=1;

[0257] Set the single expansion step size to Δs;

[0258] While i*Δs <L limit , do

[0259] Expand Δs in four directions simultaneously, i.e. lengthC = [i*Δs, i*Δs, i*Δs, i*Δs];

[0260] If the extended rectangular area X i Collision with the merged occupied grid Rmap occurs, then

[0261] lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], i.e., this extension is returned;

[0262] Return lengthC;

[0263] Else

[0264] i←i+1;

[0265] End if

[0266] End while

[0267] Return lengthC.

[0268] 4. Execute the function FunD(x io ,y io ,Rmap,lengthC)

[0269] Execute the function FunD(x io ,y io ,Rmap,lengthC) is to further expand the corridor area by gradually expanding in four directions according to the expansion increment obtained after running the FunC function, so as to ensure that the corridor is fully expanded in each direction; the input list of the FunD function is the vehicle position (x, y), the merged occupancy grid Rmap for collision detection, and the expansion increment lengthC in four directions; the output is the final expansion increment lengthD in four directions. The specific implementation steps are as follows:

[0270] Step 1, preparation: First, define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, construct the expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction; and initialize the expansion length vector lengthD corresponding to the four directions to the expansion length vector lengthC obtained by running the FunC function, and set the single expansion step length to Δs;

[0271] The second step is to expand the rectangle in each direction according to the defined expansion direction direction: traverse the expandable direction set Y one by one, increase the length in the current direction i by the unit step length Δs, that is, lengthi+=Δs, to obtain a new rectangular area, check whether the expanded rectangular area collides with the merged occupied grid Rmap and the expansion length Length in this direction iWhether the maximum expansion limit L is exceeded limit ; If a collision occurs or the limit is exceeded: reduce the extension length in direction i by Length i - = Δs, that is, the current expansion is returned; and direction i is removed from the expandable direction set Y, and the direction will not be expanded in the future;

[0272] Step 3, end condition: repeat the expansion process in step 2 until the set Y is empty, that is, there is no direction to expand, and return the final expansion length lengthD;

[0273] The specific execution process of the function is as follows:

[0274] Function lengthD = FunD(x,y,RMAP,lengthC)

[0275] Input: vehicle position (x, y), merged occupied grid Rmap and extended length increment lengthC

[0276] Output: lengthD

[0277] Define the expansion direction set direction = {π / 2, 0, -π / 2, π} (clockwise);

[0278] Construct an expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction;

[0279] Initialize the expansion increments in four directions lengthD=lengthC, and define the single expansion step length as Δs;

[0280] While Y , do

[0281] for each i∈Y,do

[0282] LengthDi←LengthDi+Δs, expand the step length Δs in the i direction;

[0283] if (the expanded rectangular area collides with the merged occupied grid RMAP)∪(lengthi>Llimit), then

[0284] LengthDi←LengthDi-Δs, retract the expansion in this direction;

[0285] Move i out of the expandable direction set Y, and no further expansion will be performed in this direction;

[0286] End if

[0287] End for

[0288] End while

[0289] Return length D.

[0290] Through the two functions FunC and FunD, the local driving corridor area where the vehicle can finally drive at a given position (x, y) is constructed. Compared with the traditional method of expanding the driving corridor in each direction one by one, the present invention proposes a flexible driving corridor construction scheme, the core of which is to dynamically adjust the expansion strategy according to the expandable space size of the driving corridor. The innovation lies in: in the initial stage of the construction of the driving corridor, when the expandable space is large, the method of simultaneous expansion in four directions is adopted; when a collision occurs during the expansion process, the current expansion is returned, and a safe solution of expanding in each direction is used to ensure that all directions can be fully expanded; this method can significantly speed up the construction process of the driving corridor in the initial stage. Please refer to Figure 7 ,from Figure 7 It can be clearly seen that, compared with the traditional solution, the solution adopted by the present invention accelerates the construction process of the driving corridor without affecting the size of the finally constructed driving corridor.

[0291] The flexible driving corridor construction method combined with rasterization processing adopted in the present invention mainly accelerates the obstacle avoidance constraint modeling process. The final obstacle avoidance constraint form is the same as Formula 6, and the trajectory planning problem model finally constructed is shown in Formula 7; after successfully constructing the trajectory planning problem model (i.e., Formula 7), it should be solved, and a numerical optimization method is used to solve the dynamic optimization problem, such as the interior point algorithm or the sequential quadratic programming algorithm. Since there are already mature constraint optimization problem solving solutions on the market, any one of them can be used, such as the interior point algorithm, which will not be described in this application.

[0292] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A high-performance intelligent vehicle trajectory planning method based on flexible rasterization, characterized in that: The following steps are involved: S1. Build a grid map within a certain range of the vehicle and mark the grids where obstacles fall as occupied; S2, processing the occupied grid map obtained in S1, merging adjacent occupied grids in each column to obtain new occupied grids, and storing all the merged occupied grids in a container for subsequent collision detection; S3, start the expansion of the driving corridor. In the early stage of the construction of the driving corridor, expand it in all directions at the same time, and perform collision detection on the driving corridor obtained after each expansion and the merged occupied grid obtained in S2. If a collision occurs or the maximum expansion increment is reached, return to this expansion, output the expansion increments in four directions and proceed to the next step; S4. Based on the extended corridor of S3, the corridor is further extended in a clockwise direction to ensure that the corridor is fully extended in the four directions of upward, rightward, downward and leftward. During the expansion process, if a collision occurs in a certain direction or the maximum expansion increment is reached, the current expansion is returned and no further expansion is performed in that direction. This process continues until all four directions cannot be expanded further. The final expansion increments in the four directions are obtained and the final driving corridor area is calculated according to formula (8): In formula (8), (x, y) is the starting position of the corridor expansion, length1 is the expansion increment on the top (π / 2 direction), length2 is the expansion increment on the right side of the vehicle (0 direction), length3 is the expansion increment on the bottom of the vehicle (-π / 2 direction), and length4 is the expansion increment on the left side of the vehicle (π direction); The overall process is executed as the following function: Boxs = GenerateCorridor (traj, map). The input list of this function is the vehicle trajectory traj, which contains N ordered trajectory points. Each trajectory point contains the vehicle location information (x, y). Map represents a map composed of the scattered coordinates of obstacles. The return value of this function is the set Boxs of the drivable corridor area corresponding to each trajectory point in this trajectory: function Boxs = GenerateCorridor(traj,map); Input: traj and map; Output: Boxes; Take out the first point (x,y) in the trajectory; Execute the function FunA(x,y,map) and get its return value occupying the grid map Gmap; Execute the function FunB(Gmap) to obtain the Rmap composed of the occupied grids after the return value is merged, which is used for collision detection in the corridor construction process; For each i in {1,2,…,N}, do Extract the coordinates of the i-th trajectory point (x i ,y i ); According to the trajectory point (x i ,y i ) and vehicle parameters to calculate the center point (x io ,y io ) Execute the function FunC(x io ,y io ,Rmap) gets its return value - the expansion increment lengthC in four directions; Execute the function FunD(x io ,y io ,Rmap,lengthC), and get its return value—the final expansion increment lengthD in four directions; According to (x io ,y io ) and length D are calculated by formula (8) to obtain the driving corridor area box = [x min ,x max ,y min ,y max ]; Boxes.push_back(box); End for Return Boxes; Among them, the input list of the execution function FunB (Gmap) is the occupied grid map Gmap obtained by running FunA, and the output list is the merged occupied grid map Rmap; the execution function FunB (Gmap) processes the M×M occupied grid map Gmap generated by the function FunA, merges the adjacent occupied grids in each column to form a new occupied grid, and finally stores all the occupied grids in Rmap for collision detection in the subsequent driving corridor construction process. The specific execution process is: For each i in {1,2,…,M-1}, do For each j in {1,2,…,M-1}, do If(Gmap[i][j].is_occupy∩!Gmap[i][j].is_used),then Gmap[i][j].is_used = true; Execute the raster merging function, Dfs_expand(i,j,Gmap), to get the merged rectange; Rmap.push_back(rectangle); End if End for End for Return Rmap; If Gmap[i][j].is_used is true, it means that the grid has been merged, if it is false, it means that the grid has not been merged; The function Dfs_expand starts from the currently occupied grid and merges the adjacent, unmerged occupied grids one by one along the Gmap column direction upwards; if the grid above is occupied and has not been merged, it continues to merge upwards; if it encounters an unoccupied grid or a merged grid or there is no grid above, the merging process stops; after stopping the merging, it outputs the rectangle formed by the currently merged grid area; the specific execution process is as follows: function rectangle = Dfs_expand(i,j,&Gmap) Input: currently occupied grid row index i, column index j, reference to the occupied grid map Gmap Output: The merged grid area rectangle row_start = i; row_current=i,col=j; While(row_current <M-1∩Gmap[row_current+1][col].is_occupy∩!Gmap[row_current+1][col].is_used),do Gmap[row_current+1][col].is_used=true; row_current←row_current+1; End while row_end = row_current; According to row_start and row_end, calculate the x, y, length and width of the merged grid using formula (9); rectangle=[x,y,length,width]; Return rectangle; Formula (9) is as follows: Where (x, y) is the center coordinate of the merged grid area, length is the length, and width is the width.

2. The high-performance intelligent vehicle trajectory planning method based on flexible gridding according to claim 1 is characterized in that: The execution function FunA(x, y, map) constructs a grid map within a certain range of the vehicle and marks the grids where obstacles in the environment fall as occupied; The input list of FunA is the vehicle position (x, y) and the map map composed of scattered obstacle points, and the output is the occupied grid map Gmap; the specific implementation steps are as follows: The first step is to determine the grid map range x min ,x max ,y min ,y max , create an occupied grid map Gmap of size M×M, and initialize all grids in the map to be unoccupied; The second step is to select the left rear of the vehicle's current position as the map starting point and the right front as the end point, that is, x_start = xx min ,y_start=yy min ,x_end=x+x max ,y_end=y+y max ; The third step is to determine the grid map resolution x_resolution = (x max -x min ) / M,y_resolution=(y max -y min ) / M; Step 4: traverse the obstacles in the map; Step 5: Return the processed occupied grid map Gmap.

3. The high-performance intelligent vehicle trajectory planning method based on flexible gridding according to claim 2 is characterized in that: The obstacles in the map traversed in the fourth step include: First, the obstacles within the range of the grid map are screened out; Next, calculate the positions of these obstacles in the grid map Gmap and mark these positions as occupied. The specific execution process of this step is as follows: For each (xo, yo) in map, do If(x_start≤xo <x_end∩y_start≤yo<y_end),then Calculate the position of the obstacle in the grid map: row = floor((yo-y_start) / y_resolution), col = floor((xo-x_start) / x_resolution); Gmap[row][col].is_occupy = true, marked as occupied; End if End for.

4. The high-performance intelligent vehicle trajectory planning method based on flexible gridding according to claim 1 is characterized in that: The execution function FunC(x io ,y io ,Rmap) is to expand the corridor area by expanding all around the rectangle at the beginning of the corridor construction; execute the function FunC(x io ,y io , Rmap) is the vehicle position (x, y) and the merged occupancy grid Rmap for collision detection, and the output is the expansion increment lengthC in four directions; the specific implementation steps of the function are as follows: Step 1, preparation: First, define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, and initialize the expansion length vector lengthC = [0, 0, 0, 0] corresponding to the four directions, initialize the current rectangular area X0 to the current center point (x, y), initialize the expansion times i to 1, and the single expansion step length to Δs; The second step is to expand the rectangle in four directions at the same time: each time the existing rectangular area is expanded in four directions at the same time by Δs, that is, lengthC = [i*Δs,i*Δs,i*Δs,i*Δs]; the expanded rectangle is defined as X i Perform collision detection with the merged occupied grid RMAP obtained by running the previous function: If a collision occurs, the current extension is invalid and the extension is returned, that is, lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], and exit the function to return the current extension length; If no collision occurs, the current extension is considered valid and the number of extensions i is increased by 1; Step 3: End condition: Repeat the expansion process in step 2 until the cumulative expansion increment is greater than the upper limit of the expansion increment, that is, i*Δs≥L limit , ends the extension and returns the final extension length lengthC; the specific execution process of the function is as follows: Function lengthC = FunC(x,y,Rmap) Input: center point (x, y), merged occupied grid Rmap Output: the expanded length vector lengthC Initialize the expansion length vector lengthC = [0,0,0,0]; Define the current rectangular area X0 and initialize it to the center point (x, y); Initialize the extension times i=1; Set the single expansion step size to Δs; While i*Δs<L limit ,do Expand Δs in four directions simultaneously, i.e. lengthC = [i*Δs, i*Δs, i*Δs, i*Δs]; If the extended rectangular area X i Collision with the merged occupied grid Rmap occurs, then lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], i.e., this extension is returned; Return lengthC; Else i←i+1; End if End while Return lengthC.

5. The high-performance intelligent vehicle trajectory planning method based on flexible gridding according to claim 1 is characterized in that: The execution function FunD(x io ,y io ,Rmap,lengthC) is to further expand the corridor area by gradually expanding in four directions according to the expansion increment obtained after running the FunC function, so as to ensure that the corridor is fully expanded in each direction; the input list of the FunD function is the vehicle position (x, y), the merged occupancy grid Rmap for collision detection, and the expansion increment lengthC in four directions; the output is the final expansion increment lengthD in four directions. The specific implementation steps are as follows: Step 1, preparation: First, define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, construct the expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction; and initialize the expansion length vector lengthD corresponding to the four directions to the expansion length vector lengthC obtained by running the FunC function, and set the single expansion step length to Δs; The second step is to expand the rectangle in each direction according to the defined expansion direction direction: traverse the expandable direction set Y one by one, increase the length in the current direction i by the unit step length Δs, that is, lengthi+=Δs, to obtain a new rectangular area, check whether the expanded rectangular area collides with the merged occupied grid Rmap and the expansion length Length in this direction i Whether the maximum expansion limit L is exceeded limit ; If a collision occurs or the limit is exceeded: reduce the extension length in direction i by Length i - = Δs, that is, the current expansion is returned; and direction i is removed from the expandable direction set Y, and the direction will not be expanded in the future; Step 3, end condition: repeat the expansion process in step 2 until the set Y is empty, that is, there is no direction to expand, and return the final expansion length lengthD; The specific execution process of the function is as follows: Function lengthD = FunD(x,y,RMAP,lengthC) Input: vehicle position (x, y), merged occupied grid Rmap and extended length increment lengthC Output: lengthD Define the expansion direction set direction = {π / 2, 0, -π / 2, π} (clockwise); Construct an expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction; Initialize the expansion increments in four directions lengthD=lengthC, and define the single expansion step length as Δs; While Y do for each i∈Y,do LengthDi←LengthDi+Δs, expand the step length Δs in the i direction; if (the expanded rectangular area collides with the merged occupied grid RMAP)∪(lengthi>Llimit), then LengthDi←LengthDi-Δs, retract the expansion in this direction; Move i out of the expandable direction set Y, and no further expansion will be performed in this direction; End if End for End while Return length D.

Images