A local path obstacle-crossing planning method and device

Abstract

The present invention relates to the field of autonomous driving technology, and provides a method and device for planning a local path overcoming obstacles, the method comprising: using an obstacle bypassing optimization algorithm to obtain a first optimized path of a target route to be planned; determining the position coordinates of obstacles that are allowed to be bypassed on the first optimized path; based on the position coordinates of each obstacle to be bypassed, the first optimized path is corrected to obtain a second optimized path of the target route to be planned. The present invention can prevent an autonomous driving vehicle from running over obstacles, reduce the trouble of the autonomous driving vehicle bypassing obstacles, and improve comfort.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a local path obstacle-crossing planning method and device. Background Art

[0002] An autonomous driving system usually includes perception, decision-making, planning, and control. An autonomous driving vehicle drives along a route planned by the global path. It detects the surrounding environment in real time through the perception system, makes behavioral decisions based on obstacles and drivable areas, and then generates a route to avoid obstacles through local path planning. The vehicle is controlled to drive along the route to achieve safe driving. Local path decision planning plays a vital role in the autonomous driving system.

[0003] Existing local path planning methods for autonomous driving use methods of bypassing obstacles or directly ignoring small obstacles when encountering obstacles. However, when driving a vehicle, real humans will use the method of riding over obstacles within a certain size range, especially for some vehicles with higher chassis, which can be ridden over more often. If obstacles that are suitable for riding over are bypassed and avoided, the risk of changing lanes and the energy loss caused by the obstacle bypassing path will increase. Some small obstacles are directly ignored, and when they are hit, there will be a serious bumpy feeling, affecting the driving and riding experience. Summary of the invention

[0004] The present invention provides a local path obstacle-crossing planning method and device, which are used to solve the defects of the prior art of bypassing obstacles that are suitable for riding over, increasing the risk of bypassing and energy loss, and ignoring small obstacles that lead to poor driving and riding experience, so as to achieve the optimization of the local path and improve the driving and riding experience.

[0005] The present invention provides a local path obstacle-crossing planning method, comprising:

[0006] Using the obstacle avoidance optimization algorithm, the first optimized path of the target route to be planned is obtained;

[0007] Determine the position coordinates of obstacles that are allowed to be crossed on the first optimized path;

[0008] Based on the position coordinates of each obstacle to be crossed, the first optimized path is corrected to obtain the second optimized path of the target route to be planned.

[0009] According to the local path obstacle-crossing planning method provided by the present invention, the step of obtaining the first optimized path of the target route to be planned by using the obstacle-circumventing optimization algorithm specifically includes:

[0010] The length of the target route to be planned at the current moment is intercepted from the global trajectory point to generate a guide line for the target route;

[0011] Mapping the information of the obstacles to be circumvented on the target route into the Frenet coordinate system;

[0012] Discretizing the target route at equal intervals along the guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point;

[0013] The drivable areas of the discrete points are connected by an optimization algorithm to obtain a first optimized path for the target route to be planned.

[0014] According to the local path obstacle planning method provided by the present invention, the step of calculating the drivable area of ​​each discrete point specifically includes:

[0015] Set the road boundary as the initial boundary of each discrete point;

[0016] If there is an obstacle to be circumvented at the location of the current discrete point, the initial boundary is corrected according to the position coordinates of the obstacle to obtain the driving boundary of each discrete point;

[0017] Based on the driving boundary, a drivable area of ​​each discrete point is obtained.

[0018] According to the local path obstacle-crossing planning method provided by the present invention, the step of connecting the drivable areas of the discrete points by an optimization algorithm specifically includes:

[0019] Based on the driving boundaries of each of the discrete points, determining the lateral offset of each of the discrete points and the first-order derivative and the second-order derivative of the lateral offset relative to each of the discrete points;

[0020] Based on the second-order derivative, the third-order derivative between adjacent discrete points is calculated;

[0021] The objective function of the optimization algorithm is set as the lateral offset of the discrete point, the minimum value of the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point, and the constraint conditions are set as the swing amplitude constraint of the center of the vehicle head and the rigid body constraint, and the lateral offset of the discrete point, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point are optimized;

[0022] Based on the optimized lateral offset, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete points, the drivable areas of the discrete points are connected.

[0023] According to the local path obstacle-crossing planning method provided by the present invention, the step of determining the position coordinates of obstacles that are allowed to be crossed on the first optimized path specifically includes:

[0024] Based on the minimum ground clearance and minimum wheel-to-wheel distance of the vehicle, a height threshold and a width threshold of obstacles that are allowed to be crossed are determined;

[0025] Based on the height threshold and the width threshold, the position coordinates of obstacles that are allowed to be crossed on the first optimized path are determined.

