Robust planning method and system for obstacle avoidance path with unknown obstacle distribution

Abstract

The application provides an unknown obstacle distribution obstacle avoidance path robust planning method and system, combines a minimum passing width which can be safely passed by a vehicle and a known global path to improve a VFH algorithm to determine an optimal passable area, and improves safety and effectiveness of the optimal passable area; a control point is determined between a local obstacle avoidance path target point and an initial point of the vehicle, an optimal planning path described by a segmented Bezier curve by introducing unknown parameters is fitted, the shortest path is realized under the premise of ensuring path smoothness, time required for optimization is small, obstacle avoidance energy consumption is reduced, and real-time performance of path planning is met; a target function is established by establishing a shortest driving path and a minimum direction change amount of the local obstacle avoidance path target point, and unknown parameters are solved by using a local obstacle avoidance path target point direction constraint condition, so that the optimal obstacle avoidance path which is the shortest in length, has the minimum target direction change and meets vehicle kinematics constraints is obtained.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of path planning, and particularly relates to an obstacle avoidance path robust planning method and system for unknown obstacle distribution. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute prior art.

[0003] Unmanned driving technology has been a research hotspot in the field of robots, and has developed rapidly after decades of development and exploration. In recent years, the operation range of mobile robots has expanded from indoors to outdoors, from closed areas to semi-closed or even completely open areas, and more and more practical technologies have been applied to low-speed vehicles. In recent years, people have conducted a lot of research on dynamic path planning of unmanned vehicles, and traditional planning algorithms can be divided into four categories: graph search-based, sampling-based, interpolation curve, and reaction-based algorithms. Intelligent bionic algorithms include genetic algorithm, particle swarm optimization algorithm, etc.

[0004] The path planning method based on graph search converts the planning problem into a graph search problem, divides the solution space into grids, establishes a graph, and then searches and solves the optimal path from the starting point to the target point. Commonly used in graph search-based methods are A* and D* algorithms. When the complexity of the environment is large, the efficiency of this algorithm cannot be guaranteed.

[0005] The sampling-based algorithm randomly samples the fixed workspace to generate a suboptimal path. Rapidly-exploring Random Tree (RRT) and Probabilistic Roadmap (PRM) methods are commonly used algorithms in motion planning. These algorithms search for the optimal path by randomly selecting scattered points in the entire space, but there are some problems, such as high cost, poor real-time performance, and the planned path may not be the optimal path.

[0006] Intelligent algorithms simulate the biological evolution and insect foraging behavior in nature, mainly including genetic algorithm (Genetic Algorithm, GA), particle swarm optimization (Particle Swarm Optimization, PSO) algorithm, etc. Genetic algorithm can be combined with various iterative algorithms more conveniently, but it has problems such as prematureness, low convergence path quality, poor population diversity, and difficulty in breaking local optimal solution. Although the traditional particle swarm optimization algorithm has a faster convergence speed, it also has some problems, such as the possibility of falling into a local optimal solution and premature convergence, which need to be solved.

[0007] The above three methods are mostly used for global path planning, but can also be combined with other methods to complete local obstacle avoidance.

[0008] Reaction algorithms are usually used as local path planners, including Artificial Potential Field (APF) and Vector Field Histogram (VFH) algorithms. Such algorithms can quickly process information, which is usually from limited on-board sensors. As a local planner, the algorithm usually plans the next immediate path or maneuver direction to avoid nearby obstacles while following the global plan made by another algorithm. The above algorithms have the characteristics of faster and higher robustness, but the generated path is not smooth.

[0009] Continuous geometric curve interpolation methods generate smooth paths, which can be used for local path generation or global path smoothing. Geometric curves include polynomial curves, B-spline curves, Bezier curves, etc. Polynomial curves can calculate the coefficients of the polynomial curve that meets the constraints by giving the expected value or constraints in the given start and end segments. In robot trajectory planning, this method is widely used to generate smooth trajectories and meet various motion constraints of the robot during motion. B-spline curves are commonly used curves in computer-aided design applications, which have more characteristics suitable for navigation. In recent years, they have been widely used in path planning of wheeled robots. Bézier curve converts the calculation of curve shape into the solution of coordinate points, and its curve is smooth and easy to calculate, which is widely used in path planning. By optimizing the position of the control point, the curvature upper limit constraint is met.

[0010] The above path planning methods ensure the smoothness and continuity of the planned path, but lack in algorithm speed and robustness. SUMMARY

[0011] To overcome the shortcomings of the prior art, the present application provides a robust obstacle avoidance path planning method and system for unknown obstacle distribution. The method ensures the executability of the obstacle avoidance trajectory, has smaller trajectory length and better trajectory stability, ensures the safety of the obstacle avoidance trajectory, and has smaller planning cost time.

[0012] To achieve the above purpose, the first aspect of the present application provides a robust obstacle avoidance path planning method for unknown obstacle distribution, comprising:

[0013] Step 1: Obtain the environmental information of the vehicle, combine the minimum passing width that the vehicle can safely pass through and the known global path, and use the VFH algorithm to determine the optimal passable area;

[0014] Step 2: determining a local obstacle avoidance path target point according to the determined optimal passable region;

[0015] Step 3: determining a control point between the determined local obstacle avoidance path target point and an initial point of the vehicle, fitting a segmented Bezier curve by introducing unknown parameters;

[0016] Step 4: establishing a target function in the shortest travel path and the minimum change in direction of the local obstacle avoidance path target point, solving the unknown parameters under the direction constraint of the local obstacle avoidance path target point, and obtaining an optimal obstacle avoidance path.

[0017] The second aspect of the application provides an obstacle avoidance path robust planning system for unknown obstacle distribution, comprising:

[0018] An optimal passable region determination module: obtaining environmental information of a vehicle, combining a minimum passable width that the vehicle can safely pass through and a known global path to determine an optimal passable region by using a VFH algorithm;

[0019] A target point determination module: determining a local obstacle avoidance path target point according to the determined optimal passable region;

[0020] A fitting curve module: determining a control point between the determined local obstacle avoidance path target point and an initial point of the vehicle, fitting a segmented Bezier curve by introducing unknown parameters;

