Safe welt cleaning path planning method and storage medium

Abstract

The invention discloses a safe welt cleaning path planning method and a storage medium. The method comprises the following steps: S1, obtaining an initial optimized path result through a path planning algorithm; s2, carrying out collision detection on the initial optimized path, judging whether collision exists or not, if not, determining that the path is effective and returning to the step S3, and otherwise, carrying out the step S3; s3, through a collision detection method, obtaining a collision depth dcollision; s4, boundary adjustment is carried out on the basis of the collision depth dCollision; s5, performing path optimization based on the adjusted boundary; and S6, judging whether the optimized path is collided or not, if not, determining that the path is effective and returning to the step S1, and otherwise, returning to the step S1. By means of the method, the automatic driving vehicle can smoothly generate the welt sweeping path, meanwhile, it is guaranteed that collision cannot occur in any topological map, and the method is suitable for path planning algorithms under different coordinate systems.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a safe edge cleaning path planning method and a storage medium. Background Art

[0002] As an important part of future transportation, the development of related technologies has also been gradually valued. There are many complex scenarios in autonomous driving that need to be overcome. The main research scenarios for current autonomous driving path planning are urban roads or highways. In these scenarios, the planned path mainly runs along the middle of the road, the path is far from the curb, and the risk of collision between the planned path and the curb is low. For outdoor road sweepers, the operating scenario is to clean the road close to the curb, and the quality requirements for the path are higher in terms of collision-free. Because the sweeper must clean close to the edge and the cleaning path must not hit the curb is one of the most challenging scenarios at present, especially along the edge in irregular places such as broken lines or large curvature right turns.

[0003] Safely cleaning paths along the edge is a recognized problem in the autonomous driving industry. Path planning in this scenario has the following challenges:

[0004] (1) The path along the edge needs to be cleaned as close to the edge as possible. However, the topological shape of the curb varies in real scenes. It is impossible to clean the path completely along the edge when considering the kinematics of the vehicle body.

[0005] (2) During the edge-to-edge path, neither the vehicle body nor the sweeper can hit the curb.

[0006] (3) For the path planning algorithm based on the Frenet coordinate system, the optimized results have collision analysis. In particular, based on the front axle, the planned path will not collide on the front axle side, but the vehicle body may collide.

[0007] (4) The current path planning algorithm based on the Cartesian coordinate system has relatively high requirements on the quality of the initial path, for example, it must be collision-free. Summary of the invention

[0008] In order to solve the above technical problems, the technical solution of the present invention provides a safe edge cleaning path planning method based on an iterative optimization strategy, which includes the following steps: S1: obtaining an initial optimized path result through a path planning algorithm; S2: performing collision detection on the initial optimized path to determine whether there is a collision. If there is no collision, the path is valid and returns, otherwise proceed to step S3; S3: obtaining a collision depth d through a collision detection method. collision ; S4: Based on the collision depth d collisionAdjust the boundary; S5: optimize the path based on the adjusted boundary; S6: determine whether the optimized path has a collision. If there is no collision, the path is valid and returns, otherwise return to step S1.

[0009] Furthermore, in step S1, an initial path optimization result is obtained by a path planning algorithm based on the Frenet coordinate system; boundary information of the current optimized path is obtained; obstacle points and boundary points are projected onto a reference line, and the lateral position projected onto the reference line is obtained.

[0010] Further, in step S3, obtaining the collision depth includes: when the curb topology on the cleaning side is relatively smooth or gentle, using the collision detection method in the Frenet coordinate system to obtain the collision depth; when the curb topology on the cleaning side is not smooth or has a large curvature, using the collision detection method OBB in the Cartesian coordinate system, with a certain step length step length The path point of the collision collision Translation, the direction of translation is the opposite direction of the collision side. After translating n times to find the non-collision posture, the collision depth is calculated:

[0011] d collision =n*step length .

[0012] Further, in step S4, the boundary is adjusted to:

[0013] lb iter =lb current +d collision

[0014] Among them, lb current is the lower boundary of the current path; lb iter is the adjusted lower boundary.

[0015] Furthermore, in step S1, the path planning algorithm based on the Frenet coordinate system adopts the EM Planner algorithm.

[0016] Furthermore, in step S1, an initial path optimization result path is obtained by a path planning algorithm based on a Cartesian coordinate system. current ; Get the iteration boundary information x of the current path lb <x k <x ub ,y lb <y k <y ub , x k ,y k is the path point coordinate, x lb is the lower boundary of x, x ub is the upper boundary of x, ylb is the lower boundary of y, y ub is the upper boundary of y; obtain obstacle information and path current The square iteration range pk for each point pk box .

