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

Abstract

The application discloses a kind of high-performance intelligent car trajectory planning method based on flexible gridding, comprising: constructing grid map in a certain range of vehicle, the grid that has obstacle falls into is marked as occupied;Adjacent occupied grid in each column is merged, new occupied grid is obtained, and all merged occupied grids are stored in a container;Start to expand driving corridor;On the basis of expanding driving corridor, continue to expand corridor using the steady method of expanding gradually along clockwise direction, ensure that four directions are fully expanded;During the expansion process, when collision occurs in a certain direction or the maximum expansion increment is reached, return to this expansion, and then no longer expand in that direction, continue this process until four directions cannot continue to expand, obtain the final expansion increment of four directions, and calculate the final driving corridor area. Compared with the traditional scheme, the application has short obstacle avoidance time and stable obstacle avoidance time.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent vehicle trajectory planning, and particularly relates to a high-performance intelligent vehicle trajectory planning method based on flexible gridding. BACKGROUND

[0002] The trajectory planning of an intelligent vehicle aims to generate a space-time trajectory reflecting the local path and speed of driving, to meet the kinematic constraints inside the vehicle and the external environmental obstacle avoidance constraints, and to optimize the established performance indicators as much as possible under the premise of strictly guaranteeing that the above constraints are met.

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

[0004] The trajectory planning problem is modeled as a constrained dynamic optimization problem, just like the modeling of all trajectory planning problems, which includes decision variables, constraints, and a cost function.

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

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

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

[0008] 3) phi ( t ) represents the deflection angle of the front wheel of the vehicle, omega ( t ) represents the rotation angular velocity corresponding to the deflection angle of the front wheel of the vehicle, theta ( t ) represents the attitude angle of the vehicle in the inertial coordinate system of the earth.

[0009] Therefore, the decision variables include the above x ( t ), y ( t ), v ( t ),​a t phi t omega t theta t

[0010] Cost function: The cost function in the dynamic optimization problem model is generally used to reflect the smoothness 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 are the weights specified by the user.

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

[0014] a) Vehicle kinematics constraints:

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

[0016]

[0017] where L W represents the wheelbase of the vehicle.

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

[0019]

[0020] where a min and a max represent the left and right boundary values of acceleration, v min and v max represent the left and right boundary values of speed, Ω max represents the maximum amplitude of the front wheel angular velocity omega (Φ t ), and Φ max represents the maximum amplitude of the front wheel steering angle phi (Ψ t ).

[0021] b) Two-point boundary constraints:

[0022] At the starting time of the trajectory planning time domain​​​​​​​​t = 0, 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, the vertical coordinate position is y0, the following constraints should be applied:

[0023]

[0024] Similarly, at the end of the trajectory planning time domain t T , the vehicle is expected to be in a certain specified motion state, for example, at that time the acceleration is af, the speed 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, the vertical coordinate position is yf, the following constraints should be applied:

[0025]

[0026] c) Obstacle avoidance constraint:

[0027] The obstacle avoidance constraint means that the driving trajectory of the intelligent vehicle needs to ensure driving safety, that is, to ensure that the vehicle does not collide with the obstacles in the environment. The obstacle avoidance constraint is the most complex and time-consuming part of the entire trajectory problem; the complexity of this constraint comes from the number, shape and dynamic changes of the obstacles in the environment, especially in complex urban environments, where there are many types of obstacles and irregular distribution, increasing the calculation difficulty.

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

[0029] Before building the driving corridor, the vehicle body is usually covered with N discs, as shown in Figure 1 Two discs are used to represent the vehicle body, and "the vehicle body does not collide with the obstacles" means that "both discs cannot collide with the obstacles". Shrink the two discs to their centers respectively, and inflate the radius R of the obstacles in the environment, then "both discs cannot collide with the obstacles" means that "the centers of the front and rear discs P ​f ,P r Not to collide with obstacles after swelling. Then start building the front and rear disc center P f ,P r The drivable rectangular area, that is, the driving corridor; and ensure P f ,P r At a certain moment, it is in the corresponding driving corridor area.

[0030] The method of building the driving corridor at this time, as shown in Figure 2 , mainly uses the method of gradually expanding, and the specific idea is: starting from the center point (x0, y0), according to the predetermined direction (clockwise in the figure), along the four sides of the rectangle gradually expanding a certain step length d. In this process, the expanded rectangle will be collided with the obstacles within a certain range around the vehicle. According to the detection result, it is decided whether to merge the expanded part into the existing driving corridor rectangle and whether to continue the expansion in this direction.

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