[0021] An optimal path determination module: establishing a target function in the shortest travel path and the minimum change in direction of the local obstacle avoidance path target point, solving the unknown parameters under the direction constraint of the local obstacle avoidance path target point, and obtaining an optimal obstacle avoidance path

[0022] The third aspect of the application provides a computer device, comprising a processor, a memory and a bus, the memory stores machine readable instructions executable by the processor, when the computer device is running, the processor and the memory communicate through the bus, and the machine readable instructions are executed by the processor to perform an obstacle avoidance path robust planning method for unknown obstacle distribution.

[0023] The fourth aspect of the application provides a computer readable storage medium, the computer readable storage medium stores a computer program, and the computer program is executed by the processor to perform an obstacle avoidance path robust planning method for unknown obstacle distribution.

[0024] The above one or more technical solutions have the following beneficial effects:

[0025] In the application, the VFH algorithm is improved to determine the optimal passable region in combination with the minimum passing width of the vehicle and the known global path, so that the safety and effectiveness of the optimal passable region are improved; the control point is determined between the initial point of the vehicle and the local obstacle avoidance path target point, the optimal planning path described by the segmented Bezier curve by introducing unknown parameters is introduced, the shortest path is realized under the premise of ensuring the smoothness of the path, the required time for optimization is small, the obstacle avoidance energy consumption is reduced, and the real-time performance of the path planning is met; the unknown parameters are solved by establishing the target function of the shortest driving path and the minimum change of the direction of the local obstacle avoidance path target point and the direction constraint condition of the local obstacle avoidance path target point, so that the optimal obstacle avoidance path with the shortest length and the minimum change of the target direction is obtained, and the kinematic constraint of the vehicle is met.

[0026] Advantages of the additional aspects of the application will be partially given in the following description, partially will become obvious from the following description, or will be understood by the practice of the application. BRIEF DESCRIPTION OF DRAWINGS

[0027] The drawings accompanying the specification of this application form a part thereof, serve to provide further understanding of the application, and together with the description of the exemplary embodiments of the application and the explanation thereof serve to explain the application, and do not constitute an improper limitation of the application.

[0028] Figure 1 It is an obstacle avoidance process schematic diagram in the embodiment one of the application;

[0029] Figure 2 It is a functional block diagram of obstacle avoidance path planning in the embodiment one of the application;

[0030] Figure 3 It is a robust path planning algorithm schematic diagram in the embodiment one of the application;

[0031] Figure 4 It is an irregular boundary constraint schematic diagram in the embodiment one of the application;

[0032] Figure 5 It is a vector field histogram in the embodiment one of the application;

[0033] Figure 6 It is a vector field binarization histogram in the embodiment one of the application;

[0034] Figure 7 It is a vehicle minimum passing width schematic diagram in the embodiment one of the application;

[0035] Fig. 8 (a) is a passable region schematic diagram under the condition that the optimal inner passable region and the global path have no intersection and both sides have obstacles in the embodiment one of the application;

[0036] Figure 8(b) is a schematic diagram of the passable area in Embodiment 1 of the present invention when there is an obstacle on one side and the optimal passable area does not intersect with the global path.

[0037] Figure 8(c) is another schematic diagram of the passable area in Embodiment 1 of the present invention when there is an obstacle on one side and the optimal passable area does not intersect with the global path.

[0038] Figure 9 This is a schematic diagram of the passable area in Embodiment 1 of the present invention, where the optimal passable area intersects with the global path.

[0039] Figure 10 This is a schematic diagram of a piecewise quadratic Bézier curve in Embodiment 1 of the present invention;

[0040] Figure 11 This is a flowchart of the local path planning in Embodiment 1 of the present invention;

[0041] Figure 12 This is the obstacle avoidance experiment scenario in Embodiment 1 of the present invention;

[0042] Figure 13 This is a schematic diagram of the obstacle avoidance trajectory in Embodiment 1 of the present invention;

[0043] Figure 14 This is a cost-time comparison chart of obstacle avoidance planning in Embodiment 1 of the present invention;

[0044] Figure 15 This is a comparison diagram of obstacle avoidance trajectory lengths in Embodiment 1 of the present invention;

[0045] Figure 16 This is a comparison chart of the root mean square error of the heading angle variation in the obstacle avoidance path in Embodiment 1 of the present invention;

[0046] Figure 17(a) shows a scene of uneven road edges in the park in Embodiment 1 of the present invention;

[0047] Figure 17(b) shows a scene in Embodiment 1 of the present invention where the park is transitioned from a spacious environment to a narrow one;

[0048] Figure 17(c) shows a scene in Embodiment 1 of the present invention where a narrow environment is entered into a spacious environment within the park;

[0049] Figure 17(d) shows a scene of an "S"-shaped road in the park in Embodiment 1 of the present invention;

[0050] Figure 18(a) shows the obstacle avoidance scenario encountered by the unmanned vehicle during global path tracking in Embodiment 1 of the present invention;

[0051] Figure 18(b) is a real-time running display diagram of an unmanned vehicle encountering obstacles during global path tracking in a real vehicle experiment in Embodiment 1 of the present invention.

[0052] Fig. 19(a) is a real-time running display diagram of the algorithm when the unmanned vehicle returns to the global path field by bypassing the obstacle in the embodiment one of the present application;

[0053] Fig. 19(b) is a real-time running display diagram of the algorithm when the unmanned vehicle returns to the global path by bypassing the obstacle in the embodiment one of the present application; DETAILED DESCRIPTION

[0054] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the present application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0055] It should be noted that the terms used herein are only for the purpose of describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application.

[0056] In the case of no conflict, the embodiments in the present application and the features in the embodiments can be combined with each other.

[0057] Embodiment one

[0058] This embodiment studies the obstacle avoidance path planning of a vehicle driving on a known global path encountering unknown obstacles in a park. The driving environment in the park can be a bounded structural road such as a one-way lane or a multi-lane, and a non-structural environment such as a square, a dispatch station, and a road boundary unknown. The path planning algorithm of the present application aims to make the unmanned vehicle safely, quickly and smoothly avoid obstacles and return to the global path under the premise of complying with the known environmental constraints.

