A dynamic correction method, device and equipment for automatic driving path planning

Abstract

The present application belongs to the field of autonomous driving technology, and discloses a dynamic correction method, device and equipment for autonomous driving path planning. The method determines the drivable area of ​​the current vehicle by acquiring the point cloud data of the laser radar and the image data of the visual camera; constructs a grid map with composite weights, and obtains the global reference path through the Dijkstra algorithm based on the grid map; uses the dynamic programming method to determine the sampling point set of the current vehicle from the global reference path, fits the sampling point set to obtain a spline curve, and performs convex space conversion on the drivable area based on the spline curve through the scattered point sampling method to obtain the convex space boundary of the drivable area; constructs a quadratic programming problem based on the convex space boundary, and solves the expected trajectory; when the reference trajectory is detected to deviate from the drivable area, adjusts the weights of the sampling point set and the quadratic programming problem, and corrects the expected trajectory, which can improve the accuracy of the issued expected trajectory.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to a method, device and equipment for dynamic correction of autonomous driving path planning. Background Art

[0002] Autonomous driving technology is in a rapid development stage and has made significant progress in many fields. Currently, there are two main technologies for autonomous driving path planning: the first is based on high-precision map matching and guidance planning methods; the other is a planning method based on online detection of drivable areas.

[0003] The method based on high-precision map guidance must first build the elements and data model of the high-precision map, take the center line of the map lane as the planning baseline, and then select the least cost planning route from the candidate lane cluster by cost calculation by considering the distance to the boundary of the drivable area. To obtain an accurate planning baseline, it is necessary to use a high-precision map for online matching; semantic information can also be used for map matching based on likelihood probability. First, the semantics of the road signs are extracted using a neural network, and then the fusion of the real environment and the high-precision map is constructed to achieve vehicle positioning. However, the use of high-precision maps faces the following problems: because high-precision sensors and professional collection vehicles are required for advance collection, the cost of map construction and maintenance is high; and in order to ensure timeliness and accuracy, the data needs to be updated and maintained regularly, which also increases the cost. Trajectory planning is highly dependent on high-precision maps. If there are changes or errors in the map, but the high-precision map is not updated in time, it will cause positioning deviations.

[0004] In the absence of a priori high-precision maps, environmental perception can be achieved by online detection of drivable areas. Information about drivable areas can be obtained in different ways, such as extracting features from images using the attention mechanism of a neural network, or obtaining a local cost map of the current frame through point cloud information from a lidar. After obtaining the drivable area, dynamic planning is used to traverse and find the optimal path, and finally the planned trajectory is obtained. However, this method also faces some problems and has a limited scope of application. For special vehicles that need to navigate on unstructured roads, it is difficult to obtain reference lines on unstructured roads. During the processing, when the curvature of the reference line is large, problems such as discontinuous trajectories, vehicle shape deformation, and increased computational burden will occur during the coordinate transformation process. Summary of the invention

[0005] To this end, the embodiments of the present application provide a dynamic correction method, device and equipment for autonomous driving path planning, thereby improving the accuracy of the issued trajectory.

[0006] In a first aspect, the present application provides a dynamic correction method for autonomous driving path planning.

[0007] This application is achieved through the following technical solutions:

[0008] A dynamic correction method for automatic driving path planning, the dynamic correction method comprising:

[0009] Obtain the point cloud data of the laser radar and the image data of the visual camera to determine the drivable area of ​​the current vehicle;

[0010] Constructing a grid map with composite weights, and obtaining a global reference path through a Dijkstra algorithm based on the grid map;

[0011] A sampling point set of the current vehicle distance from the global reference path is determined by a dynamic programming method, a spline curve is obtained by fitting the sampling point set, and the drivable area is convexly spaced based on the spline curve by a scattered point sampling method to obtain a convex space boundary of the drivable area;

[0012] A quadratic programming problem is constructed based on the convex space boundary, and a desired trajectory is obtained by solving the problem;

[0013] Check whether the reference trajectory sent by the upper layer is within the drivable area. If it is detected that the reference trajectory deviates from the drivable area, adjust the weights of the sampling point set and the quadratic programming problem to correct the expected trajectory.

[0014] In a preferred example of the present application, it can be further configured that the step of obtaining the point cloud data of the laser radar and the image data of the visual camera to determine the drivable area of ​​the current vehicle includes:

[0015] The steps of obtaining the point cloud data of the laser radar and the image data of the visual camera and determining the drivable area of ​​the current vehicle include:

[0016] Obtaining point cloud data of the laser radar, and determining obstacle information according to the point cloud data, wherein the obstacle information includes the shape, position and distance information of the obstacle;

[0017] Acquire image data from a visual camera, and analyze the image data using image processing technology to determine semantic information of obstacles;

[0018] The obstacle information and the semantic information are combined to determine a cost value corresponding to each grid in the grid map, a passable grid is determined according to the cost value, and a continuous drivable area is formed based on the passable grid.

[0019] In a preferred example of the present application, it can be further configured that the step of constructing a grid map with composite weights includes:

[0020] The calculation formula of the composite weight of the raster map is:

[0021] G i,j =w s ||a i,j -a i+1,k ||2+w r ||a i+1,k -r i+1,j ||2-w d ||a i+1,k -d i+1 ||2,

[0022] Among them, G i,j represents the weight value of the grid point in the i-th row and j-th column, a i,j represents the coordinates of the grid point in the i-th row and j-th column, a i+1,k represents the coordinates of the grid point in the i+1th row and the kth column, r represents the reference trajectory of the desired vehicle sent by the upper control terminal, and r i+1,j Indicates that the grid point in row i+1 and column j is closest to the reference trajectory, d i+1 represents the grid point occupied by the obstacle in the i+1th row, w s represents the smoothing weight, w r represents the reference trajectory weight, w d represents the obstacle weight, ||a i,j -a i+1,k ||2 represents the two-norm.

[0023] In a preferred example of the present application, it can be further configured that the step of searching for a global reference path based on the grid map by using the Dijkstra algorithm includes:

[0024] The minimum cost path is obtained by searching through the Dijkstra algorithm, and the minimum cost path of the current cycle is compared with the minimum cost path of the previous cycle to determine whether the change in trajectory cost between adjacent cycles exceeds the threshold;

[0025] When the trajectory cost change of the minimum cost path of the adjacent cycle exceeds the threshold, the minimum cost path of this time is output as the global reference path;

[0026] When the trajectory cost change of the minimum cost path of the adjacent cycle is less than the threshold, the minimum cost path of the previous cycle is output as the global reference path.

[0027] In a preferred example of the present application, it can be further configured that the step of fitting the sampling point set to obtain a spline curve includes:

[0028] According to the sampling points in the obtained sampling point set, a cubic spline curve is fitted to solve the spline coefficients to obtain the spline curve; wherein the spline coefficients are obtained by solving the interpolation conditions, continuity conditions or boundary conditions.

[0029] In a preferred example of the present application, it can be further configured that the step of converting the drivable area into a convex space by using a scattered point sampling method based on the spline curve to obtain a convex space boundary of the drivable area includes:

[0030] Locate the coordinates on the Cartesian coordinate system, scatter points on both sides of the normal of the spline curve, and expand the point set along the direction of the vehicle's travel;

[0031] Connect the boundary points of the i-th scattering point set with the boundary points of the i+1-th scattering point set and extend them, determine the intersection point of the extension line and the i+2-th normal scattering line segment, and determine whether the intersection point is in the drivable area;

[0032] When the intersection point is within the drivable area, it is considered that there is a concave space in the original drivable area, and the boundary of the drivable area corresponding to the i+2th normal scattering line segment is updated using the intersection point to obtain the convex space boundary of the drivable area.

[0033] In a preferred example of the present application, it can be further configured that a quadratic programming problem is constructed based on the convex space boundary, and the step of solving the desired trajectory includes:

[0034] Construct discrete state equations for trajectory planning;

[0035] Based on the convex space boundary and state space constraints, determine the optimization objective function and constraints of trajectory planning;

[0036] Substituting the optimization objective function and constraint conditions into the discrete state equation to obtain a quadratic programming problem;

[0037] The desired trajectory is obtained by solving the quadratic programming problem using the QP solver.

[0038] In a preferred example of the present application, it can be further set as follows:

[0039] If it is detected that the reference trajectory deviates from the drivable area, the step of adjusting the sampling point set and the weight of the quadratic programming problem to correct the expected trajectory includes: when the reference trajectory deviates from the drivable area, increasing the point density in the drivable area to expand the convex space boundary of the drivable area;

[0040] Sampling is performed according to the midpoints of the boundary points on both sides of the expanded convex space, and the sampling points are fitted with a cubic spline curve to obtain the midline of the convex space;

[0041] The weight value of the optimization objective function of the quadratic programming problem is modified based on the global reference path and the center line of the convex space to obtain the modified expected trajectory;

[0042] The corrected desired trajectory is subjected to filtering and smoothing processing in time and space, and the smoothed desired trajectory is output.

[0043] In a second aspect, the present application provides a dynamic correction device for autonomous driving path planning.

[0044] This application is achieved through the following technical solutions:

[0045] A dynamic correction device for autonomous driving path planning, the dynamic correction device is used to execute the dynamic correction method for autonomous driving path planning described in the first aspect above, the dynamic correction device comprising:

[0046] The traffic area determination module is used to obtain the point cloud data of the laser radar and the image data of the visual camera to determine the current drivable area of ​​the vehicle;

[0047] A path planning module is used to construct a grid map with composite weights, and obtain a global reference path based on the grid map through a Dijkstra algorithm search;

[0048] A convex spatialization module is used to determine a sampling point set of the current vehicle distance from the global reference path by using a dynamic programming method, fit the sampling point set to obtain a spline curve, and perform convex spatialization on the drivable area by using a scattered point sampling method based on the spline curve to obtain a convex spatial boundary of the drivable area;

[0049] A trajectory generation module, used to construct a quadratic programming problem based on the convex space boundary and solve it to obtain a desired trajectory;