[0032] 1、Expansion starting point: first, from the center point (x0, y0), the center point (x0, y0) can be regarded as a rectangle with a length of 0 and a width of 0.

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

[0034] 3、Collision detection: after each expansion, the newly generated rectangular block will be collided with the obstacles within a certain range around the vehicle. This usually involves calculating the spatial relationship between the rectangular block and the obstacles to determine whether there is overlap or intersection.

[0035] 4、Merge and return: if the collision detection shows that the newly expanded rectangular block does not collide with the obstacles, it will be merged into the existing driving corridor rectangle. If collision occurs or the maximum expansion increment is reached, return this expansion, and do not expand in this direction in the subsequent expansion process. Set the maximum expansion increment to avoid wasting calculation time by generating a rectangular area with too large size.

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

[0037]

[0038] in( x f ( t ), y f ( t ))for t The center point position of the front disc of the vehicle at the moment, ( x r ( t ), y r ( t ))for t The center point position of the vehicle's rear disc at this moment. xf min , xf max , yf min , yf max ]for t The rectangular area where the center of the disk can move before time is x Axis and y The value range on the axis; [ xr min , xr max , yr min , yr max ]for t After the moment, the center point of the disk can move in the rectangular area x Axis and y The range of values ​​on the axis.

[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 practice, each time a corridor is expanded, collision detection must be performed between the expanded rectangular area and obstacles in the environment. Since corridor construction typically involves multiple expansions, this increases the number of collision checks required, thus increasing the overall collision detection time.

[0043] 2. When the number of obstacles in the environment is large and the 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 spent on collision detection involved in the corridor construction process can be reduced, the efficiency of the driving corridor construction can be significantly improved, the time spent on 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 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. It proposes a high-performance intelligent vehicle trajectory planning method based on flexible rasterization, which includes 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. 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;

[0049] S3. Start expanding the corridor. In the initial stage of corridor construction, expand in all directions simultaneously. Perform collision detection on the corridor obtained after each expansion and 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 and the next step is entered.

[0050] S4. Based on the corridor expanded in S3, the corridor is further expanded in a clockwise direction to ensure that the four directions of upward, rightward, downward, and leftward are fully expanded. During the expansion process, if a collision occurs in a certain direction or the maximum expansion increment is reached, the expansion is rolled back and no further expansion is performed in that direction. This process is continued until all four directions are unable to expand. The final expansion 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,length 1 for the upper extension increment (π / 2 direction), length 2 for the right side extension increment (0 direction), length 3 for the lower extension increment (-π / 2 direction), length 4 for the left side extension increment (π direction);

[0053] The overall flow is implemented as the following function: Boxs = GenerateCorridor(traj, map), which takes as input a list of vehicle trajectory traj, where N ordered trajectory points are included, each of which contains vehicle position information (x, y), and a map represented by a set of obstacle scatter coordinates. The function returns a set of drivable corridor regions Boxs corresponding to each trajectory point in the trajectory:

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

[0055] Input: traj and map;

[0056] Output: Boxs;

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

[0058] Execute the function FunA(x, y, map) to obtain its return value, which is an occupancy grid map Gmap;

[0059] Execute the function FunB(Gmap) to obtain its return value, which is a merged occupancy grid Rmap used for collision detection in the corridor construction process;

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

[0061] Extract the i-th trajectory point coordinates (x i , y i ) from traj;

[0062] Calculate the center point (x i , y i ) of the corridor extension according to the trajectory point (x io , y io ) and vehicle parameters

[0063] Execute the function FunC(x io , y io, Rmap) to get its return value - four direction expansion increment lengthC;

[0064] Execute function FunD(x io , y io ,Rmap, lengthC) to get its return value - four direction final expansion increment lengthD;

[0065] According to (x io , y io ) and lengthD, the driving corridor area of the vehicle at this position is calculated by formula (8) box= [x min , x max , y min , y max ];

[0066] Boxs.push_back(box);

[0067] End for

[0068] Return Boxs;

[0069] Wherein, the input list of the execution function FunB(Gmap) is the occupancy grid map Gmap obtained by running FunA, and the output list is the merged occupancy grid map Rmap; the execution function FunB(Gmap) is to process the M×M occupancy grid map Gmap generated by function FunA, merge adjacent occupied grids in each column to form new occupied grids, and finally store all occupied grids in Rmap for subsequent collision detection in the driving corridor construction process, and 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 grid merging function, Dfs_expand(i, j, Gmap), to get the merged rectangle;

[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 false, it means that the grid has not been merged;

[0081] The function Dfs_expand is to start from the current occupied grid, along the Gmap column direction to merge the adjacent, unmerged occupied grid one by one; if the grid above is occupied and not merged, continue to merge upwards; if it encounters an unoccupied grid or has been merged or there is no grid above, stop the merging process; after stopping the merging, output the rectangle formed by the current merged grid area; the specific execution process is as follows:

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

[0083] Input: the current occupied grid row index i, column index j, the reference of 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, row_end, the x, y, length, width of the merged grid are calculated by formula (9);

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

[0094] Return rectangle;

[0095] Formula (9) is as follows:

[0096]

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

[0098] In a preferred embodiment of the present application, the function FunA(x, y, map) is to construct a grid map within a certain range of the vehicle, and mark the grid where the obstacle in the environment falls as occupied;

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

[0100] First, determine the grid map range x min , x max , y min , y max , create an occupied grid map Gmap with a size of MxM, and initialize all grids in the map as unoccupied;

[0101] Second, select the left rear of the current position of the vehicle as the starting point of the map, and the right front as the end point, i.e. x_start = x- x min , y_start = y - y min , x_end = x + x max , y_end = y + y max ;

[0102] Third, determine the grid map resolution x_resolution = (x max - x min ) / M, y_resolution = (y max - y min ) / M; ​​

[0103] Fourthly, traverse the obstacles in the map;

[0104] Fifthly, return the processed occupancy grid map Gmap.

[0105] Further, the step of traversing the obstacles in the map comprises:

[0106] Firstly, filter out the obstacles within the range of the grid map;

[0107] Secondly, calculate the positions of these obstacles in the grid map Gmap and mark these positions as occupied, the specific implementation 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, mark as occupied;

[0112] End if

[0113] End for.

[0114] In a preferred embodiment of the present application, the function FunC(x io , y io , Rmap) is executed at the beginning of the construction of the driving corridor, using the method of expanding the driving corridor area to the four sides of the rectangle simultaneously; the input list of 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] First step, preparation: first define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, and initialize the four direction corresponding expansion length vector lengthC = [0, 0, 0, 0], initialize the current rectangular region X0 as the current center point (x, y), initialize the expansion number i as 1, and the single expansion step length as Δs;

[0116] Second step, expand the rectangle in four directions: each expansion will expand the existing rectangular region in four directions by Δs, that is, lengthC = [i*Δs, i*Δs, i*Δs, i*Δs]; define the expanded rectangle as X i Collision detection with the merged occupied grid RMAP obtained by running the last function: if collision occurs, this expansion is invalid, return to this expansion, that is, lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], and exit the function, return the current expansion length length; if no collision occurs, consider this expansion effective, and increase the expansion number i by 1;