[0026] According to the local path obstacle-crossing planning method provided by the present invention, based on the position coordinates of each obstacle to be crossed, the first optimized path is corrected to obtain the second optimized path of the target route to be planned, specifically including:

[0027] Respectively obtain the coordinate points on the guide line that are closest to each obstacle to be crossed;

[0028] The position coordinates of each obstacle to be crossed are used to replace each of the coordinate points to form a corrected guide line;

[0029] Based on the corrected guide line, a second optimized path of the target route to be planned is obtained.

[0030] According to the local path obstacle-crossing planning method provided by the present invention, the step of obtaining the second optimized path of the target route to be planned based on the corrected guide line specifically includes:

[0031] The corrected guide line is smoothed, and the information of the obstacles to be circumvented and the obstacles to be crossed on the target route is mapped into the Frenet coordinate system;

[0032] Discretizing the target route at equal intervals along the corrected guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point;

[0033] The drivable areas of the discrete points are connected by an optimization algorithm to obtain a second optimized path for the target route to be planned.

[0034] The present invention also provides a local path obstacle-crossing planning device, comprising:

[0035] A first path optimization module, used to obtain a first optimized path of a target route to be planned by adopting an obstacle avoidance optimization algorithm;

[0036] A width and height threshold determination module, used to determine the height threshold and width threshold of an obstacle that is allowed to be crossed based on the minimum ground clearance and minimum wheel spacing of the vehicle;

[0037] An obstacle determination module for crossing obstacles, used to determine the position coordinates of obstacles that are allowed to be crossed on the first optimized path based on the height threshold and the width threshold;

[0038] The second path optimization module is used to correct the first optimized path based on the position coordinates of each obstacle to be crossed, so as to obtain a second optimized path of the target route to be planned.

[0039] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of any of the above-mentioned local path obstacle planning methods are implemented.

[0040] The present invention also provides a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the steps of any of the above-mentioned local path obstacle planning methods are implemented.

[0041] The local path obstacle-crossing planning method and device provided by the present invention obtains a first optimized path of the target route to be planned by adopting an obstacle-circumventing optimization algorithm; determines the position coordinates of obstacles that are allowed to be crossed on the first optimized path; and based on the position coordinates of each obstacle to be crossed, corrects the first optimized path to obtain a second optimized path of the target route to be planned. The present invention makes obstacle-crossing decisions and safe and efficient local path obstacle-crossing planning based on the size attributes and positions of obstacles, avoids running over obstacles, reduces the trouble of circumventing obstacles, and improves the driving and riding experience. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0043] Figure 1 It is a flow chart of the local path obstacle-crossing planning method provided by the present invention.

[0044] Figure 2 It is a schematic diagram of the structure of the obstacle bypassing and obstacle crossing provided by the present invention.

[0045] Figure 3 It is a schematic diagram of the result of the local path obstacle-crossing planning provided by the present invention.

[0046] Figure 4 It is a specific flow chart of the local path obstacle-crossing planning method provided by the present invention.

[0047] Figure 5 It is a structural schematic diagram of the local path obstacle-crossing planning device provided by the present invention.

[0048] Figure 6It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0050] The present invention is described in detail below in conjunction with the accompanying drawings of the specification. The specific operating method in the method embodiment can also be applied to the device embodiment or the system embodiment. In the description of the present invention, unless otherwise specified, "at least one" includes one or more. "Multiple" refers to two or more. For example, at least one of A, B and C includes: A exists alone, B exists alone, A and B exist at the same time, A and C exist at the same time, B and C exist at the same time, and A, B and C exist at the same time. In the present invention, " / " means or, for example, A / B can mean A or B; "and / or" in this article is only a kind of association relationship that describes the associated objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone.

[0051] The present invention will be described in detail below in conjunction with specific implementation modes.

[0052] In some specific embodiments of the present invention, Figure 1 As shown, this scheme provides a local path obstacle planning method, including:

[0053] Step 100: Obstacle avoidance optimization algorithm is used to obtain a first optimized path of the target route to be planned;

[0054] Step 200: determining the position coordinates of obstacles that are allowed to be crossed on the first optimized path;

[0055] Step 300: Based on the position coordinates of each obstacle to be traversed, the first optimized path is corrected to obtain a second optimized path for the target route to be planned.

[0056] It should be noted that the existing local path planning scheme does not take into account riding and straddling, which increases the risk of lane changes and the energy loss caused by obstacle avoidance paths. It also directly ignores some small obstacles, causing severe bumps when running over the small obstacles, affecting the driving and riding experience.

[0057] Therefore, the present invention determines obstacles that can be circumvented and obstacles that can be crossed, makes obstacle crossing decisions and plans safe and efficient obstacle crossing paths according to the size attributes and positions of the obstacles, thereby avoiding running over obstacles and reducing the trouble of circumventing obstacles.