[0059] The embodiment discloses a robust obstacle avoidance path planning method for unknown obstacle distribution, comprising:

[0060] Step 1: Obtain the environmental information of the vehicle, combine the minimum passing width that the vehicle can safely pass through and the known global path, and determine the optimal passable area by using the VFH algorithm;

[0061] Step 2: Determine the local obstacle avoidance path target point according to the determined optimal passable area;

[0062] Step 3: Determine the control point between the determined local obstacle avoidance path target point and the initial point of the vehicle, and fit the segmented Bezier curve by introducing unknown parameters;

[0063] Step 4: Establish a target function with the shortest driving path and the smallest change in the direction of the local obstacle avoidance path target point, and solve the unknown parameters under the direction constraint condition of the local obstacle avoidance path target point to obtain the optimal obstacle avoidance path.

[0064] As Figure 1As shown, the entire obstacle avoidance process can be divided into three stages according to the position relationship between the unmanned vehicle and the global path: global path deviation, obstacle avoidance, and global path regression. As shown in Figure 2 As shown, the method of the embodiment mainly consists of three parts, that is, determination of the optimal passable region, path construction based on the segmented quadratic Bezier curve, and robust planning of the path.

[0065] For the obstacle avoidance problem of semi-structured roads in the park, firstly, the feasible direction of obstacle avoidance needs to be determined under the condition of known obstacle information and environmental information, such as Figure 3 As shown, that is, the intermediate gray area; then the target point P4 of the local obstacle avoidance path and its gradient constraint are determined, and the determination method of the control points P1, P3 and the segmentation point P2 of the segmented quadratic Bezier curve is studied in combination with the starting point P0 and its gradient constraint, so as to realize the construction of the Bezier curve of the local obstacle avoidance path; under the constraints of the vehicle running curvature and the target point direction, the Bezier curve parameters are optimized, so that the obstacle avoidance path is the shortest and the target point direction changes the least.

[0066] The embodiment studies the path robust planning of the unmanned vehicle in the park with unknown obstacle distribution. The driving scene in the park is relatively complex, and the road boundary is fuzzy and extremely irregular. Therefore, in the process of determining the passable region, not only the perceived obstacle information needs to be considered, but also the boundary constraint of the lane edge and the unstructured scene.

[0067] The road boundary of the structured road in the park cannot be determined by the sensor, so the coordinate points can be collected along the lane edge, the boundary line of the lane is established by using the curve fitting method, and then the electronic map of the lane information is constructed.

[0068] Similarly, for the unstructured scene, the boundary of this kind of scene is constructed by collecting the boundary points of the scene. In the embodiment, the road edge collection points within the detection range of the vehicle are set as virtual obstacles, and the virtual obstacles and the currently perceived obstacle information are processed in the same way. As shown in Figure 4 As shown, the two side lines are the boundary of the current driving scene, the line passing through the black block is the global reference path that has been planned, the line where the vehicle is located is the obstacle avoidance route of the unmanned vehicle, the black block is the obstacle, and the black boundary is the road boundary within the detection range. max d is the maximum environmental perception distance of the vehicle.

[0069] In step 1 of the embodiment, the candidate passable region screening strategy of the traditional VFH algorithm is improved in combination with the minimum passing width of the vehicle; at the same time, by modifying the candidate direction loss function, the traditional VFH series algorithm is improved from end-point traction to global path traction, so as to obtain the optimal passable region. The improved VFH algorithm can solve the obstacle avoidance problem of the complex environment in the park when the global path is known.

[0070] The VFH algorithm divides the vehicle's surroundings into a grid, samples in real time using onboard sensors, continuously updates the two-dimensional grid, and defines a certain area around the intelligent vehicle as an active window for analysis. Each grid cell is considered an active unit, and the active window is then divided into sectors to calculate the obstacle strength value mn for each sector. The obstacle strength of each sector is represented as:

[0071] m n =c v 2 (a-bd n 2 (1)

[0072] Where, d n c is the distance from the obstacle to the driverless car. v Unlike traditional ultrasonic sensors, which require probabilistic assessment of the confidence level of the obstacle grid within a cone-shaped area, LiDAR can accurately return distance measurements in a specific direction. Therefore, when the obstacle grid is covered by obstacles, c is a probability function. v The value is a fixed value, the magnitude of which can be determined depending on the situation and has no special requirements; d is the distance from the active unit to the autonomous vehicle; a and b are both constants, and a and b satisfy... d max This represents the maximum detection range of the lidar. The resulting polar coordinate histogram is shown below. Figure 5 As shown.

[0073] From formula (1), we can know that each distance value d n Each obstacle has a corresponding obstacle strength value. A suitable distance is set as the distance threshold d. th Binarize the vector field histogram using the corresponding obstacle intensity value to obtain the passable sector histogram.

[0074] The histogram of traversable sectors is the foundation and prerequisite for direction selection, and the construction of the traversable sector histogram and the threshold d are crucial. th The value of d is closely related to the threshold value. th The selection of the threshold is crucial for the VFH series algorithms. Taking into account the hardware characteristics, motion characteristics, and target environment of the autonomous vehicle, a maximum distance threshold is set. and minimum distance threshold interval With a certain step size Δd th Cutting the data yields a set of distance thresholds, represented as: The set of obstacle strength thresholds can then be expressed as: An element of the obstacle strength threshold set can be represented as The binarization rule is shown in the following formula:

[0075]

[0076] Where, m n This represents the obstacle strength value. Obstacle strength threshold.

[0077] Based on the above binarization rules, a binarized histogram can be constructed under the current threshold, such as... Figure 6 As shown, the obstacle strength threshold is based on the current distance threshold. The autonomous vehicle environment is divided into safe sectors and hazardous sectors. The extent of each sector is as follows: Figure 6 The horizontal axis is defined by angles, and passable areas are filtered within the safety sector. To determine the safety of passable areas, this implementation determines the angle threshold based on a threshold distance. The angular range of the passable area is given. If the difference between the upper and lower limits of the angular range is greater than... Then it is determined as a candidate passable area.

[0078] threshold Obtained from the following formula:

[0079]

[0080] Among them, w min The minimum passage width required to ensure safe passage for vehicles is calculated using the following formula:

[0081] w min =w p +r min (4)

[0082]

[0083] w p =w+2rmin(1-cosθ) max (6)

[0084] like Figure 7 As shown, r min It is the vehicle's minimum turning radius, w p It is the narrowest driving lane width for vehicles, l w The wheelbase of the vehicle; l f This refers to the front overhang length; l k The center distance is the main sales center distance, w is the overall vehicle width, and θ is the center distance. max This is the maximum steering angle of the steering wheel.

[0085] The candidate feasible direction within a candidate passable area is the direction of the centerline of the passable area, as shown in the following formula:

[0086]

[0087] The obtained candidate direction cij Substitute the following formula to get the current obstacle intensity threshold The optimal feasible direction is:

[0088]

[0089] The traditional VFH algorithm uses the end point as the traction of the motion direction. In order to adapt to the complex routes in the park such as circular routes, curves, etc., the improved VFH algorithm uses the global path as the traction of the motion direction. Formula (8) is the loss function of the improved candidate direction, and Δθ i is the included angle between the candidate direction c ij and the tangent direction of the nearest global path point. is the included angle between the candidate direction c ij and the current motion direction of the unmanned vehicle; Δφ i represents the included angle between the candidate direction c ij and the last motion direction; μ1>0, μ2>0, μ3>0, μ1≥μ2+μ3.

[0090] Suppose the obstacle intensity threshold set has n elements, then the candidate passable direction has n, each represents a passable area. Using the cost function formula (9), the minimum value is obtained, which corresponds to and is recorded as the optimal passable direction c * and the optimal distance threshold The purpose is to obtain a safe distance threshold and a passable direction close to the global path direction. According to c * Combined with formula (7), the optimal passable area angle upper and lower limits are obtained will be used to determine the target point of the local obstacle avoidance path in step 2 of the present embodiment. The cost formula is as follows:

[0091]

[0092] where η (η>0) is a threshold weight coefficient, is the included angle between the candidate direction c and the direction of the end point.

[0093] In step 2 of the present embodiment, a piecewise quadratic Bezier curve is used to describe the local obstacle avoidance path. According to different obstacle avoidance stages, the determination method of the curve control point and the segmentation point is given. In addition, the calculation amount of the higher order Bezier curve is large, which cannot meet the real-time requirements of the algorithm. On the other hand, in order to make the planned local path the shortest, the present embodiment takes the shortest path length as the optimization target, and the length of the high-order Bezier curve is difficult to accurately solve.

[0094] Based on the optimal passable area determined in step 1 like Figure 7 As shown, to ensure the safety of trajectory planning, the endpoints of obstacles at the edge of the optimal passable area are further expanded. The expansion radius is the minimum turning radius of the vehicle, resulting in a safer optimal passable area, which is called the optimal inner passable area.

[0095] The optimal traversable area angle range, depending on the external environment, can be expressed as: For vectors direction, For vectors direction, Let P0 be the point of tangency of the tangent line drawn from point P0 to the left endpoint of the expansion circle of the obstacle on the right side of the vehicle. Point P0 is the tangent point to the tangent line drawn from the right end of the expansion circle of the obstacle on the left side of the vehicle.

[0096] It should be noted that, for ease of description, the "left" and "right" mentioned above refer to left and right in the direction of vehicle travel.

[0097] This embodiment determines the target state in two cases: Case 1: The optimal passable area and the global path do not intersect; Case 2: The optimal passable area and the global path intersect.

[0098] Specifically, for case 1, as shown in Figure 8(a), the global path does not intersect with the optimal accessible area, and the angle range of the accessible area is... Then the local path target point and The point with the smallest global path distance is the local path target point, that is:

[0099]

[0100] Where, d l d r They are respectively The shortest distance to the global path.

[0101] Figures 8(b) and 8(c) show the cases where the global path and the inner passable area do not intersect when there is an obstacle on one side of the optimal inner passable area. The angle range of the inner passable area is... P * The optimal feasible direction c * The distance between the vehicle and the above is At this point, the local path target point is taken and P * The point with the shortest path distance is the local path target point, that is:

[0102]

[0103] Where, d l d * They are respectively P * The shortest distance to the global path.

[0104] In case 1, the direction at the local path target point is the direction of vector P4-P0, which can be represented as:

[0105]

[0106] For case 2, such as Figure 9 As shown, if there are global pathpoints within the passable area, the detection distance of the passable area is the maximum detection distance d. max P4 is the intersection of the farthest boundary of the passable area and the global path. P4 is also called the regression point.

[0107] When the local path target point is taken as the global path point, its direction is the tangent direction of that point. Let the tangent direction vector of that point be G. tan G tan The angle between the vector P4-P0 and the vector P4-P0 is θ. tan ,but:

[0108]

[0109] In step 3 of this embodiment, the local obstacle avoidance path described by the piecewise Bézier curve is as follows: Figure 10 As shown, P0 is the initial point of the first quadratic Bézier curve, P4 is the ending point of the second quadratic Bézier curve, P1 and P3 are the control points of the two Bézier curves respectively, and P2 is the segmentation point of the two curves. By moving the control points, a safe and comfortable obstacle avoidance path curve can be obtained.

[0110] This path can be described as:

[0111] Bz1(t)=(1-t 2 P0+2t(1-t)P1+t 2 P2, 0 ≤ t ≤ 1

[0112] Bz2(t)=(1-t 2 P2+2t(1-t)P3+t 2 P4, 0≤t≤1 (14)

[0113] Where Bz1 and Bz2 correspond to the first and second quadratic Bézier curves respectively, P0 is the initial point of the first quadratic Bézier curve, P4 is the ending point of the second quadratic Bézier curve, P1 and P3 are the control points of the two Bézier curves respectively, and P2 is the segmentation point of the two curves.

[0114] Specifically, the position constraint of the initial state can be simply satisfied by setting the first control point P0, which is the coordinate point of the vehicle-mounted laser radar in the global coordinate system. G1 is the current unmanned vehicle heading unit vector. When performing path planning, it is generally required that the tangent direction of the starting point of the Bezier curve is the same as the starting point heading of the unmanned vehicle, and the tangent direction of the terminal point is the same as the terminal point heading. Let the distance between P0 and P1 be l, and the coordinates of the first control point can be obtained as:

[0115] P1 = [P 01 + G 11 l, P 02 + G 12 l] T (15)

[0116] The target point P4 and its direction G4 have been obtained in step 2 above. According to the future running direction given by the relative relationship between the obstacle and the vehicle, it does not necessarily satisfy all the constraints during the vehicle's travel. Therefore, to improve the success rate of trajectory planning, two parameters α and β are introduced to fine-tune G4. The specific method is as follows:

[0117]

[0118] The solution method of P3 is as follows:

[0119]

[0120] By adjusting α and β, the direction and length of the vector P4~P3 can be adjusted, increasing the degree of freedom of the terminal direction, improving the success rate of path planning, and enhancing the robustness of the planning algorithm.

[0121] To ensure the smoothness of the piecewise quadratic Bezier curve, P2 is taken as a point on the line connecting P1 and P3, that is:

[0122]

[0123] where G1 = [G 11 , G 12 ] T is the current unmanned vehicle heading unit vector, G 11 and G 12 are the horizontal and vertical coordinates of G1, respectively, P 01 and P 02 are the horizontal and vertical coordinates of P0, respectively; P 11 and P 12 are the horizontal and vertical coordinates of P1, respectively; P 31 and P 32 are the horizontal and vertical coordinates of P3, respectively; P 41 and P 42 are the horizontal and vertical coordinates of P4, respectively; For

[0124] The local obstacle avoidance path described based on the segmented Bezier curve is continuous and smooth, and is known,

[0125] Bz1(t) = (1-t) 2 P0+2t(1-t)P1+t 2 P2,0≤t≤1

[0126] Bz2(t) = (1-t) 2 P2+2t(1-t)P3+t 2 P4,0≤t≤1

[0127] Therefore,

[0128]

[0129] Because,

[0130] Bz'1(t) = 2(t-1) 2 P0+2(1-2t)P1+2tP2,0≤t≤1

[0131] Bz'1(t) = 2(t-1) 2 P2+2(1-2t)P3+2tP4,0≤t≤1

[0132] Therefore,

[0133]

[0134] As shown above, the above method constructs a segmented quadratic Bezier curve that is continuous and smooth.

[0135] In step 4 of the embodiment, a target function of the shortest path and the minimum change of the target direction is constructed, and the target point direction variable margin constraint and the curvature constraint are given, and the sequence quadratic programming (SQP) algorithm is used to solve the optimal path parameters.

[0136] For the target point direction variable margin constraint, in order to improve the robustness of the path planning, introduce α, β to make the original target direction have a certain variable margin, and the constraint is as follows:

[0137]

[0138] When ,

[0139]

[0140] When ,

[0141]

[0142] Simplify to get the target direction variable margin constraint:

[0143]

[0144] Wherein, α, β are fine-tuning parameters, δ is variable margin constraint.

[0145] It should be noted that, in order to avoid division by zero, in practical application, let G 21 = G 21 + ε, ε is a very small value.

[0146] For curvature bounded constraint, the curvature formula of any point on the curve is:

[0147]

[0148] A new upper limit value of the curve curvature can be obtained by inequality scaling, and the scaling process is as follows:

[0149]

[0150] From equation (24), the denominator of the rightmost formula is a quadratic function of t, and its minimum value can be expressed as: D(l, α, β) > 0, the numerator is a constant with respect to the optimization parameters l, α, β, denoted as F(l, α, β), so the upper limit of ||k|| is:

[0151]

[0152] Further, the curvature upper limit constraint can be obtained, that is:

[0153]

[0154] Where, k max is the curvature upper limit, r min is the minimum turning radius.

[0155] To solve the unknown parameters in the Bezier curve, the objective function is given, which not only requires the shortest length, but also requires the minimum change of the target direction of the path, as shown in equation (27), and under the corresponding constraint condition, the curve parameters describing the path are optimized:

[0156]

[0157] The optimization objective is a function of parameters l, α, β, δ as variables, and ω is the weight parameter. The main purpose of this objective function is to encourage the steering operation and driving risk of the autonomous vehicle to be minimized. Equation (28) gives the target optimization problem with constraints, that is:

[0158] minimize: J(l, a, b, d)

[0159]

[0160] l > 0

[0161] a > 0

[0162] b > 0

[0163]

[0164] 0 < d < 1 (28)

[0165] The embodiment adopts a sequence quadratic programming (SQP) algorithm to solve the optimal path parameter solving problem, and finally obtains an optimal obstacle avoidance path with the shortest length, the smallest target direction change, and satisfying the kinematic constraint of the vehicle.

[0166] The unmanned vehicle detects the external environment in real time during driving, and when it is detected that the front of the global or local obstacle avoidance path currently driven is impassable, local path planning is performed. The embodiment proposes a path monitoring strategy for the path planning algorithm, which determines whether the obstacle avoidance path needs to be re-planned according to the currently detected known environment. The strategy greatly saves the time cost of calculating the overall obstacle avoidance process while ensuring the safety of the obstacle avoidance process.

[0167] The path planning monitoring strategy is as shown in Figure 11 , including a planning mechanism and a safety detection mechanism. In order to ensure the safety of the local path, it is necessary to ensure that the local path is always within the passable area. Since the passable area calculated in each control period is centered on the vehicle, even if the obstacles do not change, the optimal passable area angle range will also change as the vehicle drives. Therefore, by monitoring whether the path is always in the optimal passable area in real time, the safety of the path cannot be accurately determined. In summary, the embodiment monitors the distance change amount Ad T and the angle change amount Aq T of the local path target point calculated at each moment and the planned path target point in real time, sets the angle change threshold q T and the distance change threshold d T , and if the distance change amount and the angle change amount are both greater than the corresponding threshold, the path is re-planned, as shown in the following formula:

[0168]

[0169]

[0170]

[0171] P TiFor each control cycle, a path planning target point is generated. Whether a new path needs to be optimized depends on the degree of change in the distance and angle between the previous target point and the current target point. When the change exceeds a set threshold, it indicates that the passable area has changed, and the current local path is no longer suitable. In this case, a new local path is planned and generated.

[0172] Starting from the same point and with different types of obstacle scenarios, the obstacle avoidance performance of the proposed robust path planning algorithm, the polynomial programming algorithm based on the Frenet coordinate system, and the VFH algorithm for autonomous vehicle motion planning problems were compared. The success rate, cost, time, path length, and heading angle stability of the three algorithms were compared across 10 scenarios. Furthermore, an irregularly structured scenario was constructed to verify the proposed algorithm. Finally, a real-vehicle experiment was conducted on a campus to verify the algorithm.

[0173] The computer used in the simulation experiment had an Intel Core i5-5200U CPU and 8GB of memory. The algorithm was run on MATLAB 2018 b under Windows 10 (64-bit) system. The specific parameters of the three planning algorithms are shown in Table 1 below:

[0174] Table 1. Trajectory Planning Algorithm Parameter Settings

[0175]

[0176]

[0177] 1. Comparison of planning success rates

[0178] To verify the obstacle avoidance performance of the proposed algorithm in complex environments, a simulation experiment was conducted. The experiment used a starting point of (0, 20) and a target point of (90, 40), and included 10 different obstacle scenarios, such as... Figure 12 As shown.

[0179] Table 2 shows the success rates of obstacle avoidance planning using three different algorithms.

[0180] Table 2 Success Rate of Trajectory Planning Algorithm

[0181]

[0182] like Figure 13 As shown, four scenarios are selected to demonstrate the effect of the planned trajectory. The method proposed in this embodiment is represented by the blue trajectory, the autonomous driving trajectory planning algorithm based on the Frenet coordinate system is represented by the pink trajectory, and the VFH algorithm for autonomous vehicle motion planning problem is represented by the red trajectory.

[0183] The unmanned trajectory planning algorithm based on the Frenet coordinate system generates a trajectory with the global path as a reference, and the generated trajectory is constrained by the global path. In the face of complex environments, the obstacle avoidance planning performs poorly, and is successful in 7 of 10 scenarios. The VFH algorithm for unmanned vehicle motion planning problem adopts a "double threshold" strategy based on perception data, which to some extent alleviates the VFH threshold sensitivity problem. However, when faced with more complex obstacle scenarios, the fixed threshold cannot adapt to all obstacle avoidance scenarios, so there will be planning failures. In scenario 4, the planning fails, and is successful 9 times. Due to the freedom setting of the target point in the method proposed in the application, and the use of a candidate passable region screening method based on adaptive threshold and vehicle narrowest passage constraint, the selection and collection of passable directions are more flexible, and the planning success rate is higher in the face of more complex external environment. All 10 scenarios successfully avoid obstacles.

[0184] 2. Time cost comparison

[0185] In 10 different obstacle environment scenarios, the path planning algorithm proposed in the embodiment, the Frenet-based polynomial curve planning algorithm, and the improved VFH algorithm, the path planning cost time comparison of the same start and end points is as shown in the following table. Figure 14 As shown in the following table, the cost time is the overall time spent on path planning from the start point to the end point, and the unit is seconds. Figure 13 As shown in the following table, the algorithm proposed in the embodiment has the smallest time cost compared with the other two algorithms.

[0186] The VHF algorithm for unmanned vehicles generates trajectory points according to the optimal direction in each control period with a certain step size, and finally obtains an obstacle avoidance trajectory, so it needs to perform the entire planning process in each control period. Similarly, the Frenet-based polynomial planning algorithm replans the obstacle avoidance trajectory in each control period. Therefore, the cost time of these two algorithms is relatively long. The path planning algorithm proposed in the application proposes a trajectory monitoring decision, which replans the next trajectory only when it is determined that the last detected trajectory is unsafe. Therefore, the cost time of the algorithm proposed in the application is relatively small.

[0187] 3. Path length comparison

[0188] The path lengths of the path planning algorithm, the polynomial planning algorithm based on Frenet, and the improved VFH algorithm in 10 scenarios under the same conditions of start and end points and obstacle environment are counted. The path points obtained by the polynomial planning algorithm based on Frenet and the improved VFH algorithm are path points determined at intervals of a control period vehicle speed. The simulation environment of the application is low-speed uniform speed driving, so the planned path is a path point with fixed intervals. The algorithm proposed in the application is not real-time planning, so a segmented quadratic Bezier curve is generated for uniform speed interpolation to finally obtain the same path point fixed interval obstacle avoidance path as the other two algorithms, and the fixed control period is 0.3 s and the vehicle speed is 10 km / h. Therefore, the broken line method can be uniformly used to calculate the path length for comparison.

[0189] The results are as follows Figure 15 As shown in the figure, the path length of the path robust planning algorithm proposed in the application is the shortest compared with the other two algorithms. Since the optimization target of the algorithm proposed in the application is the minimum trajectory when generating the trajectory, the curvature constraint is set according to the minimum turning radius, so the path is the shortest while ensuring the feasibility of the path.

[0190] 4. Path heading angle comparison

[0191] From the start point to the obstacle avoidance end point, the obstacle avoidance path planning is completed, the obstacle avoidance path can be discretized according to a certain time scale, the heading angle of each path point is obtained, and the stability and smoothness of the whole obstacle avoidance path are reflected by the mean square deviation of the heading angle of the whole path. The following figure shows the mean square deviation of the trajectory heading angle of the obstacle avoidance planning of the three algorithms in 10 scenarios, as shown in the figure Figure 16 As shown in the figure, the trajectory heading angle change mean square deviation and amplitude of the algorithm proposed in the application are the smallest in 4 scenarios of 7 scenarios in which the three algorithms are successfully planned. Therefore, the trajectory planned by the algorithm proposed in the application is more stable.

[0192] 5. Simulation experiment of complex boundary scenario

[0193] The application simulates the typical scenario of an unmanned vehicle in a park, and simulates the running of the path robust planning algorithm proposed in the application. The obstacle avoidance process is as shown in the figure Figures 17(a)-17(d) As shown in the figure, the green curve is the known global path, and the red curve is the edge of the current driving scenario. The data of the two is obtained from the constructed electronic map information, the blue curve is the driving trajectory of the unmanned vehicle, and the black curve represents the obstacle.

[0194] Fig. 17(a) is a scene of rough road edge in the park, Fig. 17(b) is a scene of entering narrow environment from wide environment in the park, Fig. 17(c) is a scene of entering wide environment from narrow environment in the park, and Fig. 17(d) is a scene of "S" type road in the park. As shown in the four typical scenes of the park, the unmanned vehicle successfully avoids the obstacles and reaches the destination. The experiment proves that the algorithm proposed in the application can adapt to various complex boundary constraints in the park and has high robustness.

[0195] 6. Real vehicle experiment

[0196] The real vehicle experiment scene is a campus square, and the path points are collected around a giant flower bed to generate a global path for real vehicle test. The real vehicle test uses an Ackerman steering platform JD-01, a vehicle-mounted Huadai GNSS receiver for vehicle positioning and orientation, and a SICK 16 line laser radar for obstacle information detection.

[0197] Firstly, the path points are collected around the flower bed, and a global path is obtained by using a cubic polynomial fitting, which is a green curve in the figure. The blue trajectory in the figure is the driving trajectory of the unmanned vehicle, and the pink curve is the local path planned by the unmanned vehicle during driving. The effect of the unmanned vehicle driving around the flower bed is shown in Figs. 17 and 18.

[0198] In Fig. 18(a), the unmanned vehicle encounters an obstacle avoidance scene during tracking the global path. In Fig. 18(b), it is a real-time running display diagram of the algorithm when the unmanned vehicle encounters an obstacle during tracking the global path in the real vehicle experiment. As shown in the figure, the unmanned vehicle successfully plans a local path to avoid the obstacle after perceiving the pedestrian in front.

[0199] In Fig. 19(a), the unmanned vehicle returns to the global path after passing through the obstacle during tracking the global path. In Fig. 19(b), it is a real-time running display diagram of the algorithm when the unmanned vehicle returns to the global path after avoiding the obstacle in the real vehicle experiment. As shown in the figure, the unmanned vehicle successfully plans a local path to return to the global path.

[0200] After the real vehicle test, the algorithm proposed in the application can make the unmanned vehicle successfully avoid the obstacles appearing on the global path.

[0201] Embodiment two

[0202] The purpose of this embodiment is to provide an obstacle avoidance path robust planning system for unknown obstacle distribution, which comprises:

[0203] The optimal passable area determination module: acquires the environment information of the vehicle, combines the minimum passable width of the vehicle and the known global path, and determines the optimal passable area by using the VFH algorithm;

[0204] The target point determination module: determines the local obstacle avoidance path target point according to the determined optimal passable area.

[0205] a fitting curve module: determining a control point between the determined local obstacle avoidance path target point and the initial point of the vehicle, fitting a piecewise Bezier curve by introducing unknown parameters;

[0206] an optimal path determination module: establishing a target function with the shortest travel path and the smallest change in direction of the local obstacle avoidance path target point, and solving the unknown parameters under the direction constraint of the local obstacle avoidance path target point to obtain an optimal obstacle avoidance path.

[0207] Embodiment three

[0208] The purpose of this embodiment is to provide a computing device, including a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor executes the program to implement the steps of the above method.

[0209] Embodiment four

[0210] The purpose of this embodiment is to provide a computer-readable storage medium.

[0211] A computer-readable storage medium having a computer program stored thereon, wherein the program is executed by a processor to perform the steps of the above method.

[0212] The steps and methods involved in the above embodiments two, three and four correspond to embodiment one, and the specific embodiments can be referred to the relevant description part of embodiment one. The term "computer-readable storage medium" should be understood to include a single medium or multiple media of one or more instruction sets; it should also be understood to include any medium capable of storing, encoding or carrying instruction sets for execution by a processor and causing the processor to perform any of the methods in the present application.

[0213] Those skilled in the art should understand that the above modules or steps of the present application can be realized by a general computer device, alternatively, they can be realized by program codes executable by a computing device, so that they can be stored in a storage device for execution by a computing device, or they can be made into individual integrated circuit modules, or a plurality of modules or steps among them can be made into a single integrated circuit module. The present application is not limited to any specific combination of hardware and software.

[0214] The above describes the specific embodiments of the present application in combination with the accompanying drawings, but is not a limitation on the scope of protection of the present application. Those skilled in the art should understand that various modifications or variations made by those skilled in the art on the basis of the technical solutions of the present application without creative labor are still within the scope of protection of the present application.

Claims

1. A method for robust planning of obstacle-avoiding path with unknown obstacle distribution, characterized in that, The method comprises the following steps: Step 1: obtaining environment information of a vehicle, combining a minimum passing width that the vehicle can safely pass through and a known global path to determine an optimal passable region by using a VFH algorithm; In the step 1, specifically comprising: Dividing the environment where the vehicle is located in a grid form, updating the grid in real time according to the information collected by the vehicle-mounted sensor, dividing the grid into sectors, calculating the obstacle intensity of each sector, and determining the passable region; Determining an angle threshold by using one point in the minimum passing width that the vehicle can safely pass through and the vehicle distance threshold set; If the angle range difference of the passable region is greater than the angle threshold, the passable region is a candidate passable region; Determining a candidate direction of the candidate passable region according to the angle range of the candidate passable region; Determining an optimal feasibility direction based on the included angle between the candidate direction and the tangent direction of the nearest known global path point, the included angle between the candidate direction and the current motion direction of the vehicle, and the included angle between the candidate direction and the last motion direction of the vehicle; Determining an optimal passing direction and an optimal distance threshold by using a cost function based on the optimal feasibility direction and the one point in the vehicle distance threshold set; Obtaining the upper and lower limits of the optimal passable region angle based on the optimal passing direction; Step 2: determining a local obstacle avoidance path target point according to the determined optimal passable region; Step 3: determining a control point between the determined local obstacle avoidance path target point and the initial point of the vehicle, and fitting the control point into a segmented Bezier curve by introducing unknown parameters; in the step 3, specifically comprising: Taking the current initial position of the vehicle as the initial point of the first segment of the quadratic Bezier curve, and taking the local obstacle avoidance path target point as the terminal point of the second segment of the quadratic Bezier curve; According to the principle that the tangent direction of the starting point of the Bezier curve is the same as the starting point heading of the vehicle, and the tangent direction of the ending point is the same as the ending point heading of the vehicle, control points on the first segment of the quadratic Bezier curve are determined ; According to the local obstacle avoidance path target point and its direction, introducing unknown parameters to fine-tune the direction of the local obstacle avoidance path target point; According to the fine-tuned local obstacle avoidance path target point direction and the local obstacle avoidance path target point, a second segment quadratic Bezier curve control point is determined ; According to the control points and the control points Based on the smoothness of the curve , The segment points on the line, and then get the segmented Bezier curve Step 4: establishing a target function with the shortest driving path and the smallest change in the direction of the local obstacle avoidance path target point, and solving the unknown parameters under the direction constraint of the local obstacle avoidance path target point to obtain an optimal obstacle avoidance path; in the step 4, the direction constraint of the local obstacle avoidance path target point includes a target point direction variable margin constraint and a curvature bounded constraint, and a sequential quadratic programming is used to solve the target function containing unknown parameters under the target point direction variable margin constraint and the curvature bounded constraint to obtain the optimal obstacle avoidance path.

2. The obstacle avoidance path robust planning method for unknown obstacle distribution of claim 1, wherein, In the step 2, specifically comprising: Determining the range where the obstacle is located by taking the end point of the obstacle detected by the vehicle as the origin and the minimum turning radius of the vehicle as the inflation radius; Determining an optimal inner passable region according to the determined range where the obstacle is located and the optimal passable region; Determining a local obstacle avoidance path target point according to whether there is an intersection between the optimal inner passable region and the known global path.

3. The obstacle avoidance path robust planning method of unknown obstacle distribution of claim 2, wherein, If there is no intersection between the optimal inner passable region and the known global path, and there are obstacles on both left and right sides of the optimal inner passable region, taking the point on the right end of the left obstacle and the left end of the right obstacle to the known global path with the minimum distance as the local obstacle avoidance path target point.

4. The obstacle avoidance path robust planning method of unknown obstacle distribution of claim 2, wherein, If the optimal inner passable region has no intersection with the known global path and the optimal inner passable region has a single-sided obstacle, the end point of the obstacle and the point farthest from the optimal distance threshold in the optimal passable direction are respectively taken as the local obstacle avoidance path target points.

5. The obstacle avoidance path robust planning method for unknown obstacle distribution of claim 2, wherein, If the optimal inner passable region has intersection with the known global path, the intersection point of the boundary of the optimal inner passable region and the global path is taken as the local obstacle avoidance path target point.

6. The obstacle avoidance path robust planning method of unknown obstacle distribution of claim 1, wherein, Further comprising: According to the distance variation and the angle variation of the local obstacle avoidance path target point obtained at each moment and the planned path target point, and the size of the set distance variation threshold and the angle variation threshold, it is judged whether the path needs to be re-planned.

7. A robust planning system for obstacle avoidance paths with unknown obstacle distribution, characterized in that, Comprising: The optimal passable region determination module: obtaining the environmental information of the vehicle, combining the minimum passable width of the vehicle and the known global path, and determining the optimal passable region by using the VFH algorithm; specifically comprising: The environment in which the vehicle is located is divided in the form of a grid, the grid is updated in real time according to the information collected by the vehicle-mounted sensor, the grid is divided into sectors, the obstacle intensity of each sector is calculated, and the passable region is determined; The minimum passable width of the vehicle and one point in the vehicle distance threshold set are used to determine the angle threshold; If the angle range difference of the passable region is greater than the angle threshold, the passable region is a candidate passable region; The candidate direction of the candidate passable region is determined according to the angle range of the candidate passable region; Based on the included angle between the candidate direction and the tangent direction of the nearest known global path point, the included angle between the candidate direction and the current motion direction of the vehicle, and the included angle between the candidate direction and the last motion direction of the vehicle, the optimal feasibility direction is determined; Based on the optimal feasibility direction and one point in the vehicle distance threshold set, the optimal passable direction and the optimal distance threshold are determined by using a cost function; The upper and lower limits of the angle of the optimal passable region are obtained based on the optimal passable direction; The target point determination module: determining the local obstacle avoidance path target point according to the determined optimal passable region; The fitting curve module: determining the control points between the determined local obstacle avoidance path target point and the initial point of the vehicle, and fitting the control points into a segmented Bezier curve by introducing unknown parameters; specifically: The current initial position of the vehicle is taken as the initial point of the first segment of the quadratic Bezier curve, and the local obstacle avoidance path target point is taken as the terminal point of the second segment of the quadratic Bezier curve; According to the principle that the tangent direction of the starting point of the Bezier curve is the same as the starting point heading of the vehicle, and the tangent direction of the ending point is the same as the ending point heading of the vehicle, control points on the first segment of the quadratic Bezier curve are determined ; According to the local obstacle avoidance path target point and its direction, the unknown parameters are introduced to fine-tune the direction of the local obstacle avoidance path target point. According to the fine-tuned local obstacle avoidance path target point direction and the local obstacle avoidance path target point, a second segment quadratic Bezier curve control point is determined ; According to the control points and the control points Based on the smoothness of the curve , The segmented points on the line, and then get the segmented Bezier curve The optimal path determination module: establishing a target function with the shortest driving path and the smallest direction variation of the local obstacle avoidance path target point, solving the unknown parameters under the direction constraint of the local obstacle avoidance path target point, and obtaining the optimal obstacle avoidance path; the direction constraint of the local obstacle avoidance path target point includes the target point direction variable margin constraint and the curvature bounded constraint, and the sequential quadratic programming is used to solve the target function containing the unknown parameters under the target point direction variable margin constraint and the curvature bounded constraint, and the optimal obstacle avoidance path is obtained.

8. A computer device, comprising: Comprising: The processor, the memory, and the bus, the memory stores machine readable instructions executable by the processor, when the computer equipment runs, the processor and the memory communicate through the bus, the machine readable instructions are executed by the processor and execute the unknown obstacle distribution obstacle avoidance path robust planning method of any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and the computer program is executed by the processor and executes the unknown obstacle distribution obstacle avoidance path robust planning method of any one of claims 1 to 6.

Images