[0050] The trajectory correction module is used to detect whether the reference trajectory sent by the upper layer is within the drivable area. If it is detected that the reference trajectory deviates from the drivable area, the weights of the sampling point set and the quadratic programming problem are adjusted to correct the expected trajectory.

[0051] On the third aspect, the present application is realized through the following technical solutions:

[0052] A dynamic correction device for autonomous driving path planning, the dynamic correction device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any one of the above-mentioned dynamic correction methods for autonomous driving path planning when executing the computer program.

[0053] In summary, compared with the prior art, the technical solution provided in the embodiment of the present application has at least the following beneficial effects:

[0054] The present application first determines the drivable area of ​​the current vehicle; constructs a grid map with composite weights, and obtains a global reference path based on the grid map through the Dijkstra algorithm search; uses a dynamic programming method to determine the sampling point set of the current vehicle from the global reference path, fits the sampling point set to obtain a spline curve, and based on the spline curve, the drivable area is convexly spaced through the scattered point sampling method to obtain the convex space boundary of the drivable area; constructs a quadratic programming problem based on the convex space boundary and solves the expected trajectory; detects whether the reference trajectory sent down from the upper layer is within the drivable area. If the reference trajectory is detected to deviate from the drivable area, adjusts the sampling point set and the weights of the quadratic programming problem to correct the expected trajectory. The dynamic correction method proposed in the present application determines the drivable area based on the cost map to identify environmental obstacles without relying on high-precision maps and high-precision positioning. It has low development cost and strong adaptability; constructs a safe boundary convex space by scattering point sampling in the Cartesian coordinate system to avoid the distortion of the vehicle and road shape in the Frenet coordinate system, and can consider real physical constraints and dynamic environmental changes. In addition, the present application performs spatiotemporal smoothing on the expected trajectory sent down, so that the output trajectory has good stability and improved robustness when dealing with environmental noise. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 A schematic diagram of a flow chart of a dynamic correction method for autonomous driving path planning provided in one embodiment of the present application;

[0056] Figure 2 A schematic diagram of convex space conversion of a drivable area provided in an embodiment of the present application;

[0057] Figure 3 A schematic diagram of a process for correcting a desired trajectory provided in an embodiment of the present application;

[0058] Figure 4 A schematic diagram of a reference trajectory deviating from a drivable area provided in yet another embodiment of the present application;

[0059] Figure 5 It is a schematic diagram of the trajectory of straight-line driving when no deviation correction operation is performed;

[0060] Figure 6 A schematic diagram of a trajectory after the trajectory is corrected according to another embodiment of the present application;

[0061] Figure 7 A schematic diagram of the structure of a dynamic correction method for autonomous driving path planning provided in one embodiment of the present application.

[0062] Description of reference numerals:

[0063] Passage area determination module-1, path planning module-2, convex space module-3, trajectory generation module-4, trajectory correction module-5. DETAILED DESCRIPTION

[0064] This specific embodiment is merely an explanation of the present application and is not a limitation of the present application. After reading this specification, those skilled in the art may make modifications to the present embodiment without any creative contribution as needed. However, as long as it is within the scope of the claims of the present application, it shall be protected by the patent law.

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

[0066] In addition, the term "and / or" in this application is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this application, unless otherwise specified, generally indicates that the associated objects before and after are in an "or" relationship.

[0067] In this application, the terms "first", "second", etc. are used to distinguish identical or similar items with basically the same effects and functions. It should be understood that there is no logical or temporal dependency between "first", "second", and "nth", nor is there any limitation on quantity and execution order.

[0068] The following is a further detailed description of the embodiments of the present application in conjunction with the accompanying drawings. Figure 1 As shown, the steps of the method include:

[0069] S10: Obtain the point cloud data of the laser radar and the image data of the visual camera to determine the drivable area of ​​the current vehicle.

[0070] Laser radar (LiDAR) and visual cameras are used to obtain data about the vehicle's surroundings. Accurate distance and spatial information can be analyzed from the point cloud data of the laser radar, and road features such as road surface texture, color, lane lines, etc. can be analyzed from the image data of the visual camera. It can obtain and process data about the surrounding environment in real time without relying on the production of high-precision maps, and can respond quickly to dynamically changing environments, such as temporary obstacles or changes in traffic signs. The data collected by laser radar and visual cameras at the same time is more economical than high-precision maps. In addition, combining data from multiple sensors can provide a more accurate data source for autonomous driving path planning.

[0071] S20: Construct a grid map with composite weights, and obtain a global reference path based on the grid map through Dijkstra algorithm search.

[0072] Among them, the composite weight includes the smoothing weight of the grid, the reference trajectory weight and the obstacle weight. The smoothing weight reflects the smoothness of the path. The smooth path has a lower weight, and the path with ups and downs has a higher weight. The reference trajectory weight represents the consistency between the path and the reference trajectory. The reference trajectory is sent by the upper control end and is the reference trajectory of the expected vehicle. It can be defined by calculating the distance between the grid and the reference trajectory. The closer the distance, the lower the weight, and the farther the distance, the higher the weight. The obstacle weight reflects the impact of the obstacles of the grid on the path. The more obstacles there are, the greater the impact, and the greater the corresponding weight. The composite weight takes into account multiple factors, making the path planning more robust in the face of complex environments.