[0058] In some possible implementations of the present invention, the step of using an obstacle avoidance optimization algorithm to obtain a first optimized path of the target route to be planned specifically includes:

[0059] The length of the target route to be planned at the current moment is intercepted from the global trajectory point to generate a guide line for the target route;

[0060] Mapping the information of the obstacles to be circumvented on the target route into a Frenet coordinate system (Mercator coordinate system);

[0061] Discretizing the target route at equal intervals along the guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point;

[0062] The drivable areas of the discrete points are connected by an optimization algorithm to obtain a first optimized path for the target route to be planned.

[0063] Specifically, this embodiment provides an implementation method for obtaining a first optimized path using an obstacle avoidance optimization algorithm, by discretizing the target route at equal intervals, calculating the drivable area of ​​each discrete point, and connecting the drivable areas of each discrete point through an optimization algorithm to obtain the first optimized path of the target route to be planned.

[0064] In a possible embodiment, the autonomous driving vehicle determines its own position and the destination position through a positioning system, uses the road information of a navigation map to generate trajectory points of a global path, and drives according to the global trajectory points.

[0065] In a possible embodiment, the autonomous driving vehicle uses perception sensors such as cameras, millimeter-wave radars, lidars, etc. to perceive the environment on the driving path in real time, detect information about obstacles ahead, obtain information such as the location coordinates, width and height, moving speed, and category of the obstacles, and pass it to a functional module that executes local path planning.

[0066] Furthermore, the functional module that performs local path planning intercepts the length of the path to be planned at the current moment from the global trajectory point and uses it as a guide line. The obstacle information that needs to be avoided in the environment is mapped into the Frenet coordinate system, where the Frenet coordinate system is constructed based on the guide line, with the left side as the positive direction and the right side as the negative direction. In order to pass through obstacles more safely, the length and width of the obstacles are expanded, such as adjusting according to needs, for example, adding 0.1m to the left and right sides of the width, and adding 0.2m to the front and back of the height. Discretize the target route along the guide line direction according to the same interval Δs, and calculate the drivable area of ​​each discrete point s.

[0067] In some possible implementations of the present invention, the step of calculating the drivable area of ​​each discrete point specifically includes:

[0068] Set the road boundary as the initial boundary of each discrete point;

[0069] If there is an obstacle to be circumvented at the location of the current discrete point, the initial boundary is corrected according to the position coordinates of the obstacle to obtain the driving boundary of each discrete point;

[0070] Based on the driving boundary, a drivable area of ​​each discrete point is obtained.

[0071] Specifically, this embodiment provides an implementation method for calculating the drivable area of ​​discrete points, by obtaining the position coordinates of the obstacle to be circumvented, correcting the initial boundary according to the position coordinates, and obtaining the drivable boundary of the area where the discrete points are located.

[0072] Specifically, Figure 2 As shown, Figure 2 It is a structural schematic diagram of obstacles for bypassing and crossing obstacles provided by the present invention. The initial boundaries of the discrete point s are the two sides of the road. For the obstacle that needs to be bypassed and avoided at the current discrete point s, if the distance of the leftmost part of the obstacle to the center line of the lane is closer than the distance of the rightmost part to the center line of the lane, then the right boundary of the current point takes the leftmost part of the obstacle, and the left boundary remains unchanged. Otherwise, the left boundary of the current point takes the rightmost part of the obstacle, and the right boundary remains unchanged.

[0073] The above arrangement of this embodiment calculates the obstacle crossing boundary by the left and right boundaries of the obstacle and the distance between the vehicle wheels, thereby effectively constraining the obstacle crossing drivable area.

[0074] In some possible implementations of the present invention, the step of connecting the drivable areas of the discrete points by using an optimization algorithm specifically includes:

[0075] Based on the driving boundaries of each of the discrete points, determining the lateral offset of each of the discrete points and the first-order derivative and the second-order derivative of the lateral offset relative to each of the discrete points;

[0076] Based on the second-order derivative, the third-order derivative between adjacent discrete points is calculated;

[0077] The objective function of the optimization algorithm is set as the lateral offset of the discrete point, the minimum value of the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point, and the constraint conditions are set as the swing amplitude constraint of the center of the vehicle head and the rigid body constraint, and the lateral offset of the discrete point, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point are optimized;

[0078] Based on the optimized lateral offset, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete points, the drivable areas of the discrete points are connected.

[0079] Specifically, this embodiment provides an implementation method for connecting the drivable areas of each discrete point through an optimization algorithm, and controls the centripetal acceleration of the lateral movement by optimizing the lateral offset, first-order derivative, second-order derivative, and third-order derivative of the discrete points to ensure the comfort of the path.