[0017] Further, in step S3, obtaining the collision depth includes: using the collision detection method OBB in the Cartesian coordinate system to detect the collision path point point with a certain step length steplength collision Translation, the direction of translation is from the obstacle center to the path point dir obs-to-path-point , after translating n times to find a non-collision posture, the collision depth is calculated:

[0018] d collision =n*step length .

[0019] Further, in step S4, the boundary is adjusted to:

[0020] x lb-iter =x lb -d collision *sin(dir obs-to-path-point )

[0021] x ub-iter =x ub -d collision *sin(dir obs-to-path-point )

[0022] y lb-iter =y lb +d collision *cos(dir obs-to-path-point )

[0023] y ub-iter =y ub +d collision *cos(dir obs-to-path-point )

[0024] Among them, x lb-iter is the adjusted lower boundary of x, x ub-iter is the adjusted upper boundary of x, y lb-iter is the adjusted y lower boundary, y ub-iter is the adjusted upper y boundary.

[0025] Furthermore, in step S1, the path planning algorithm based on the Cartesian coordinate system adopts the DL-IAPS algorithm.

[0026] The technical solution of the present invention also provides a computer-readable storage medium containing a computer program, which is used to execute the method as described above when the computer program is executed by one or more processors.

[0027] The technical solution of the present invention enables an ADV (autonomous driving vehicle) to smoothly generate an edge-cleaning path while ensuring that there will be no collision in maps of any topology and is applicable to path planning algorithms under different coordinates. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0029] Figure 1 It is a schematic diagram of the iterative optimization strategy of the Frenet coordinate system path planning algorithm of the present invention;

[0030] Figure 2 It is a schematic diagram of the iterative optimization strategy of the Cartesian coordinate system path planning algorithm of the present invention. DETAILED DESCRIPTION

[0031] The preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0032] The technical solution of the present invention only considers static obstacles and curb boundaries, and does not consider dynamic obstacles. In order to generate a path that meets the conditions, it is necessary to continuously perform collision detection on the path. After obtaining the collision depth, the boundary is shrunk on one side of the collision, and the size of the shrinkage is. The direction of the shrinkage is the direction of the other side of the collision. For iteration, the key points are to be able to detect the collision and to obtain the depth of the collision, that is, the maximum length of the overlapping area.

[0033] Therefore, it is important to choose a suitable collision detection method. The commonly used collision detection methods are as follows:

[0034] 1. Collision detection in Cartesian coordinate system:

[0035] 1.1 Collision detection method 1:

[0036] The vehicle and the obstacle are equivalent to circumscribed circles. The distance between the two circles' centers minus the radius of the two circles can be used to determine whether there is a collision. When the following conditions are met, there is a collision:

[0037] distance<=R ego +R obs

[0038] Among them, R ego is the radius of the circumcircle of the vehicle, R obs is the radius of the circumscribed circle of the obstacle, and distance is the distance between the two centers of the circles.

[0039] The advantage of this method is that it has fast calculation speed and can also know the collision depth, but the accuracy is too low.

[0040] 1.2 Collision detection method 2:

[0041] Axis Aligned Bounding Box: This method extracts the maximum and minimum values ​​of the x / y axis of each bounding box, and turns the collision problem into a line segment coincidence detection problem on a one-dimensional axis. If the x-axis or y-axis does not intersect, there is no collision. This detection method is more accurate than the previous method, but the accuracy is still not enough and the collision depth is unknown.

[0042] 1.3 Collision detection method three:

[0043] OBB directional bounding box collision detection, through the separating axis theorem. If the projections of two OBBs on all axes overlap, they intersect, otherwise they do not intersect.

[0044] Therefore, in order to know whether two OBBs overlap, it is only necessary to find an axis that makes the projections of A and B not overlap. This method can accurately detect collisions but does not know the collision depth.

[0045] 2. Collision detection method in Frenet coordinate system:

[0046] Collision detection is performed by comparing the lateral distance of each point projected on the reference line. For example, when cleaning to the right, if the projection of the cleaning point on the reference line is l clean Or the projection of a point on the side of the vehicle on the reference line l edge If it is smaller than the projection of the curb point, it is a collision. The formula is as follows:

[0047] l curb > edge +d buffer