[0117] Third step, end condition: repeat the expansion process of the second step until the cumulative expansion increment is greater than the upper limit of the expansion increment, that is, i*Δs≥L limit , end expansion, and return the final expansion 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: expanded length vector lengthC

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

[0122] Define the current rectangular region X0, initialize it as the center point (x, y);

[0123] Initialize the expansion number i = 1;

[0124] Set the single expansion step length as Δs;

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

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

[0127] If the expanded rectangular area X i collides with the merged occupied grid Rmap, then

[0128] lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], i.e. retract the current expansion;

[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 application, the function FunD(x io , y io ,Rmap, lengthC) is executed after running the FunC function, according to the obtained expansion increment, using the stable way of gradually expanding in four directions, to further expand the driving corridor area, and to ensure that the driving corridor is fully expanded in each direction; the input list of the FunD function is the vehicle position (x, y), the merged occupied grid Rmap for collision detection, and the four-direction expansion increment lengthC; the output is the final expansion increment lengthD in four directions, and the specific implementation steps are as follows:

[0136] First step, preparation: first define the rectangular four expansion directions 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 as the expansion length vector lengthC obtained by running the FunC function, and let the single expansion step length be Δs;

[0137] Second step, extend the rectangle in each direction according to the defined expansion direction direction: traverse the set of expandable directions Y one by one, increase the length in the current direction i by a unit step Δs, i.e. lengthi += Δs, to obtain a new rectangular region, check whether the extended rectangular region collides with the merged occupied grid Rmap and the expansion length Length i whether it exceeds the maximum expansion limit L limit ; if collision occurs or the limit is exceeded: reduce the expansion length in direction i by Length i -= Δs, i.e. back to this expansion; and remove direction i from the set of expandable directions Y, so that this direction will not be expanded in the future;

[0138] Third step, end condition: repeat the expansion process of the second step until the set Y is empty, i.e. there is no expandable direction, and return the final expansion length lengthD;

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

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

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

[0142] Output: lengthD

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

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

[0145] Initialize the four direction expansion increment lengthD = lengthC, and define the single expansion step size as Δs;

[0146] While Y ⊄∅,do

[0147] for each i ∈ Y,do

[0148] LengthDi ← LengthDi +Δs, extend step size Δs in i direction;

[0149] if (the extended rectangular region 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 set Y, after which no expansion in this direction is performed;

[0152] End if

[0153] End for

[0154] End while

[0155] Return lengthD.

[0156] Compared with the traditional scheme, the scheme provided by the application has the following beneficial effects:

[0157] 1. Short time consumption for obstacle avoidance: the driving corridor construction process is optimized, so that the number of collision detection times in the construction process is reduced; the occupancy grid map is optimized, so that the number of collision detection objects is reduced; thereby the time consumption of collision detection is reduced, and the real-time performance of the trajectory planning system is improved;

[0158] 2. Stable time consumption for obstacle avoidance: even in the scene where there are many obstacles in the environment and the complexity is high, the time consumption of collision detection will not be significantly increased, and the stability of the trajectory planning system is improved. BRIEF DESCRIPTION OF DRAWINGS

[0159] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description only some embodiments of the application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

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

[0161] Figure 2 It is a schematic diagram of a driving corridor constructed by a traditional method;

[0162] Figure 3 It is a grid map constructed by executing function FunA;

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

[0164] Figure 5 It is a schematic diagram of grid optimization;

[0165] Figure 6 It is a schematic diagram of the expansion method of FunC;

[0166] Figure 7 A comparison diagram between the present application and the conventional method is shown. DETAILED DESCRIPTION

[0167] The technical solutions in the embodiments of the present application will be clearly and completely described with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

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

[0169] S1, a gridding map is constructed within a certain range of the vehicle, and the gridding in which an obstacle falls is marked as occupied.

[0170] S2, the occupied gridding map obtained in S1 is processed, the adjacent occupied gridding in each column is merged to obtain new occupied gridding, and all the merged occupied gridding is stored in a container for subsequent collision detection.

[0171] S3, the expansion of the driving corridor is started. Generally, in the initial stage of the construction of the driving corridor, the expandable space is usually large, so the original method of gradually expanding the driving corridor in four directions is abandoned here, and the method of expanding in four directions at the same time is adopted. The driving corridor obtained after each expansion is subjected to collision detection with the merged occupied gridding obtained in S2. If collision occurs or the maximum expansion increment is reached, the current expansion is returned, and the expansion increments in four directions are output and the next step is entered.

[0172] S4, based on the driving corridor expanded in S3, the driving corridor is continuously expanded by adopting the stable method of gradually expanding along the clockwise direction, to ensure that the driving corridor is fully expanded in the upward, rightward, downward and leftward directions. During the expansion process, when collision occurs in a certain direction or the maximum expansion increment is reached, the current expansion is returned, and the expansion in the direction is not performed thereafter. The process is continued until the driving corridor cannot be expanded in the four directions, and the final expansion increments in the four directions are obtained. The final driving corridor region 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 application, the expansion direction is {π / 2, 0, -π / 2, π}, which represents the upward, rightward, downward and leftward directions respectively.

[0175] In formula (8), x , y) is the corridor expansion start position, length 1 ) is the corridor expansion increment in the up direction (π / 2 direction), length 2 ) is the corridor expansion increment in the right direction (0 direction), length 3 ) is the corridor expansion increment in the down direction (-π / 2 direction), length 4 ) is the corridor expansion increment in the left direction (π direction);