[0080] In a possible embodiment, after obtaining the new boundaries of all discrete points, i.e., local path points, local path planning is performed based on a piecewise acceleration algorithm. Specifically, for each discrete point, the lateral offset l of the point, and the first-order and second-order derivatives l′ and l″ of the lateral offset relative to s are taken as optimization variables. Assume that between every two discrete points, there is a constant third-order derivative term l′″ connecting adjacent discrete points. This third-order derivative term can be calculated by differential means from adjacent second-order derivatives. It is hoped that the lateral offset of each point is as close to the guide line as possible. For the first-order, second-order and third-order derivatives, we hope that they are as small as possible, so that the centripetal acceleration of the lateral motion can be controlled to ensure the comfort of the path. Then consider the swing amplitude constraint of the center of the vehicle head and the rigid body constraint for final optimization.

[0081] Furthermore, the algorithm is used to optimize the optimal path for bypassing large obstacles.

[0082] In some possible implementations of the present invention, the step of determining the position coordinates of the obstacle that is allowed to be crossed on the first optimized path specifically includes:

[0083] Based on the minimum ground clearance and minimum wheel-to-wheel distance of the vehicle, a height threshold and a width threshold of obstacles that are allowed to be crossed are determined;

[0084] Based on the height threshold and the width threshold, the position coordinates of obstacles that are allowed to be crossed on the first optimized path are determined.

[0085] Specifically, this embodiment provides an implementation method for determining obstacles that are allowed to be traversed on the first optimization path, by limiting the size of the obstacles that can be traversed, and locating the obstacles that are allowed to be traversed on the first optimization path according to the size restriction.

[0086] In a possible embodiment, after obtaining the minimum ground clearance and wheel-to-wheel distance of the vehicle, the width and height of obstacles that are allowed to be crossed are set. For example, the width threshold and height threshold can be set as: height < 0.7 times the minimum ground clearance, width < 0.7 times the minimum wheel-to-wheel distance. Obstacles larger than the width and height threshold range and dynamic obstacles are avoided by bypassing them, and static obstacles smaller than the width and height threshold are crossed.

[0087] That is to say, in this embodiment, a method of first planning to bypass large obstacles and then optimizing the obstacle-crossing path is adopted. Some small obstacles have an impact on vehicle driving on the original path, but are not on the obstacle-circumventing path and can be ignored, thereby reducing the processing of such small obstacles.

[0088] In some possible implementations of the present invention, based on the position coordinates of each obstacle to be traversed, the first optimized path is corrected to obtain the second optimized path of the target route to be planned, specifically including:

[0089] Respectively obtain the coordinate points on the guide line that are closest to each obstacle to be crossed;

[0090] The position coordinates of each obstacle to be crossed are used to replace each of the coordinate points to form a corrected guide line;

[0091] Based on the corrected guide line, a second optimized path of the target route to be planned is obtained.

[0092] Specifically, this embodiment provides an implementation method for correcting the first optimized path by acquiring the position coordinates of each obstacle to be crossed and replacing the position coordinates of each obstacle to be crossed with the nearest coordinate point on the guide line.

[0093] In a possible embodiment, the initially planned obstacle-avoiding path is converted back to the original Mercator coordinate system as a new guideline. Then, the coordinate points on the center line of the crossable obstacles in the vehicle passage area near the guideline are replaced with the points on the guideline closest to it, and the replaced guideline is smoothed as the second optimized guideline, and then the second optimized path is obtained based on the guideline.

[0094] In some possible implementations of the present invention, the step of obtaining a second optimized path of the target route to be planned based on the corrected guide line specifically includes:

[0095] The corrected guide line is smoothed, and the information of the obstacles to be circumvented and the obstacles to be crossed on the target route is mapped into the Frenet coordinate system;

[0096] Discretizing the target route at equal intervals along the corrected guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point;

[0097] The drivable areas of the discrete points are connected by an optimization algorithm to obtain a second optimized path for the target route to be planned.

[0098] Specifically, this embodiment provides an implementation method for obtaining a second optimized path, and according to the position coordinates of the obstacle to be circumvented and the obstacle to be crossed, the second optimized path of the target route to be planned is obtained through an optimization algorithm.

[0099] Specifically, see Figure 2 , map the obstacle information in the environment whose size is greater than the width threshold and / or greater than the height threshold to the Frenet coordinate system constructed based on the second optimization guide line. In order to pass the obstacle more safely, the length and width of the obstacle are expanded. Discretize the guide line direction according to the same interval Δs, and calculate the drivable area of ​​each discrete point s. The initial boundaries of the discrete point s are the two sides of the road. For the obstacles that need to be bypassed at the current discrete point s, if the distance from the leftmost side of the obstacle to the center line of the lane is closer than the distance from the rightmost side to the center line of the lane, the right boundary of the current point is the leftmost side of the obstacle, and the left boundary remains unchanged. Otherwise, the left boundary of the current point is the rightmost side of the obstacle, and the right boundary remains unchanged. For obstacles that can be crossed, the right boundary of the obstacle minus the distance between the vehicle wheels is taken as the left boundary of the current point, and the left boundary of the obstacle plus the distance between the vehicle wheels is taken as the right boundary of the current point.