[0048] Among them, d buffer is the error margin of the curb obstacle,

[0049] The depth of the collision is:

[0050] d collision = l curb -(l edge +d buffer )

[0051] where d collision :Collision Depth

[0052] l edge : Transverse position of the projection of the vehicle body point along the side onto the reference line

[0053] l curb : The lateral position of the projection of the curb point on the reference line

[0054] 3. Iterative optimization strategy based on Frenet coordinate system path planning algorithm:

[0055] Path planning algorithms based on the Frenet coordinate system (such as the EM Planner algorithm), Em is a mass point model, and the optimized result can generally only ensure that the result at a certain point of the vehicle body will not collide. For the path planned by the rear axle, the two sides of the vehicle body projected at this path point will not collide, but the left and right edges of the front end of the vehicle may hit the curb. If the sweeper is set in front of the vehicle body, there is a risk of hitting the sweeper. On the contrary, if based on the front axle optimization, the sweeper of the optimized path will not hit the curb, and the possibility of vehicle collision will increase. For this, it is necessary to perform collision detection on the optimized path, and then calculate the corresponding collision depth, and adjust the optimization boundary according to the collision depth. See Figure 1 , the specific steps are as follows:

[0056] (1) Get the current path optimization result path current (obtained based on the Frenet coordinate system path planning algorithm);

[0057] (2) Obtain the boundary information bound for optimizing the current path lower and bound upper ;

[0058] (3) The obstacle point and the curb point (boundary point) are projected onto the reference line to obtain their lateral distance projected on the reference line. obs or curb ;

[0059] (4) Path first Perform collision detection on each path point of (initialized path). If there is no collision, the path is valid and returns. (For most scenarios, such as straight lines or relatively gentle curves, the first optimized path already meets the requirements, after all, the optimization constraints take into account obstacle constraints). If a collision occurs, continue to execute the following steps:

[0060] a. When the curb topology on the cleaning side is relatively smooth or flat (for example, a certain threshold can be set and the smoothness or curvature of the curb topology can be compared with the threshold), the collision detection method in 2 above is used (the collision detection method in 1.3 is a detection method in the Cartesian coordinate system, which can also be applied to the path planned based on the Frenet coordinate system here) to obtain the collision depth d collison ;

[0061] b. When the curb topology on the cleaning side is not smooth or has a large curvature: Use the collision detection method (OBB) in 1.3 above. Since the collision depth is unknown, only a certain step length can be used. length The path point of the collision collision Translation, the direction of translation is the opposite direction of the collision side. When the non-collision posture is found after translation n times, the collision depth can be calculated.

[0062] d collision =n*step length

[0063] (5) Boundary Adjustment

[0064] lb iter =lb current +d collision

[0065] Among them, lb:lowerbound lower boundary lb iter : The lower boundary adjusted after iteration;

[0066] (6) Optimizing based on the adjusted boundaries;

[0067] (7) If there is no collision, return; otherwise, output the path.

[0068] 4. Iterative optimization strategy based on Cartesian coordinate system path planning algorithm:

[0069] In Cartesian coordinate system path planning algorithms such as the DL-IAPS algorithm, after each iterative smoothing, the result will be checked for collision with all obstacles. If a collision occurs, the collision point will be re-smoothed by reducing the rectangular box until the collision constraint is satisfied.

[0070] However, after a collision, Figure 2 As shown, the iteration process is performed again in the small rectangular box. If there is an obstacle in the small rectangular box, the efficiency of the iteration is greatly reduced. If the initial path point Pk(xk,yk) is translated away from the obstacle, Pk_shift(xk,yk) is obtained so that the translated box no longer overlaps with the obstacle, the optimization efficiency is greatly improved.

[0071] The specific steps are as follows:

[0072] (1) Get the current path optimization result path current ;

[0073] (2) Obtain the iterative boundary information x for optimizing the current path lb <x k <

[0074] x ub ,y lb <y k <y ub ;

[0075] (3) Obtain obstacle information and path current The square iteration range pk for each point pk box ;

[0076] (4) For each pk box Perform collision detection with obstacles. If there is no collision, the path is valid and returns. (For most scenarios, such as straight lines or relatively gentle curves, the first optimized path already meets the requirements, after all, the optimization constraints take obstacle constraints into account). If a collision occurs, execute the following steps;

[0077] (5) Using the collision detection method in 1.3 above, since the collision depth is unknown, it can only be detected with a certain step length. length The path point for the collision collision Translation, the direction of translation is from the obstacle center to the path point dir obs-to-path-point , when the non-collision posture is found after translation n times, the collision depth can be calculated