[0176] The overall flow is implemented as the following function: Boxs = GenerateCorridor(traj, map), which takes as input a list of vehicle trajectory traj, where N ordered trajectory points are included, each of which contains vehicle position information (x, y), and a map represented by a set of obstacle scatter coordinates. The function returns a set of drivable corridor regions Boxs corresponding to each trajectory point in the trajectory:

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

[0178] Input: traj and map;

[0179] Output: Boxs;

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

[0181] Execute the function FunA(x, y, map) to get its return value, which is the occupancy grid Gmap;

[0182] Execute the function FunB(Gmap) to get its return value, which is the merged occupancy grid Rmap used for collision detection in the corridor construction process;

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

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

[0185] Calculate the center point (x i , y i ) of the corridor expansion based on the trajectory point (x io , y io ) and vehicle parameters

[0186] Execute the function FunC(x io , y io ), Rmap) to get its return value - four direction expansion increment lengthC;

[0187] Execute function FunD(x io , y io , Rmap, lengthC) to get its return value - four direction final expansion increment lengthD;

[0188] According to (x io , y io ) and lengthD, the vehicle's driving corridor area at this position is calculated by formula (8) box= [x min , x max , y min , y max ];

[0189] Boxs.push_back(box);

[0190] End for

[0191] Return Boxs.

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

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

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

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

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