[0100] The above arrangement of this embodiment calculates the obstacle crossing boundary by the left and right boundaries of the obstacle and the distance between the vehicle wheels, thereby effectively constraining the obstacle crossing drivable area.

[0101] Furthermore, after obtaining the new boundary of each discrete point, i.e., the local path point, local path planning is performed based on the piecewise acceleration algorithm. Specifically, for each discrete point, the lateral offset l of the point, and the first-order and second-order derivatives l′ and l″ of the lateral offset relative to s are taken as optimization variables. Assume that between every two discrete points, there is a constant third-order derivative term l′″ connecting adjacent discrete points, and the third-order derivative term can be calculated by differential means from adjacent second-order derivatives. The optimization goal is to make the lateral offset of each discrete point as close to the guide line as possible. At the same time, for the first-order, second-order, and third-order derivatives, the optimization goal is also to make them as small as possible. Through optimization, the centripetal acceleration of the lateral motion can be controlled to ensure the comfort of the path. At the same time, the swing amplitude constraint of the center of gravity and the rigid body constraint are considered for final optimization. For example, Figure 3 As shown, Figure 3 It is a schematic diagram of the result of the local path obstacle crossing planning provided by the present invention. The local path planning is performed according to the above method to obtain the final second optimized path combining the obstacle bypassing and obstacle crossing functions.

[0102] The embodiment of the present invention provides a method for obstacle crossing decision and obstacle crossing path planning, so that the autonomous driving vehicle can avoid running over obstacles, reduce the trouble of circumventing obstacles, and improve comfort.

[0103] In some specific embodiments of the present invention, Figure 4 As shown, Figure 4 The specific flow chart of the local path obstacle planning method is shown. The local path obstacle planning method provided by this solution includes the following steps:

[0104] Step 1: Obtain global path trajectory points:

[0105] In this step, the autonomous driving vehicle determines its own position and the destination position through the positioning system, uses the navigation map road information to generate the trajectory points of the global path, and drives according to the global trajectory points.

[0106] Step 2: Obstacle information acquisition:

[0107] In this step, obstacle information is perceived through the environment to determine whether to bypass or cross the obstacle.

[0108] Specifically, autonomous driving vehicles use perception sensors such as cameras, millimeter-wave radars, lidars, etc. to perceive the environment on the driving path in real time, detect obstacle information ahead, obtain the obstacle's location coordinates, width and height, moving speed, category and other information, and pass the obstacle information to the module that executes local path planning.

[0109] Step 3: Obstacle screening:

[0110] In this step, the width and height of obstacles that are allowed to be crossed can be set by the vehicle's minimum ground clearance and wheel spacing. Obstacles larger than this width and height and dynamic obstacles are avoided by bypassing them, and static obstacles smaller than this width and height are crossed.

[0111] Step 4: Obtain driving boundary:

[0112] In this step, the module that performs local path planning intercepts the path length to be planned at the current moment from the global trajectory point as a guide line or reference line. The obstacle information that can only be bypassed and avoided in the environment obtained by step three screening is mapped to the Frenet coordinate system, wherein the Frenet coordinate system is based on the guide line construction, the left side is the positive direction, and the right side is the negative direction. In order to pass the obstacle more safely, the length and width of the obstacle are expanded, and can be adjusted according to the actual environment and the situation of the vehicle, such as adding a certain value to the width on the left and right, and adding a certain value to the height before and after. Discretize the target path along the guide line direction according to the same interval Δ s, and calculate the drivable area of ​​each discrete point s. The initial boundary of the discrete point s is the two sides of the road. For the obstacle that needs to be bypassed and avoided at the current discrete point s position, if the distance from the leftmost side of the obstacle to the center line of the lane is closer than the distance from the rightmost side to the center line of the lane, then the right boundary of the current point takes the leftmost side of the obstacle, and the left boundary remains unchanged. On the contrary, the left boundary of the current point takes the rightmost side of the obstacle, and the right boundary remains unchanged.

[0113] Step 5: First path optimization:

[0114] After obtaining the new boundaries of all local path points in step 4, local path planning is performed based on the segmented acceleration algorithm.