[0073] S30: Determine the sampling point set of the current vehicle distance from the global reference path by using a dynamic programming method, fit the sampling point set to obtain a spline curve, and convert the drivable area into a convex space based on the spline curve by using a scattered point sampling method to obtain a convex space boundary of the drivable area.

[0074] The spline curve provides a smooth global reference path, which is conducive to smooth vehicle driving.

[0075] S40: Construct a quadratic programming problem based on the convex space boundary and solve it to obtain the desired trajectory.

[0076] S50: Detect whether the reference trajectory sent from the upper layer is within the drivable area. If it is detected that the reference trajectory deviates from the drivable area, adjust the weights of the sampling point set and the quadratic programming problem to correct the expected trajectory.

[0077] This application can effectively correct the deviation of the expected trajectory sent down without relying on high-precision maps and high-precision RTK positioning, relying only on GNSS positioning accuracy; scattering points on the Cartesian coordinate system to construct a convex space of a safe drivable area, avoiding the distortion of the vehicle and road shape in the Frenet coordinate system, and taking into account real physical constraints and dynamic environmental changes; after calculating the expected trajectory, when it is monitored that the reference trajectory sent down by the upper layer deviates from the drivable area, the expected trajectory will be corrected by adjusting the sampling points and the quadratic programming problem, and the expected trajectory will be limited to the drivable area, and the expected trajectory sent down can be corrected in time when there is a deviation. In particular, when special vehicles are driving on unstructured roads, the sent path may not be within the drivable area, and this method can automatically correct the deviated part.

[0078] In some embodiments, the specific implementation process of step S10 includes: obtaining point cloud data of a laser radar, determining obstacle information according to the point cloud data, and the obstacle information includes the shape, position and distance information of the obstacle; obtaining image data of a visual camera, analyzing the image data through image processing technology to determine the semantic information of the obstacle; combining the obstacle information and semantic information, determining the cost value corresponding to each grid of the grid map, determining the passable grid according to the cost value, and forming a continuous drivable area based on the passable grid. In the actual implementation process, the obstacle information and semantic information are combined as the perceived environmental information, the difficulty of passage is determined according to the environmental information, the environmental information is mapped to the grid map, and the corresponding cost value is assigned to each grid according to the difficulty of passage of each grid. At the same time, a suitable cost value threshold is set. When the cost value of a grid is higher than the cost value threshold, a non-drivable label is added to the grid as a non-drivable grid; when the cost value of a grid is lower than the cost value threshold, a drivable label is added to the grid as a drivable grid. Connectivity analysis is performed on low-cost drivable grids, and isolated low-cost drivable grids and some high-cost noise are deleted to ensure that the drivable grids can form a continuous drivable area. The cost value threshold can be set according to actual needs.

[0079] In some embodiments, the step of constructing a grid map with composite weights in step S20 includes: constructing a grid map with composite weights according to the initial point and the end point of the reference path sent by the upper layer, first assigning a double-precision maximum value to the grid point, and then calculating the composite weight of the grid map according to the following formula:

[0080] G i,j =w s ||a i,j -a i+1,k ||2+w r ||a i+1,k-r i+1,j ||2-w d ||a i+1,k -d i+1 ||2,

[0081] Among them, G i,j represents the weight value of the grid point in the i-th row and j-th column, a i,j represents the coordinates of the grid point in the i-th row and j-th column, a i+1,k represents the coordinates of the grid point in the i+1th row and the kth column, r represents the reference trajectory of the desired vehicle sent by the upper control terminal, and r i+1,j Indicates that the grid point in row i+1 and column j is closest to the reference trajectory, d i+1 represents the grid point occupied by the obstacle in the i+1th row, w s represents the smoothing weight, w r represents the reference trajectory weight, w d represents the obstacle weight, ||a i,j -a i+1,k ||2 represents the two-norm.

[0082] After obtaining the grid map with composite weights, the specific steps of searching for a global reference path based on the grid map through the Dijkstra algorithm include:

[0083] The minimum cost path is searched through the Dijkstra algorithm, and the minimum cost path of the current cycle is compared with the minimum cost path of the previous cycle to determine whether the change in trajectory cost between adjacent cycles exceeds the threshold; when the change in trajectory cost of the minimum cost path of adjacent cycles exceeds the threshold, the minimum cost path of this time is output as the global reference path; when the change in trajectory cost of the minimum cost path of adjacent cycles is less than the threshold, the minimum cost path of the previous cycle is output as the global reference path. Among them, the change refers to the difference in trajectory cost values, and the threshold can be set according to actual needs. In the actual implementation process, the trajectory with the minimum cost is searched according to the Dijkstra algorithm. Let for Check if the points in the safe area of ​​point set i+1 exist in the trajectory of the dynamic programming of the previous cycle. If so, set the weights of other paths to the maximum value. The trajectory cost value and G i,j The cost value of the trajectory searched in the previous moment is compared. If the change in the trajectory cost value is less than the threshold, the dynamic planning trajectory of the previous moment is sampled. This setting is to avoid the situation where the vehicle is trapped in multiple optimal trajectories in front of obstacles and cannot choose a stable trajectory. It can save computing resources, improve the efficiency of path planning, and provide path stability, providing passengers with a more reliable and safe driving experience.