[0197] First, determine the grid map range x min , x max , y min , y max, create an M x M occupancy grid map Gmap and initialize all the grids in the map as unoccupied;

[0198] Second, select the left rear of the current position of the vehicle as the starting point of the map and the right front as the end point, i.e. x_start = x - x min , y_start = y - y min , x_end = x + x max , y_end = y + y max ;

[0199] Third, determine the resolution of the grid map x_resolution = (x max - x min ) / M, y_resolution = (y max - y min ) / M;

[0200] Fourth, 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, filter out the obstacles within the range of the grid map;

[0202] 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:

[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, mark as occupied;

[0207] End if

[0208] End for;

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

[0210] By introducing the grid map, 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 the amount of calculation of collision detection is always fixed as the collision detection of the driving corridor and at most MxM regular rectangular grids, regardless of the number or complexity of the obstacles around the vehicle. In this way, the problem of significant increase in collision detection time due to the increase in the number or complexity of obstacles in a complex environment is effectively avoided.

[0211] Execute function FunB(Gmap)

[0212] The input list of the function FunB(Gmap) is the occupancy grid map Gmap obtained by running FunA, and the output list is the merged occupancy grid map Rmap. The execution of the function FunB(Gmap) is to process the MxM occupancy grid map Gmap generated by the function FunA, merge the adjacent occupied grids in each column to form new occupied grids, and finally store all the occupied grids in Rmap for subsequent collision detection in the driving corridor construction process. Please refer to Figure 4 In Figure 4 Rmap is composed of 5 merged occupancy grids.

[0213] The specific execution process of this step is as follows:

[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 grid merging function Dfs_expand(i, j, Gmap) to get the merged rectangle;

[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 the grid has been merged, if false, it means the grid has not been merged.

[0225] Function Dfs_expand is to start from the current occupied grid, along the Gmap column direction to merge adjacent, unmerged occupied grid; if the grid above is occupied and not merged, continue to merge upwards; if it encounters unoccupied grid or has been merged or there is no grid above, stop the merging process; after stopping the merging, output the rectangle formed by the current merged grid area.

[0226] The specific implementation process is as follows:

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

[0228] Input: current occupied grid row index i, column index j, reference of occupied grid map Gmap

[0229] Output: 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, row_end, the x, y, length, width of the merged grid are calculated by formula (9);

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

[0239] Return rectangle;

[0240] Formula (9) is as follows:

[0241]

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

[0243] One of the great innovations of the present application is that FunB merges adjacent occupied grids in each column to form new occupied grids for collision detection. By this method, instead of performing collision detection between the extended driving corridor and each occupied grid in the grid map, collision detection is performed between the extended driving corridor and a relatively small number of merged occupied grids. This reduces the number of objects that need to be detected and reduces the computational complexity, thereby reducing the time required for collision detection. As shown in FIG. 6, instead of performing collision detection between the extended driving corridor and 14 occupied grids, collision detection is performed between the extended driving corridor and 5 merged occupied grids. It is worth noting that the grids before and after merging are regular rectangles, which means that the computational complexity does not increase due to the merging of the grids. Figure 4

[0244] In addition, further optimization of the occupied grid can be performed, as shown in FIG. 7. On the basis of merging adjacent occupied grids in each column, two merged occupied grids in adjacent columns that have the same height and whose center points are located at the same horizontal position are further merged. In this way, the number of collision detection objects can be further reduced, thereby improving the real-time performance of collision detection. Figure 5 Figure 5 In FIG. 7, the part shown by the red box is a new occupied grid formed by further merging three equal-height occupied grids in adjacent columns.

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

[0246] Execute function FunC(x io , y​io , Rmap) is to expand the driving corridor area by extending the rectangle in four directions simultaneously at the beginning of the driving corridor construction. Please refer to Figure 6 As shown in Figure 6 , by this method, the work that originally takes four times to complete can be completed at once, greatly speeding up the driving corridor construction process.

[0247] The input list of the execution function FunC(x io , y io , Rmap) is the vehicle position (x, y) and the merged occupied grid Rmap for collision detection, and the output is the four-direction expansion increment lengthC; the specific implementation steps of the function are as follows:

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

[0249] Second step, expand the rectangle in four directions simultaneously: each expansion expands the existing rectangular area in four directions simultaneously by Δs, i.e. lengthC = [i*Δs, i*Δs, i*Δs, i*Δs]; define the expanded rectangle as X i Perform collision detection with the merged occupied grid RMAP obtained by running the previous function: if a collision occurs, this expansion is invalid, return to this expansion, i.e. lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], and exit the function, returning the current expansion length length; if no collision occurs, consider this expansion valid and increment the expansion times i by 1;

[0250] Third step, end condition: repeat the expansion process of the second step until the cumulative expansion increment is greater than the upper limit of the expansion increment, i.e. i*Δs≥L limit , end the expansion, and return the final expansion length lengthC; the specific execution process of the function implementation is as follows:

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

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

[0253] Output: expanded length vector lengthC

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

[0255] Define the current rectangular area X0, initialized as the center point (x, y);

[0256] Initialize the expansion times i = 1;

[0257] Set the single expansion step size as Δ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 expanded rectangular area X i collides with the merged occupied grid Rmap, then

[0261] lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], i.e., retract this expansion;

[0262] Return lengthC;

[0263] Else

[0264] i ← i + 1;

[0265] End if

[0266] End while

[0267] Return lengthC.

[0268] Four, execute the function FunD(x io , y io ,Rmap, lengthC)

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

[0270] First step, preparation: first define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, build 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 as the expansion length vector lengthC obtained by running the FunC function, and let the single expansion step length be Δs;

[0271] Second step, expand the rectangle according to the defined expansion direction direction: traverse the expandable direction set Y one by one, increase the length on the current direction i by a unit step Δs, that is, lengthi += Δs, get the new rectangular area, check whether the expanded rectangular area collides with the merged occupied grid Rmap and the expansion length Length i Whether it exceeds the maximum expansion limit L limit ; If collision occurs or the limit is exceeded: reduce the expansion length on the direction i by Length i -= Δs, that is, return this expansion; And remove direction i from the expandable direction set Y, and no longer expand in this direction;

[0272] Third step, end condition: repeat the expansion process of the second step until the set Y is empty, that is, there is no expandable direction, and return the final expansion length lengthD;

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

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

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

[0276] Output: lengthD

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

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

[0279] Initialize the expansion increment lengthD = lengthC of the four directions, 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 size Δ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 extension 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] The FunC and FunD functions construct the local corridor area where the vehicle can finally travel at a given position (x, y). Compared to the traditional method of expanding the corridor in each direction one by one, this invention proposes a flexible corridor construction scheme. The core of this scheme is to dynamically adjust the expansion strategy according to the expandable space of the corridor. The innovation lies in: in the initial stage of corridor construction, when the expandable space is large, the four directions are expanded simultaneously; if a collision occurs during the expansion process, the current expansion is rolled back and the safer solution of expanding in each direction is used instead to ensure that all directions can be fully expanded; this method can significantly speed up the corridor construction process in the initial stage. See Figure 7 ,from Figure 7 It can be clearly seen that, compared with the traditional solution, the solution adopted by the present invention speeds up the construction process of the driving corridor without affecting the size of the finally constructed driving corridor.