[0115] Specifically, for each discrete point, the lateral offset l of the point, the first-order and second-order derivatives l′ and l″ of the lateral offset relative to s are taken as optimization variables. Assume that there is a constant third-order derivative term l′″ connecting adjacent discrete points between every two discrete points. The third-order derivative term can be calculated by differential means of adjacent second-order derivatives. The optimization result is to make the lateral offset of each discrete point as close to the guide line as possible. The first-order, second-order and third-order derivatives need to be as small as possible to control the centripetal acceleration of the lateral motion and ensure the comfort of the path. The final optimization is then performed considering the swing amplitude constraint of the center of gravity and the rigid body constraint.

[0116] The algorithm is used to optimize the optimal path for bypassing large obstacles.

[0117] Step 6: Coordinate point replacement:

[0118] The obstacle-avoiding path obtained by the first path optimization is converted back to the initial Mercator coordinate system as a new guideline. Then the coordinate points on the center line of the crossable obstacles in the vehicle passage area near the guideline are replaced with the points on the guideline closest to it, and the replaced guideline is smoothed as the guideline for the second optimization. At this time, the Frenet coordinate system constructed by the second optimized guideline is obtained.

[0119] Step 7: Obtain the driving boundary of the obstacle crossing object:

[0120] In this step, the information of obstacles to be bypassed and obstacles to be crossed in the environment is mapped to the Frenet coordinate system constructed based on the second optimized guide line. In order to pass the obstacle more safely, the length and width of the obstacle are expanded. The guide line direction is discretized according to the same interval Δs, and the drivable area of ​​each discrete point s is calculated. The initial boundaries of the discrete point s are the two sides of the road. For the obstacles that need to be bypassed at the current discrete point s, if the distance from the leftmost side of the obstacle to the center line of the lane is closer than the distance from the rightmost side to the center line of the lane, the right boundary of the current point is the leftmost side of the obstacle, and the left boundary remains unchanged. Otherwise, the left boundary of the current point is the rightmost side of the obstacle, and the right boundary remains unchanged. For obstacles that can be crossed, the right boundary of the obstacle minus the distance between the vehicle wheels is taken as the left boundary of the current point, and the left boundary of the obstacle plus the distance between the vehicle wheels is taken as the right boundary of the current point.

[0121] Step 8: Get the second optimization path:

[0122] After obtaining the new boundary of all local path points including the obstacles to be circumvented and the obstacles to be crossed in step seven, local path planning is performed based on the segmented acceleration algorithm.

[0123] That is, the segmented acceleration algorithm of step four is used for local path planning, and the algorithm is used to optimize the local path obstacle-crossing planning result that combines the obstacle avoidance and obstacle-crossing functions.

[0124] The local path obstacle-crossing planning method provided by the embodiment of the present invention sets the width and height of obstacles that are allowed to be crossed through the vehicle through the minimum ground clearance and wheel-to-wheel distance; optimizes the optimal path for bypassing large obstacles through an algorithm and then optimizes the obstacle-crossing path; in the specific optimization process, by adding the center line of the obstacle to the guide line and combining the constraint boundary setting of the obstacle-crossing path, obstacle-crossing decisions and obstacle-crossing path planning are performed on the basis of the obstacle-circling optimized path, and obstacle-crossing decisions and safe and efficient obstacle-crossing path planning are performed through the size attributes and position of the obstacle, so as to avoid running over the obstacle and reduce the trouble of bypassing the obstacle.

[0125] The local path obstacle-crossing planning device provided by the present invention is described below. The local path obstacle-crossing planning device described below and the local path obstacle-crossing planning method described above can be referred to each other.

[0126] In some specific embodiments of the present invention, Figure 5 As shown, this solution provides a local path obstacle planning device, including:

[0127] A first path optimization module 51 is used to obtain a first optimized path of the target route to be planned by using an obstacle avoidance optimization algorithm;

[0128] The obstacle determination module 52 is used to determine the position coordinates of obstacles that are allowed to be crossed on the first optimized path;

[0129] The second path optimization module 53 is used to correct the first optimized path based on the position coordinates of each obstacle to be crossed, so as to obtain a second optimized path of the target route to be planned.

[0130] Possibly, the step of using an obstacle avoidance optimization algorithm to obtain a first optimized path of the target route to be planned specifically includes:

[0131] The length of the target route to be planned at the current moment is intercepted from the global trajectory point to generate a guide line for the target route;

[0132] Mapping the information of the obstacles to be circumvented on the target route into the Frenet coordinate system;

[0133] Discretizing the target route at equal intervals along the guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point;

[0134] The drivable areas of the discrete points are connected by an optimization algorithm to obtain a first optimized path for the target route to be planned.

[0135] Specifically, this embodiment provides an implementation method for obtaining a first optimized path using an obstacle avoidance optimization algorithm.

[0136] Possibly, the steps of calculating the drivable area of ​​each discrete point include:

[0137] Set the road boundary as the initial boundary of each discrete point;

[0138] If there is an obstacle to be circumvented at the location of the current discrete point, the initial boundary is corrected according to the position coordinates of the obstacle to obtain the driving boundary of each discrete point;