[0084] In some embodiments, the step of fitting the sampling point set to obtain a spline curve in step S30 includes:

[0085] According to the sampling points in the obtained sampling point set, a cubic spline curve is fitted to solve the spline coefficients to obtain the spline curve; wherein the spline coefficients are obtained by solving the interpolation conditions, continuity conditions or boundary conditions.

[0086] After obtaining the spline curve, the drivable area is further converted into a convex space by a scattering sampling method, and the steps of obtaining the convex space boundary of the drivable area include: locating coordinates on a Cartesian coordinate system, scattering points on both sides of the normal of the spline curve, and expanding the point set along the vehicle's forward direction; connecting and extending the boundary points of the i-th scattering point set with the boundary points of the i+1-th scattering point set, determining the intersection point of the extension line with the i+2-th normal scattering line segment, and judging whether the intersection point is in the drivable area; when the intersection point is in the drivable area, it is considered that there is a concave space in the original drivable area, and the intersection point is used to update the boundary of the drivable area corresponding to the i+2-th normal scattering line segment to obtain the convex space boundary of the drivable area.

[0087] In the actual implementation process, the dynamic programming method is used to search for the sampling point set R = {(x i ,y i )},i∈0,...,N g , where N g is the number of sampling points. All sampling points are fitted with cubic spline curves. When the global reference path is in the interval [x i ,x i+1 ], the spline function can be expressed as:

[0088] S i (x) = a i +b i (xx i )+c i (xx i ) 2 +d i (xx i ) 3 ,

[0089] Among them, a i ,b i ,c i ,d i These are spline coefficients, which can be obtained by solving the linear equations through interpolation conditions, continuity conditions and boundary conditions.

[0090] like Figure 2As shown in the figure, the points are scattered at the positioning coordinates in the Cartesian coordinate system, and the points are scattered on both sides of the normal of the spline curve, and the point set is expanded along the spline curve and the vehicle's driving direction. Specifically, the arc length of the preview arc forward along the spline curve is s, and the arc length spacing of each set of points is Δs. Each set of points is represented by i+k, where k∈{0,...,s / Δs}. According to the cost value of the normal point set position, the boundaries of the drivable area can be obtained, which are AA′, BB′, CC′, DD′ in the figure.

[0091] The polygonal constraints of the drivable area are set by scattering sampling. However, the polygon has a concave space and cannot be directly used to solve the linear quadratic problem. Therefore, the polygon needs to be convex. For example, connect the boundary point A of the i-th scattering point set and the boundary point B of the i+1-th scattering point set and extend them to Indicates that inspection and the scattered line segment of the i+1th scattered point set If the intersection point is outside the drivable area, the original space is considered to be a convex space; if the intersection point is within the drivable area, the extension line intersection point needs to be used to update the original polygon to a convex polygon.

[0092] In some embodiments, a quadratic programming problem is constructed based on the convex space boundary, and the step of solving the desired trajectory includes:

[0093] Construct the discrete state equation for trajectory planning. The discrete state equation is:

[0094]

[0095] in, Indicates the input quantity, represents the velocity variable of the horizontal axis within a single cycle, Represents the velocity variable of the ordinate within a single cycle; x k+1 represents the horizontal coordinate of the planned trajectory at the k+1th moment, x k represents the horizontal coordinate of the planned trajectory at the kth moment, Δx k represents the change in the horizontal coordinate of the planned trajectory at the kth moment, y k+1 represents the ordinate of the planned trajectory at the k+1th moment, y k represents the ordinate of the planned trajectory at the kth moment, Δy k Indicates the change in the ordinate of the planned trajectory at the kth moment, Δx k+1 Indicates the horizontal coordinate change of the planned trajectory at the k+1th time, Δy k+1 Indicates the change in the ordinate of the planned trajectory at the k+1th moment.

[0096] Based on the convex space boundary and state space constraints, the optimization objective function and constraints of trajectory planning are determined. The optimization objective function is the optimization problem to be achieved by the trajectory planning problem. The constraints include state space equality constraints and convex polygon inequality constraints:

[0097]

[0098] Among them, x i Represents the state quantity coordinate vector, r i Represents the reference line coordinate vector, m i Represents the coordinate vector of the reference line of the drivable area, u i represents the input acceleration vector, w ref ,w poly ,w u They represent the reference weight, the drivable area reference line weight, and the input weight respectively. A represents the state transfer matrix of the state space and B represents the input matrix of the state space. It is the representation of the convex space constraint of the state quantity and the input matrix constraint, and t represents the convex space boundary constraint of the state quantity and the input constraint quantity.