[0291] The flexible driving corridor construction method adopted by the present application mainly accelerates the obstacle avoidance constraint modeling process by combining grid processing. The final obstacle avoidance constraint form is the same as formula 6, and the finally constructed trajectory planning problem model is shown as formula 7. After successfully constructing the model of the trajectory planning problem (i.e. formula 7), it should be solved. A numerical optimization method is used to solve the dynamic optimization problem, for example, an interior point algorithm or a sequential quadratic programming algorithm. Since there are mature constraint optimization problem solving schemes on the market, any one can be used, for example, an interior point algorithm, which will not be described in the present application.

[0292] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A high-performance intelligent vehicle trajectory planning method based on flexible gridding, characterized in that, The method comprises the following steps: S1, constructing a grid map within a certain range of the vehicle, and marking the grid where the obstacle falls into as occupied; S2, processing the occupied grid map obtained in S1, merging the 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, starting to expand the driving corridor, in the initial stage of driving corridor construction, expanding to the four directions at the same time, performing collision detection on the driving corridor obtained after each expansion and the merged occupied grids obtained in S2, and if collision occurs or the maximum expansion increment is reached, returning to the current expansion, outputting the expansion increments in the four directions, and entering the next step; S4, based on the driving corridor expanded in S3, continuing to expand the corridor by gradually expanding in the clockwise direction, and ensuring that the four directions of upward, rightward, downward and leftward are fully expanded; In the expansion process, when collision occurs in a certain direction or the maximum expansion increment is reached, the current expansion is returned, and expansion in the direction is not performed thereafter, and the process continues until the four directions cannot be expanded, and the final expansion increments in the four directions are obtained, and the final driving corridor area is calculated according to formula (8): In Equation (8), x , y is a corridor expansion start position, length 1 is an upward expansion increment (π / 2 direction), length 2 is a right side expansion increment (0 direction), length 3 is a downward expansion increment (-π / 2 direction), length 4 is a left side expansion increment (π direction); The overall flow is executed as the following function: Boxs = GenerateCorridor(traj, map), the input list of which is a vehicle trajectory traj containing N ordered trajectory points, each trajectory point containing vehicle position information (x, y), and map represents a map composed of obstacle scatter coordinates, and the return value of the function is a set of drivable driving corridor areas Boxs corresponding to each trajectory point in the trajectory: Function Boxs = GenerateCorridor(traj, map); Input: traj and map; Output: Boxs; Take out the first point (x, y) in the trajectory; Execute function FunA(x, y, map) to obtain the return value occupied grid map Gmap; Execute function FunB(Gmap) to obtain the return value Rmap composed of merged occupied grids, which is used for collision detection in the corridor construction process; For each i in {1, 2, …, N}, do Extract the i-th trajectory point coordinate (x i , y i ) from the traj; According to the trajectory point (x i , y i ) and the vehicle parameter, the center point (x io , y io ) of the corridor expansion is calculated The function FunC(x io , y io , Rmap) is executed to obtain its return value - four directional spread increments lengthC; Execute function FunD(x io , y io , Rmap, lengthC) to obtain its return value - the final spread increment lengthD in four directions; According to (x io , y io ) and lengthD, the driving corridor area of the vehicle at this position is calculated by formula (8) as box =[aabb _ x _min , aabb_x_ max , aabb_y_ min , aabb_y_ max ]. Boxs.push_back(box); End for Return Boxs; Wherein, 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 MxM occupied grid map Gmap generated by function FunA, merges the adjacent occupied grids in each column to form new occupied grids, and finally stores all the occupied grids in Rmap for subsequent collision detection in the driving corridor construction process, and 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 grid merging function, Dfs_expand(i, j, Gmap), to get the merged rectangle; 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 false, it means that the grid has not been merged; The function Dfs_expand is to start from the current occupied grid, and merge adjacent unmerged occupied grids along the column direction of Gmap upwards. If the grid above is occupied and unmerged, continue to merge upwards. If an unoccupied grid or a merged grid or no grid above is encountered, stop the merging process. After stopping the merging, output the rectangle formed by the currently merged grid area. The specific execution process is as follows: Function rectangle = Dfs_expand(i, j, &Gmap) Input: current occupied grid row index i, column index j, reference of occupied grid map Gmap Output: 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, row_end, calculate x, y, length, width after merging the grid according to formula (9); rectangle = [x, y, length, width]; Return rectangle; Formula (9) is as follows: in( x , y ) is the center coordinate of the merged grid area, length is the length, width is the width, x_resolution, y_resolution are the grid resolutions. 2.The flexible-grid based high-performance intelligent vehicle trajectory planning method according to claim 1, wherein, The execution function FunA(x, y, map) is to construct a grid map within a certain range of the vehicle, and mark the grid where the obstacle in the environment falls as occupied. The input list of FunA is the vehicle position (x, y) and the map map composed of obstacle scatter points, and the output is the occupancy grid map Gmap; the specific implementation steps are as follows: First, determine the range of the grid map x min , x max , y min , y max , create an occupancy grid map Gmap of size MxM, and initialize all grids in the map as unoccupied; Second, select the left rear of the current position of the vehicle as the starting point of the map, and the right front as the end point, that is, x_start = x - x min , y_start = y - y min , x_end = x + x max , y_end = y + y max ; Third step, determine the grid map resolution x_resolution = (x max - x min ) / M, y_resolution = (y max - y min ) / M; Fourth, traverse the obstacles in the map; Fifth, return the processed occupancy grid map Gmap. 3.The flexible-grid based high-performance intelligent vehicle trajectory planning method according to claim 2, characterized in that, The fourth step of traversing the obstacles in the map includes: First, filter out the obstacles within the range of the grid map; 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, mark as occupied; End if End for. 4.The flexible-grid based high-performance intelligent vehicle trajectory planning method according to claim 1, wherein, The execution function FunC(x io , y io , Rmap) is to expand the driving corridor area by the method of expanding to the four sides of the rectangle at the beginning of the driving corridor construction; the input list of the execution function FunC(x io , y io , Rmap) is the vehicle position (x, y) and the combined 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: The first step is preparation: first define the four expansion directions of the rectangle direction = {π / 2, 0, -π / 2, π}, and initialize the four direction corresponding expansion length vector lengthC = [0, 0, 0, 0], initialize the current rectangular region X0 as the current center point (x, y), initialize the expansion times i as 1, and the single expansion step length as Δs; Second step, expand the rectangle in four directions at the same time: each expansion will expand the existing rectangular area in four directions at the same time by Δs, that is, lengthC = [i*Δs, i*Δs, i*Δs, i*Δs]; define the expanded rectangle as X i Collision detection with the merged occupied grid RMAP obtained by running the last function: if a collision occurs, this expansion is invalid, return to this expansion, that is, lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], and exit the function and return the current expansion length length; if no collision occurs, consider this expansion valid and increase the expansion number i by 1; Third step, ending condition: repeat the expansion process of the second step until the cumulative expansion increment is greater than the upper limit of the expansion increment, that is, i*Δs≥L limit , ending expansion, and returning the final expansion length lengthC; the specific execution process of the function implementation is as follows: Function lengthC = FunC(x, y, Rmap) Input: center point (x, y), merged occupied grid Rmap Output: expanded length vector lengthC Initialize the expansion length vector lengthC = [0, 0, 0, 0]; Define the current rectangular region X0, initialized as the center point (x, y); Initialize the expansion times i = 1; Set the single expansion step length as Δs; While i*Δs < L limit , do Expand Δs along the four directions simultaneously, i.e. lengthC = [i*Δs, i*Δs, i*Δs, i*Δs]; If the extended rectangle region X i collides with the merged occupied grid Rmap, then lengthC = [(i-1)*Δs, (i-1)*Δs, (i-1)*Δs, (i-1)*Δs], i.e. return to this expansion; Return lengthC; Else i ← i + 1; End if End while Return lengthC. 5.The flexible-grid based high-performance intelligent vehicle trajectory planning method according to claim 1, wherein, The execution function FunD(x io , y io ,Rmap, lengthC) is to further expand the driving corridor area according to the obtained expansion increment in a stable way of step-by-step expansion in four directions after running the FunC function, and to ensure that the driving corridor is fully expanded in each direction; the input list of the FunD function is the vehicle position (x, y), the combined 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, and the specific implementation steps are as follows: First, the preparation work: first define the four extended direction of the rectangle direction = {π / 2, 0, -π / 2, π}, build the expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction; And initialize the four direction corresponding to the expansion length vector lengthD is the expansion length vector lengthC obtained by running the FunC function, let the single expansion step length is Δs; Second, extend the rectangle in each direction according to the defined extension direction direction: traverse the set of extendable directions Y one by one, increase the length in the current direction i by a unit step Δs, i.e. lengthi += Δs, to obtain a new rectangular region, and check whether the extended rectangular region collides with the merged occupied grid Rmap and the extension length Length i whether the maximum extension limit L is exceeded limit ; if a collision occurs or the limit is exceeded: reduce the extension length in the direction i by Length i -= Δs, i.e. retract this extension; and remove the direction i from the set of extendable directions Y, so that the direction is no longer extended in the future; Third, the end condition: repeat the expansion process of the second step until the set Y is empty, that is, there is no expandable direction, 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 expansion length increment lengthC Output: lengthD Define the expansion direction set direction = {π / 2, 0, -π / 2, π} (clockwise direction); Build the expandable direction set Y = {1, 2, 3, 4} corresponding to the four directions in direction; Initialize the four direction expansion increment lengthD = lengthC, define the single expansion step length as Δs; While Y ⊄∅,do for each i ∈ Y, do LengthDi ← LengthDi +Δs, expand step length Δs in i direction; if (the expanded rectangular region collides with the merged occupied grid RMAP)∪(lengthi> Llimit), then LengthDi ← LengthDi -Δs, return the expansion in this direction; Remove i from the expandable direction set Y, and no longer expand in this direction; End if End for End while Return lengthD.

Images