[0139] Based on the driving boundary, a drivable area of ​​each discrete point is obtained.

[0140] Specifically, this embodiment provides an implementation method for calculating the drivable area of ​​each discrete point.

[0141] Possibly, the step of connecting the drivable areas of the discrete points by an optimization algorithm specifically includes:

[0142] Based on the driving boundaries of each of the discrete points, determining the lateral offset of each of the discrete points and the first-order derivative and the second-order derivative of the lateral offset relative to each of the discrete points;

[0143] Based on the second-order derivative, the third-order derivative between adjacent discrete points is calculated;

[0144] The objective function of the optimization algorithm is set as the lateral offset of the discrete point, the minimum value of the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point, and the constraint conditions are set as the swing amplitude constraint of the center of the vehicle head and the rigid body constraint, and the lateral offset of the discrete point, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point are optimized;

[0145] Based on the optimized lateral offset, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete points, the drivable areas of the discrete points are connected.

[0146] Specifically, this embodiment provides an implementation method for connecting the drivable areas of the discrete points.

[0147] Possibly, the step of determining the position coordinates of obstacles on the first optimized path that are allowed to be crossed includes:

[0148] Based on the minimum ground clearance and minimum wheel-to-wheel distance of the vehicle, a height threshold and a width threshold of obstacles that are allowed to be crossed are determined;

[0149] Based on the height threshold and the width threshold, the position coordinates of obstacles that are allowed to be crossed on the first optimized path are determined.

[0150] Specifically, this embodiment provides an implementation method for determining the position coordinates of an obstacle that allows passage over the obstacle.

[0151] Possibly, based on the position coordinates of each obstacle to be traversed, the first optimized path is corrected to obtain the second optimized path of the target route to be planned, specifically comprising:

[0152] Respectively obtain the coordinate points on the guide line that are closest to each obstacle to be crossed;

[0153] The position coordinates of each obstacle to be crossed are used to replace each of the coordinate points to form a corrected guide line;

[0154] Based on the corrected guide line, a second optimized path of the target route to be planned is obtained.

[0155] Specifically, this embodiment provides an implementation method for correcting the first optimization path.

[0156] Possibly, the step of obtaining a second optimized path of the target route to be planned based on the corrected guide line specifically includes:

[0157] The corrected guide line is smoothed, and the information of the obstacles to be circumvented and the obstacles to be crossed on the target route is mapped into the Frenet coordinate system;

[0158] Discretizing the target route at equal intervals along the corrected guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point;

[0159] The drivable areas of the discrete points are connected by an optimization algorithm to obtain a second optimized path for the target route to be planned.

[0160] Specifically, this embodiment provides an implementation method for obtaining a second optimized path.

[0161] Figure 6 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 6 As shown, the electronic device may include: a processor 610, a communication interface 620, a memory 630 and a communication bus 640, wherein the processor 610, the communication interface 620 and the memory 630 communicate with each other through the communication bus 640. The processor 610 may call the logic instructions in the memory 630 to execute the local path obstacle-crossing planning method, which includes: using an obstacle-circumventing optimization algorithm to obtain a first optimized path of the target route to be planned; determining the position coordinates of obstacles on the first optimized path that are allowed to cross obstacles; based on the position coordinates of each obstacle to be crossed, the first optimized path is corrected to obtain a second optimized path of the target route to be planned.

[0162] In addition, the logic instructions in the above-mentioned memory 630 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0163] On the other hand, the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute the local path obstacle-crossing planning method provided by the above-mentioned methods, and the method includes: using an obstacle avoidance optimization algorithm to obtain a first optimized path of the target route to be planned; determining the position coordinates of obstacles on the first optimized path that allow obstacle crossing; based on the position coordinates of each obstacle to be crossed, correcting the first optimized path to obtain a second optimized path of the target route to be planned.

[0164] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the above-mentioned local path obstacle-crossing planning methods, the method comprising: using an obstacle avoidance optimization algorithm to obtain a first optimized path of the target route to be planned; determining the position coordinates of obstacles on the first optimized path that allow traversal of obstacles; based on the position coordinates of each obstacle to be traversed, correcting the first optimized path to obtain a second optimized path of the target route to be planned.