[0099] Substituting the optimization objective function and constraints into the discrete state equation, we get the quadratic programming problem:

[0100] Let the state variable z = [x0 … x k y0 … y k ] T , the input is

[0101] According to the discrete state equation, we can obtain: z=Ax0+BAΔx0+BBu; where z represents the intermediate state quantity, BA represents the product of matrix A and matrix B, and BBu represents the product of matrix B, matrix B and matrix u.

[0102] in,

[0103] Substituting the intermediate state quantity z into the optimization objective function, we can get the standard quadratic programming problem (QP problem):

[0104]

[0105] The desired trajectory is obtained by solving the quadratic programming problem using the QP solver. Specifically, the acceleration measures of the horizontal and vertical coordinates are solved, and the desired trajectory is obtained according to the formula z=Ax0+BAΔx0+BBu and the initial point.

[0106] In step S50, when it is detected that the desired trajectory deviates from the drivable area, the weights of the sampling point set and the quadratic programming problem are adjusted to correct the desired trajectory within the drivable area. Figure 3 As shown:

[0107] S501: when the reference trajectory deviates from the drivable area, increasing the density of points in the drivable area to expand the convex space boundary of the drivable area;

[0108] S502: sampling according to the midpoints of the boundary points on both sides of the expanded convex space, and fitting the sampling points with a cubic spline curve to obtain the midline of the convex space;

[0109] S503: Correcting the weight of the optimization objective function of the quadratic programming problem based on the global reference path and the center line of the convex space to obtain a corrected desired trajectory;

[0110] S504: Perform filtering and smoothing processing on the corrected expected trajectory in time and space, and output the smoothed expected trajectory.

[0111] Specifically, Figure 4 As shown in the example, when the reference trajectory deviates from the drivable area, there is a deviation between the reference trajectory sent down from the upper layer and the actual drivable area, and the reference trajectory deviates from the drivable area in the second half. At this time, the reference trajectory is detected to see if it deviates from the drivable area based on the scattered points. If the drivable area is on one side of the reference trajectory, it means that the reference trajectory deviates from the drivable area, and the weight coefficient is dynamically corrected; otherwise, the initial weight coefficient of the optimization objective function is maintained; the density and length of the scattered points are increased on one side of the drivable area of ​​the reference trajectory. For example, the side of the i+3 scattered point set close to point D in the figure is too short to cover the drivable area. If the scattered point set is too sparse, the area of ​​the convex space of the drivable area will be too small; according to the point set i to the point set i+N, select the midpoint A of the line connecting the vertices on both sides of the convex space. m ,B m ,C m ,D m As sampling points, use cubic spline curve to fit the sampling points to get the convex space centerline; according to the relative position deviation between the reference trajectory r and the convex space centerline curve m, reduce the weight w accordingly ref , increase the weight w poly The specific value can be obtained based on actual calibration.

[0112] According to the reference line sampling points that have been obtained, time and space filtering can be performed. The input planning path point is (x i (0) ,y i (0) ),i∈0,1,…,N, the path point output after smoothing is (x i(k) ,y i (k) ), where k is the number of iteration steps. The iteration equation of the coordinate point is as follows:

[0113]

[0114]

[0115]

[0116] When the maximum number of iterations k ≥ k max Or the iteration error is less than the threshold Δ (k) ≤Δ thr , the iteration process is exited.

[0117] For this smoothing process, when w d The larger the value, the closer the processed point is to the initial point. s The larger the value, the smoother the adjacent points of the curve.

[0118] For the smoothed sampling points, a cubic spline curve can be fitted, and the fitted curve is used as the output curve of trajectory planning.

[0119] like Figure 5 The figure shows the trajectory of straight-line driving without deviation correction, where the physical meaning of the elements in the figure is shown on the right. As can be seen from the figure, the planned trajectory still converges to the deviated blue reference trajectory sent by the upper layer, and is not within the drivable area, which poses a safety hazard.

[0120] like Figure 6 The figure shows the effect of the application after the deviation correction when the issued trajectory deviates from the drivable area. It can be seen from the figure that the blue issued trajectory deviates from the drivable area and it will be dangerous if it converges to the blue trajectory. After dynamically modifying the weight and lateral sampling range, the black planned trajectory can converge to the brown drivable area midline, and the issued trajectory can be corrected.

[0121] The autonomous driving path planning method of this application can bring the following benefits:

[0122] (1) Dynamic programming samples a weighted grid map and ensures the stability of the dynamic programming trajectory. By giving the dynamic map smoothness, reference trajectory, and obstacle weight information, the optimal trajectory can be dynamically searched. By comparing the deviation from the dynamic trajectory at the previous moment, it is ensured that there are multiple trajectories that cannot be selected in front of an obstacle, thus ensuring the stability of the dynamic programming.

[0123] (2) Perform point sampling in the Cartesian coordinate system to determine the drivable area. In the Cartesian coordinate system, the vehicle is used as the point scattering center, the normal direction of the trajectory is used as a group of points, and the tangent direction of the trajectory is used as the preview direction. The next group of tangent point sets are sampled according to the determined arc length interval.