[0078] d collision =n*step length

[0079] (6) Adjust the iteration range

[0080] x lb-iter =x lb -d collision *sin(dir obs-to-path-point )

[0081] x ub-iter =x ub -d collision *sin(dir obs-to-path-point )

[0082] y lb-iter =y lb +d collision *cos(dir obs-to-path-point )

[0083] y ub-iter =y ub +d collision *cos(dir obs-to-path-point )

[0084] where x lb-iter The lower bound after iteration, x ub-iter The upper boundary of x and y after iteration lb-iter The lower boundary of y after iteration, y ub-iter The upper y boundary after iteration;

[0085] (7) Optimizing based on the adjusted boundaries;

[0086] (8) If there is no collision, return; otherwise, return to the first step.

[0087] In an embodiment of the present invention, a safe edge cleaning path planning method based on an iterative optimization strategy includes the following steps: S1: obtaining an initial optimized path result through a path planning algorithm; S2: performing a collision detection on the initial optimized path to determine whether there is a collision. If there is no collision, the path is valid and returns, otherwise proceed to step S3; S3: obtaining a collision depth d through a collision detection method. collision ; S4: Based on the collision depth d collision Adjust the boundary; S5: optimize the path based on the adjusted boundary; S6: determine whether the optimized path has a collision. If there is no collision, the path is valid and returns, otherwise return to step S1.

[0088] Furthermore, in step S1, an initial path optimization result is obtained by a path planning algorithm based on the Frenet coordinate system; boundary information of the current optimized path is obtained; obstacle points and boundary points are projected onto a reference line, and the lateral position projected onto the reference line is obtained.

[0089] Further, in step S3, obtaining the collision depth includes: when the curb topology on the cleaning side is relatively smooth or gentle, using the collision detection method in the Frenet coordinate system to obtain the collision depth; when the curb topology on the cleaning side is not smooth or has a large curvature, using the collision detection method OBB in the Cartesian coordinate system, with a certain step length step1 ength The path point of the collision collision Translation, the direction of translation is the opposite direction of the collision side. After translating n times to find the non-collision posture, the collision depth is calculated:

[0090] d collision =n*step length .

[0091] Further, in step s4, the boundary is adjusted to:

[0092] lb iter =lb cuurrent +d collision

[0093] Among them, lb current is the lower boundary of the current path; lb iter is the adjusted lower boundary.

[0094] Furthermore, in step s1, the path planning algorithm based on the Frenet coordinate system adopts the EM Planner algorithm.

[0095] Furthermore, in step S1, an initial path optimization result path is obtained by a path planning algorithm based on a Cartesian coordinate system. current ; Get the iteration boundary information x of the current path lb <x k <x ub , yl b <y k <y ub , x k ,y k is the path point coordinate, x lb is the lower boundary of x, x ub is the upper boundary of x, yl b is the lower boundary of y, yu b is the upper boundary of y; obtain obstacle information and path current The square iteration range pk for each point pk box .

[0096] Further, in step S3, obtaining the collision depth includes: using the collision detection method OBB in the Cartesian coordinate system with a certain step length step length The path point of the collision collision Translation, the direction of translation is from the obstacle center to the path point dir obs-to-path-point , after translating n times to find a non-collision posture, the collision depth is calculated:

[0097] d collision =n*step length .

[0098] Further, in step S4, the boundary is adjusted to:

[0099] x lb-iter =x lb -d collision *sin(dir obs-to-path-point )

[0100] x ub-iter =xub -d collision *sin(dir obs-to-path-point )

[0101] y lb-iter =y lb +d collision *cos(dir obs-to-path-point )

[0102] y ub-iter =y ub +d collision *cos(dir obs-to-path-point )

[0103] Among them, x lb-iter is the adjusted lower boundary of x, x ub-iter is the adjusted upper boundary of x, y lb-iter is the adjusted y lower boundary, y ub-iter is the adjusted upper y boundary.

[0104] Furthermore, in step S1, the path planning algorithm based on the Cartesian coordinate system adopts the DL-IAPS algorithm.

[0105] In another embodiment of the present invention, a computer-readable storage medium containing a computer program is provided. When the computer program is executed by one or more processors, the computer program is used to perform the method as described above.