[0165] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0166] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0167] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A local path obstacle planning method, characterized in that: include: Using the obstacle avoidance optimization algorithm, the first optimized path of the target route to be planned is obtained; Determine the position coordinates of obstacles that are allowed to be crossed on the first optimized path; Based on the position coordinates of each obstacle to be crossed, the first optimized path is corrected to obtain a second optimized path of the target route to be planned; The step of using the obstacle avoidance optimization algorithm to obtain the first optimized path of the target route to be planned specifically includes: The length of the target route to be planned at the current moment is intercepted from the global trajectory point to generate a guide line for the target route; Mapping the information of the obstacles to be circumvented on the target route into the Frenet coordinate system; Discretizing the target route at equal intervals along the guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point; Connecting the drivable areas of the discrete points through an optimization algorithm to obtain a first optimized path of the target route to be planned; The step of determining the position coordinates of obstacles that are allowed to be crossed on the first optimized path specifically includes: Based on the minimum ground clearance and minimum wheel-to-wheel distance of the vehicle, a height threshold and a width threshold of obstacles that are allowed to be crossed are determined; Based on the height threshold and the width threshold, determining the position coordinates of obstacles that are allowed to be crossed on the first optimized path; The step of correcting the first optimized path based on the position coordinates of each obstacle to be crossed to obtain the second optimized path of the target route to be planned specifically includes: Respectively obtain the coordinate points on the guide line that are closest to each obstacle to be crossed; The position coordinates of each obstacle to be crossed are used to replace each of the coordinate points to form a corrected guide line; Based on the corrected guide line, a second optimized path of the target route to be planned is obtained.

2. The local path obstacle planning method according to claim 1, characterized in that: The steps for calculating the drivable area of ​​each discrete point include: Set the road boundary as the initial boundary of each discrete point; If there is an obstacle to be circumvented at the location of the current discrete point, the initial boundary is corrected according to the position coordinates of the obstacle to obtain the driving boundary of each discrete point; Based on the driving boundary, a drivable area of ​​each discrete point is obtained.

3. The local path obstacle planning method according to claim 2, characterized in that: The step of connecting the drivable areas of the discrete points by using an optimization algorithm specifically includes: Based on the driving boundaries of each of the discrete points, determining the lateral offset of each of the discrete points and the first-order derivative and the second-order derivative of the lateral offset relative to each of the discrete points; Based on the second-order derivative, the third-order derivative between adjacent discrete points is calculated; The objective function of the optimization algorithm is set as the lateral offset of the discrete point, the minimum value of the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point, and the constraint conditions are set as the swing amplitude constraint of the center of the vehicle head and the rigid body constraint, and the lateral offset of the discrete point, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete point are optimized; Based on the optimized lateral offset, the first-order derivative, the second-order derivative, and the third-order derivative of the discrete points, the drivable areas of the discrete points are connected.

4. The local path obstacle planning method according to any one of claims 1 to 3, characterized in that: The step of obtaining a second optimized path of the target route to be planned based on the corrected guide line specifically includes: The corrected guide line is smoothed, and the information of the obstacles to be circumvented and the obstacles to be crossed on the target route is mapped into the Frenet coordinate system; Discretizing the target route at equal intervals along the corrected guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point; The drivable areas of the discrete points are connected by an optimization algorithm to obtain a second optimized path for the target route to be planned.

5. A local path obstacle planning device, characterized in that: include: A first path optimization module, used to obtain a first optimized path of a target route to be planned by adopting an obstacle avoidance optimization algorithm; An obstacle determination module for overcoming obstacles, used to determine the position coordinates of obstacles that are allowed to be overcome obstacles on the first optimized path; A second path optimization module, used for correcting the first optimized path based on the position coordinates of each obstacle to be crossed, to obtain a second optimized path of the target route to be planned; The step of using the obstacle avoidance optimization algorithm to obtain the first optimized path of the target route to be planned specifically includes: The length of the target route to be planned at the current moment is intercepted from the global trajectory point to generate a guide line for the target route; Mapping the information of the obstacles to be circumvented on the target route into the Frenet coordinate system; Discretizing the target route at equal intervals along the guide line direction in the Frenet coordinate system, and calculating the drivable area of ​​each discrete point; Connecting the drivable areas of the discrete points through an optimization algorithm to obtain a first optimized path of the target route to be planned; The step of determining the position coordinates of obstacles that are allowed to be crossed on the first optimized path specifically includes: Based on the minimum ground clearance and minimum wheel-to-wheel distance of the vehicle, a height threshold and a width threshold of obstacles that are allowed to be crossed are determined; Based on the height threshold and the width threshold, determining the position coordinates of obstacles that are allowed to be crossed on the first optimized path; The step of correcting the first optimized path based on the position coordinates of each obstacle to be crossed to obtain the second optimized path of the target route to be planned specifically includes: Respectively obtain the coordinate points on the guide line that are closest to each obstacle to be crossed; The position coordinates of each obstacle to be crossed are used to replace each of the coordinate points to form a corrected guide line; Based on the corrected guide line, a second optimized path of the target route to be planned is obtained.

6. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps of the local path obstacle planning method as described in any one of claims 1 to 4 are implemented.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the local path obstacle planning method according to any one of claims 1 to 4 are implemented.

Images