[0124] (3) Construct a convex space based on the sampling point matrix. Connect the boundary points of adjacent point sets and extend them forward. Check whether they intersect with the next set of boundary point segments to determine whether it is a convex space. If it is concave, use the intersection point as the new boundary point to construct a convex space as a constraint for the quadratic programming problem.

[0125] (4) The objective function includes the reference trajectory, the midpoint of the drivable area, and the smoothing term. In the optimization problem, not only the reference trajectory and the smoothing term are considered, but also the midpoint of the drivable area. When the reference trajectory deviates from the drivable area, effective correction can be made to ensure that the planned trajectory does not deviate to one side.

[0126] (5) Dynamically correct the objective function weight and the tangent point set sampling interval. Because the sampling is based on the reference trajectory, the reference trajectory has a great influence on the final planned trajectory. When the reference trajectory deviates from the drivable area, the length and density of the points are increased on the side of the drivable area to expand the perception area, and the relative weight of the midline term in the objective function is adaptively increased.

[0127] Another embodiment of the present application further provides a dynamic correction device for autonomous driving path planning, the dynamic correction device is used to execute the above-mentioned dynamic correction method, such as Figure 7 As shown, the dynamic correction device comprises:

[0128] The traffic area determination module 1 is used to obtain the point cloud data of the laser radar and the image data of the visual camera to determine the current drivable area of ​​the vehicle;

[0129] Path planning module 2, used to construct a grid map with composite weights, and obtain a global reference path based on the grid map through Dijkstra algorithm search;

[0130] The convex space module 3 is used to determine the sampling point set of the current vehicle distance from the global reference path by using a dynamic programming method, fit the sampling point set to obtain a spline curve, and perform convex space conversion on the drivable area by using a scattered point sampling method based on the spline curve to obtain a convex space boundary of the drivable area;

[0131] The trajectory generation module 4 is used to construct a quadratic programming problem based on the convex space boundary and solve the desired trajectory;

[0132] The trajectory correction module 5 is used to detect whether the reference trajectory sent from the upper layer is within the drivable area. If it is detected that the reference trajectory deviates from the drivable area, the weights of the sampling point set and the quadratic programming problem are adjusted to correct the expected trajectory.

[0133] The specific definition of a dynamic correction device for autonomous driving path planning provided in this embodiment can be found in the embodiment of the dynamic correction method for autonomous driving path planning above, which will not be repeated here. Each module in the above dynamic correction device can be implemented in whole or in part by software, hardware and a combination thereof. Each of the above modules can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0134] The embodiment of the present application provides a dynamic correction device for autonomous driving path planning, which may include a processor, a memory, a network interface and a database connected via a system bus. Among them, the processor of the dynamic correction device is used to provide computing and control capabilities. The memory of the device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the device is used to communicate with an external terminal through a network connection to process the computer program being executed by the processor, so that the processor executes the steps of the dynamic correction method for autonomous driving path planning as in any of the above embodiments.

[0135] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0136] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0137] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the system described in the present application is divided into different functional units or modules to complete all or part of the functions described above.

Claims

1. A dynamic correction method for automatic driving path planning, characterized in that: The dynamic correction method comprises: Obtain the point cloud data of the laser radar and the image data of the visual camera to determine the drivable area of ​​the current vehicle; Constructing a grid map with composite weights, and obtaining a global reference path through a Dijkstra algorithm based on the grid map; A sampling point set of the current vehicle distance from the global reference path is determined by a dynamic programming method, a spline curve is obtained by fitting the sampling point set, and the drivable area is convexly spaced based on the spline curve by a scattered point sampling method to obtain a convex space boundary of the drivable area; A quadratic programming problem is constructed based on the convex space boundary, and a desired trajectory is obtained by solving the problem; Check whether the reference trajectory sent by the upper layer is within the drivable area. If it is detected that the reference trajectory deviates from the drivable area, adjust the weights of the sampling point set and the quadratic programming problem to correct the expected trajectory.

2. The dynamic correction method for automatic driving path planning according to claim 1, characterized in that: The steps of obtaining the point cloud data of the laser radar and the image data of the visual camera and determining the drivable area of ​​the current vehicle include: Obtaining point cloud data of the laser radar, and determining obstacle information according to the point cloud data, wherein the obstacle information includes the shape, position and distance information of the obstacle; Acquire image data from a visual camera, and analyze the image data using image processing technology to determine semantic information of obstacles; The obstacle information and the semantic information are combined to determine a cost value corresponding to each grid in the grid map, a passable grid is determined according to the cost value, and a continuous drivable area is formed based on the passable grid.