[0106] The method of the present invention enables an autonomous driving vehicle to smoothly generate an edge-cleaning path while ensuring that there will be no collision in maps of any topology, and is applicable to path planning algorithms under different coordinates.

[0107] The above is only a preferred embodiment of the present invention. It should be pointed out that a person skilled in the art can make several modifications and improvements without departing from the inventive concept of the present invention, which all belong to the protection scope of the present invention.

[0108] References:

[0109] 【1】“Baidu Apollo EM Motion Planner” Haoyang Fan, Fan Zhu, Changchun Liu, Liangliang Zhang, Li Zhuang, Dong Li, Weicheng Zhu, Jiangtao Hu, Hongye Li, Qi Kong

[0110] 【2】Jinyun Zhou 1, Runxin He 1, Yu Wang, Shu Jiang, Zhenguang Zhu, Jiangtao Hu, Jinghao Miao and Qi Luo 2

[0111] "DL-IAPS and PJSO: A Path / Speed ​​Decoupled Trajectory Optimization and its Application in Autonomous Driving".

Claims

1. A safe edge cleaning path planning method based on iterative optimization strategy, characterized in that: The steps include: s1: Obtain the initial optimized path result through the path planning algorithm; S2: Perform collision detection on the initial optimized path to determine whether there is a collision. If there is no collision, the path is valid and returns, otherwise proceed to step s3; S3: Obtain collision depth d through collision detection method collision ; S4: Based on the collision depth d collision Make boundary adjustments; S5: Path optimization based on the adjusted boundary; S6: Determine whether the optimized path has a collision. If not, the path is valid and returns. Otherwise, return to step S1.

2. The method according to claim 1, characterized in that In step S1, The initial path optimization result is obtained by using the path planning algorithm based on the Frenet coordinate system; Get the boundary information of the current optimization path; Project both the obstacle point and the boundary point onto the reference line and obtain the lateral position projected on the reference line.

3. The method according to claim 2, characterized in that In step S3, obtaining the collision depth includes: When the curb topology on the cleaning side is relatively smooth or flat, the collision depth is obtained using the collision detection method in the Frenet coordinate system; When the curb topology on the cleaning side is not smooth or has a large curvature, the collision detection method OBB in the Cartesian coordinate system is used with a certain step size step length The path point of the collision collision Translation, the direction of translation is the opposite direction of the collision side. After translating n times to find the non-collision posture, the collision depth is calculated: d collision =n*step length 。 4. The method according to claim 3, characterized in that In step s4, the boundaries are adjusted to: lb iter =lb current +d collision Among them, lb current is the lower boundary of the current path; lb iter is the adjusted lower boundary.

5. The method according to claim 4, characterized in that In step s1, the path planning algorithm based on the Frenet coordinate system adopts the EM Planner algorithm.

6. The method according to claim 1, characterized in that In step s1, The initial path optimization result path is obtained through the path planning algorithm based on the Cartesian coordinate system current ; Get the iteration boundary information x of the current path lb <x k <x ub ,y lb <y k <y ub , x k ,y k is the path point coordinate, x lb is the lower boundary of x, x ub is the upper boundary of x, y lb is the lower boundary of y, y ub is the upper boundary of y; Obtain obstacle information and path current The square iteration range pk for each point pk box .

7. The method according to claim 6, characterized in that In step s3, obtaining the collision depth includes: Use the collision detection method OBB in the Cartesian coordinate system with a certain step size step length The path point of the collision collision Translation, the direction of translation is from the obstacle center to the path point dir obs-to-path-point , after translating n times to find a non-collision posture, the collision depth is calculated: d collision =n*step length 。 8. The method according to claim 7, characterized in that In step s4, the boundaries are adjusted to: x lb-iter =x lb -d collision *sin(dir obs-to-path-point ) x ub-iter =x ub -d collision *sin(dir obs-to-path-point ) and lb-iter =and lb +d collision *cos(dir obs-to-path-point ) and ub-iter =and ub +d collision *cos(dir obs-to-path-point ) Among them, x lb-iter is the adjusted lower boundary of x, x ub-iter is the adjusted upper boundary of x, y lb-iter is the adjusted y lower boundary, y ub-iter is the adjusted upper y boundary.

9. The method according to claim 8, characterized in that In step s1, the path planning algorithm based on the Cartesian coordinate system adopts the DL-IAPS algorithm.

10. A computer-readable storage medium containing a computer program, characterized in that: When the computer program is executed by one or more processors, it is used to perform the method according to any one of claims 1 to 9.

Images