3. The dynamic correction method for automatic driving path planning according to claim 1, characterized in that: The steps of constructing a grid map with composite weights include: The calculation formula of the composite weight of the raster map is: G i,j =w s ||a i,j -a i+1,k ||2+w r ||a i+1,k -r i+1,j ||2-w d ||a i+1,k -d i+1 ||2, Among them, G i,j represents the weight value of the grid point in the i-th row and j-th column, a i,j represents the coordinates of the grid point in the i-th row and j-th column, a i+1,k represents the coordinates of the grid point in the i+1th row and the kth column, r represents the reference trajectory of the desired vehicle sent by the upper control terminal, and r i+1,j Indicates that the grid point in row i+1 and column j is closest to the reference trajectory, d i+1 represents the grid point occupied by the obstacle in the i+1th row, w s represents the smoothing weight, w r represents the reference trajectory weight, w d represents the obstacle weight, ||a i,j -a i+1,k ||2 represents the two-norm.

4. The dynamic correction method for automatic driving path planning according to claim 3, characterized in that: The step of searching for a global reference path based on the grid map by using the Dijkstra algorithm includes: The minimum cost path is obtained by searching through the Dijkstra algorithm, and the minimum cost path of the current cycle is compared with the minimum cost path of the previous cycle to determine whether the change in trajectory cost between adjacent cycles exceeds the threshold; When the trajectory cost change of the minimum cost path of the adjacent cycle exceeds the threshold, the minimum cost path of this time is output as the global reference path; When the trajectory cost change of the minimum cost path of the adjacent cycle is less than the threshold, the minimum cost path of the previous cycle is output as the global reference path.

5. The dynamic correction method for automatic driving path planning according to claim 1, characterized in that: The step of fitting the sampling point set to obtain a spline curve comprises: According to the sampling points in the obtained sampling point set, a cubic spline curve is fitted to solve the spline coefficients to obtain the spline curve; wherein the spline coefficients are obtained by solving the interpolation conditions, continuity conditions or boundary conditions.

6. The method for dynamic correction of automatic driving path planning according to claim 5, characterized in that: The step of converting the drivable area into a convex space by using a scattered point sampling method based on the spline curve to obtain a convex space boundary of the drivable area comprises: Locate the coordinates on the Cartesian coordinate system, scatter points on both sides of the normal of the spline curve, and expand the point set along the direction of the vehicle's travel; Connect the boundary points of the i-th scattering point set with the boundary points of the i+1-th scattering point set and extend them, determine the intersection point of the extension line and the i+2-th normal scattering line segment, and determine whether the intersection point is in the drivable area; When the intersection point is within the drivable area, it is considered that there is a concave space in the original drivable area, and the boundary of the drivable area corresponding to the i+2th normal scattering line segment is updated using the intersection point to obtain the convex space boundary of the drivable area.

7. The dynamic correction method for automatic driving path planning according to claim 1, characterized in that: Based on the convex space boundary, a quadratic programming problem is constructed, and the steps of solving the desired trajectory include: Construct discrete state equations for trajectory planning; Based on the convex space boundary and state space constraints, determine the optimization objective function and constraints of trajectory planning; Substituting the optimization objective function and constraint conditions into the discrete state equation to obtain a quadratic programming problem; The desired trajectory is obtained by solving the quadratic programming problem using the QP solver.

8. The method for dynamic correction of automatic driving path planning according to claim 7, characterized in that: If it is detected that the reference trajectory deviates from the drivable area, the steps of adjusting the sampling point set and the weight of the quadratic programming problem to correct the expected trajectory include: When the reference trajectory deviates from the drivable area, the point density is increased in the drivable area to expand the convex space boundary of the drivable area. Sampling is performed according to the midpoints of the boundary points on both sides of the expanded convex space, and the sampling points are fitted with a cubic spline curve to obtain the midline of the convex space; The weight value of the optimization objective function of the quadratic programming problem is modified based on the global reference path and the center line of the convex space to obtain the modified expected trajectory; The corrected desired trajectory is subjected to filtering and smoothing processing in time and space, and the smoothed desired trajectory is output.

9. A dynamic correction device for automatic driving path planning, characterized in that: The dynamic correction device is used to execute the dynamic correction method for automatic driving path planning according to any one of claims 1 to 8, and the dynamic correction device includes: The traffic area determination module is used to obtain the point cloud data of the laser radar and the image data of the visual camera to determine the current drivable area of ​​the vehicle; A path planning module is used to construct a grid map with composite weights, and obtain a global reference path based on the grid map through a Dijkstra algorithm search; A convex spatialization module is used to determine a sampling point set of the current vehicle distance from the global reference path by using a dynamic programming method, fit the sampling point set to obtain a spline curve, and perform convex spatialization on the drivable area by using a scattered point sampling method based on the spline curve to obtain a convex spatial boundary of the drivable area; A trajectory generation module, used to construct a quadratic programming problem based on the convex space boundary and solve it to obtain a desired trajectory; The trajectory correction module is used to detect whether the reference trajectory sent by the upper layer is within the drivable area. If it is detected that the reference trajectory deviates from the drivable area, the weights of the sampling point set and the quadratic programming problem are adjusted to correct the expected trajectory.

10. A dynamic correction device for automatic driving path planning, characterized in that: The dynamic correction device includes a memory, a processor and a computer program stored in the memory, and the processor executes the computer program to implement the steps of the dynamic correction method of autonomous driving path planning as described in any one of claims 1 to 8.

Images