Complex road surface path planning method, system and equipment based on B-spline curved surface and medium

Abstract

The invention discloses a complex road surface path planning method, system and device based on a B-spline curved surface and a medium, and relates to the field of path planning, the method comprises the following steps: obtaining perception information and positioning information of a vehicle, and generating a point cloud map; determining a control point according to the point cloud map, and fitting to obtain a B spline surface; performing global path planning on the B spline surface by adopting an improved A * algorithm to generate an initial B spline curve; a non-uniform B-spline curve is used as a decision variable, the smoothness of an initial B-spline curve is considered, and a local path optimization objective function is constructed through the time of the initial B-spline curve, the deviation degree of a B-spline surface and a point cloud overall terrain assessment value; and optimizing the initial B-spline curve based on the local path optimization objective function to generate a non-uniform B-spline curve, namely a planned complex road surface path. According to the invention, a smooth and continuous path with a high road surface fitting degree is constructed, and planning of a complex road surface path is realized.

Description

Technical Field

[0001] The present application relates to the field of path planning, and in particular to a method, system, device and medium for complex road path planning based on B-spline surfaces. Background Art

[0002] With the improvement of science and technology, the performance of sensors has become more and more mature, accompanied by the rapid development of autonomous driving technology. The research results of the path planning module in unmanned system navigation have also been increasingly applied to real life, such as sweeping robots, unmanned express delivery vehicles, and navigation robots in shopping malls or hotels. However, the above technologies all use the unmanned autonomous navigation path planning module on structured roads, that is, two-dimensional path planning.

[0003] In the face of practical needs, more and more unmanned vehicles will be applied to complex and rugged field environments. In the face of rugged environments, unmanned autonomous navigation will face unprecedented problems in the above examples. For example, in the military field, off-road vehicles need to perform tasks in complex terrains, such as patrolling, searching, and reconnaissance. Similarly, tasks such as resource exploration and disaster relief require unmanned vehicles to be able to plan appropriate paths in rugged terrains for exploration and search tasks. However, the above two-dimensional path planning technology is obviously not competent when facing complex and rugged roads. Summary of the invention

[0004] The purpose of this application is to provide a complex road path planning method, system, equipment and medium based on B-spline surfaces, which can construct a smooth, continuous path with a high degree of road surface fit.

[0005] To achieve the above objectives, this application provides the following solutions:

[0006] In a first aspect, the present application provides a complex road path planning method based on a B-spline surface, characterized in that the complex road path planning method based on a B-spline surface comprises:

[0007] Obtain the vehicle's perception information and positioning information, and generate a point cloud map;

[0008] Determine the control points according to the point cloud map, and obtain a B-spline surface by fitting;

[0009] Based on the point cloud map and the B-spline surface, an improved A* algorithm is used to perform global path planning on the B-spline surface to generate an initial B-spline curve; the G value of the improved A* algorithm takes into account the distance, slope and terrain fit; the H value of the improved A* algorithm takes into account the distance and terrain fit;

[0010] Taking the non-uniform B-spline curve as the decision variable, the smoothness of the initial B-spline curve, the time to pass the initial B-spline curve, the degree of deviation of the B-spline surface and the overall terrain evaluation value of the point cloud are considered to construct the objective function equation of the local path optimization.

[0011] The initial B-spline curve is optimized based on the local path optimization objective function to generate a non-uniform B-spline curve, where the non-uniform B-spline curve is the planned complex road path.

[0012] Optionally, determining control points according to the point cloud map and fitting to obtain a B-spline surface specifically includes:

[0013] The point cloud map is divided into blocks of equal proportions in the X direction and the Y direction to obtain multiple grids, each of which includes multiple coordinate points;

[0014] For each grid, calculate the coordinates of each coordinate point;

[0015] Determine a control point in each grid based on the coordinates of each coordinate point in each grid;

[0016] The B-spline surface is fitted based on the control points in each grid.

[0017] Optionally, based on the point cloud map and the B-spline surface, an improved A* algorithm is used to perform global path planning on the B-spline surface to generate an initial B-spline curve, including:

[0018] Based on the point cloud map and the B-spline surface, the improved A* algorithm is used to search for each node in the B-spline surface, and the G value and H value of each node are calculated; one node corresponds to one coordinate point;

[0019] Path nodes are determined based on the G value and H value of each node, and an initial B-spline curve is generated based on the path nodes.

[0020] Optionally, the calculation formula of the G value of each node is:

[0021] G(n)=λ 1 ×g 1 +λ 2 ×g 2 +λ 3 ×g 3 ;

[0022]

[0023] Among them, G(n) is the G value of the nth node; g 1 is the distance from the starting point to the nth node, g 2 is the slope from the starting point to the nth node, g 3is the degree of terrain fit from the starting point to the nth node, λ 1 , 2 , 3 are weight values, a i is the three-dimensional coordinate of the i-th node; R m is an m-dimensional real number field, where m is 3; x i is the horizontal axis coordinate value of the i-th node; y i is the vertical axis coordinate value of the i-th node; z i is the Z-axis coordinate value of the i-th node; Z bi is the Z coordinate value of the ith node in the B-spline surface; Z ri is the actual Z coordinate value of the i-th node in the point cloud map.

[0024] Optionally, the calculation formula of the H value of each node is:

[0025]

[0026] Where H(n) is the H value of the nth node; d is the target path node; a j is the three-dimensional vector of the jth node; R m is an m-dimensional real number field, where m is 3; R is a one-dimensional real number field; Z bj is the Z coordinate value of the jth node in the B-spline surface; Z rj is the actual Z coordinate value of the jth node in the point cloud map.

[0027] Optionally, the local path optimization objective function equation is:

[0028] minJ(q,s)=λ s J s +λ t J t +λ d J d +λ m J m ;

[0029]

[0030] Where J(q,s) is the evaluation value of the non-uniform B-spline curve; q is the control point of the non-uniform B-spline curve, which is N in total; s is the node of the non-uniform B-spline curve, which is N in total; J s is the smoothness of the initial B-spline curve; J t is the time to pass the initial B-spline curve; J d is the deviation degree of B-spline surface; J m is the overall terrain evaluation value of the point cloud; s , t , d, m are weight values; l (2) is the smoothness of the lth control point of the initial B-spline curve; R k is a k-dimensional real field vector, where k is 3; T l is the time to pass the lth control point of the initial B-spline curve; Indicates that J t Not related to control point q; Z rl is the actual Z coordinate value of the lth point in the point cloud map; Z bl is the Z coordinate value of the lth control point of the initial B-spline curve; R is a one-dimensional real number field; ω 1 ,ω 2 ,ω 3 All are weight values; The highest Z-axis coordinate value of the lth point in the point cloud map; The lowest Z-axis coordinate value of the lth point in the point cloud map; is the slope of the lth point in the point cloud map; is the point cloud sparsity of the lth point in the point cloud map.

[0031] Optionally, the constraint condition of the local path optimization objective function is:

[0032]

[0033] in, is the first-order derivative of the speed and angular velocity of the point passing through the non-uniform B-spline curve in the x, y, and z directions with respect to time, with a total of 6 dimensions; Φ is the speed and angular velocity of the point passing through the non-uniform B-spline curve in the x, y, and z directions, with a total of 6 dimensions; u max is a constant, which is used to constrain the six-dimensional vector of the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions; Φ(t)-Ψ| is the distance from the point on the non-uniform B-spline curve to the control point on the B-spline surface; Φ(t) is the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions at time t; Ψ is the control point on the B-spline surface; p d is a constant; is the time value of t;

[0034] The points of the non-uniform B-spline curve include control points of the non-uniform B-spline curve and nodes of the non-uniform B-spline curve.

[0035] In a second aspect, the present application provides a complex road path planning system based on a B-spline surface, wherein the complex road path planning system based on a B-spline surface is applied to any of the complex road path planning methods based on a B-spline surface described above, and the complex road path planning system based on a B-spline surface comprises:

[0036] A perception module, used to obtain the perception information of the vehicle;

[0037] A positioning module, used to obtain the positioning information of the vehicle;

[0038] The point cloud map generation module is connected to the perception module and the positioning module respectively, and is used to fuse the perception information and positioning information of the vehicle to generate a point cloud map;

[0039] A B-spline surface generation module, connected to the point cloud map generation module, is used to determine control points according to the point cloud map and obtain a B-spline surface by fitting;

[0040] The global path planning module is connected to the point cloud map generation module and the B-spline surface generation module respectively, and is used to perform global path planning on the B-spline surface according to the point cloud map and the B-spline surface using an improved A* algorithm to generate an initial B-spline curve; the G value of the improved A* algorithm takes into account the distance, slope and terrain fit; the H value of the improved A* algorithm takes into account the distance and terrain fit;

[0041] The local path planning module is connected to the global path planning module, and is used for taking the non-uniform B-spline curve as a decision variable, considering the smoothness of the initial B-spline curve, the time passing through the initial B-spline curve, the degree of deviation of the B-spline surface, and the overall terrain evaluation value of the point cloud, to construct a local path optimization objective function equation; based on the local path optimization objective function, the initial B-spline curve is optimized to generate a non-uniform B-spline curve, and the non-uniform B-spline curve is the planned complex road path.

[0042] In a third aspect, the present application provides a computer device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the complex road path planning methods based on B-spline surfaces described above.

[0043] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements any of the above-mentioned complex road path planning methods based on B-spline surfaces.

[0044] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0045] The present application provides a complex road path planning method, system, device and medium based on B-spline surface. The global path planning is performed on the B-spline surface through the G value and H value of the improved A* algorithm. Since the G value and H value take into account the distance information, slope information and terrain fit information, a three-dimensional B-spline curve for local path planning is generated. The control points and nodes of the non-uniform B-spline curve are further determined through the local path optimization objective function, and the non-uniform B-spline curve is generated. A smooth and continuous path with a high degree of road fit is constructed, thereby completing the planning of the complex road path. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0047] Figure 1 A schematic diagram of a complex road surface where a vehicle is traveling in a wild desert environment;

[0048] Figure 2 Schematic diagram of another complex road surface of a vehicle in a wild desert environment;

[0049] Figure 3 A schematic diagram of a flow chart of a complex road path planning method based on a B-spline surface in an embodiment of the present application;

[0050] Figure 4 for Figure 4 The point cloud map generated in step 1;

[0051] Figure 5 It is a schematic diagram of the convex hull property of the B-spline surface wrapped by the control points when it has a slope;

[0052] Figure 6 It is a schematic diagram of the convex hull property of the B-spline surface wrapped by the control points without slope;

[0053] Figure 7 A schematic diagram of the functional modules of a complex road path planning system based on B-spline surfaces provided in one embodiment of the present application;

[0054] Figure 8 It is a complete schematic diagram of the unmanned system structure;

[0055] Fig. 9 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. Description of the drawings:

[0057] Perception module-1, positioning module-2, point cloud map generation module-3, B-spline surface generation module-4, global path planning module-5, local path planning module-6, planning module-7, control module-8, chassis-9. DETAILED DESCRIPTION

[0058] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only 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.

[0059] For existing path planning, Carnegie Mellon University proposed a method of constructing an initial solution of a plane map and solving the local planning path by constructing an optimal control equation. The basic principle is that the terrain under three-dimensional rugged road conditions has great uncertainty. Therefore, under the premise of having a two-dimensional plane trajectory library, an optimization equation is constructed, and the gradient descent method is used to optimize the solution near the initial path. The state is estimated using the state equation based on the chassis model of the vehicle, and finally a path under three-dimensional road conditions is inferred. However, there is a problem here. Since the optimal control is selected from the solution obtained in the two-dimensional plane, after adding the interference of uncertain factors under three-dimensional rugged road conditions, the actual curve obtained is not continuous, but a curve subject to environmental factors.

[0060] Dalian University of Technology proposed a passability assessment method based on B-spline surfaces. The basic principle is that after generating relevant point cloud raw data, the control points of the point cloud data are selected to generate B-spline surfaces, and the B-spline surfaces are used to solve information such as the slope of the terrain to evaluate the passability information. However, this is only a means of constructing a passability map, which is equivalent to generating a passability cost map, but it does not propose a way to generate a continuous path planning curve under three-dimensional rugged road conditions.

[0061] Unmanned vehicles and robots need to perform three-dimensional path planning in actual exploration and inspection tasks, such as in mines, oil fields, forests, etc. However, ordinary two-dimensional path planning cannot adapt well to terrain changes. Here, we take the field survey environment as an example for analysis. Figure 1 and Figure 2 shown. Figure 1 and Figure 2A complex road surface in a wild desert environment is shown. The complex road surface has the characteristics of unstructured road surface, rugged hard road surface, and high complexity. The application scenarios of this application are mainly faced with three-dimensional rugged road conditions like the following. In this case, path planning is the problem we need to solve.

[0062] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0063] In an exemplary embodiment, Figure 3 As shown, a complex road path planning method based on B-spline surface is provided, including:

[0064] Step 1: Obtain the vehicle's perception information and positioning information and generate a point cloud map, such as Figure 4 shown.

[0065] Step 2: Determine the control points based on the point cloud map and fit the B-spline surface.

[0066] Step 3: Based on the point cloud map and the B-spline surface, the improved A* algorithm is used to perform global path planning on the B-spline surface to generate an initial B-spline curve. The G value of the improved A* algorithm takes into account the distance, slope information and terrain fit; the H value of the improved A* algorithm takes into account the distance and terrain fit.

[0067] Step 4: Taking the non-uniform B-spline curve as the decision variable, considering the smoothness of the initial B-spline curve, the time to pass the initial B-spline curve, the degree of deviation of the B-spline surface, and the overall terrain evaluation value of the point cloud, the local path optimization objective function is constructed.

[0068] Step 5: Optimize the initial B-spline curve based on the local path optimization objective function to generate a non-uniform B-spline curve, which is the planned complex road path.

[0069] In a specific embodiment, step 2 specifically includes:

[0070] Step 21: Divide the point cloud map into blocks of equal proportions in the X and Y directions to obtain a plurality of grids, each of which includes a plurality of coordinate points.

[0071] Step 22: For each grid, calculate the coordinates of each coordinate point.

[0072] Step 23: Determine the control point in each grid based on the coordinates of each coordinate point in each grid.

[0073] Step 24: The B-spline surface is obtained by fitting the control points in each grid. The specific formula is:

[0074]

[0075] Among them, q b is the three-dimensional vector of the control points of the B-spline surface; N b is the total number of coordinate points in each grid; b v is the three-dimensional vector of the control points of the vth B-spline surface; R 3 is a three-dimensional field of real numbers.

[0076] like Figure 5 and Figure 6 As shown, Figure 5 It is a schematic diagram of the convex hull property of the B-spline surface wrapped by the control points when it has a slope. Figure 6 The diagram is a diagram of the convex hull property of a B-spline surface wrapped by control points when there is no slope. A B-spline surface is a surface uniquely determined by control points, nodes and order, and a B-spline curve will exhibit a convex hull property wrapped by control points when the control points are determined. This property is one of the main cost values ​​for constructing the A* algorithm cost term in the global planning layer of this application, and is also one of the main optimization items for constructing the objective function in the local planning optimization problem of this application.

[0077] In the case of rough roads with large slopes, the gap between the B-spline surface and the control points (i.e., the actual point cloud values) is larger, while on roads with relatively gentle slopes, the gap between the B-spline surface and the control points is smaller. This feature can be used to characterize the degree of terrain fit between the path obtained by our path planning and the actual road surface, and is also the main optimization item introduced in this application. This indicator well characterizes the fit between the path and the terrain, making it easier for the vehicle to meet certain continuity and fit requirements on this basis after subsequent control. It is also one of the differences between this application and other path planning methods under B-spline surfaces.

[0078] In a specific embodiment, step 3 specifically includes:

[0079] Step 31: Based on the point cloud map and the B-spline surface, the improved A* algorithm is used to search for each node in the B-spline surface, and the G value and H value of each node are calculated; one node corresponds to one coordinate point.

[0080] Step 32: Determine the path nodes based on the G value and H value of each node, and generate an initial B-spline curve based on the path nodes.

[0081] Specifically, in the search process of the A* algorithm, it is necessary to expand the nodes one by one. The G value represents the cost (or penalty, which can be understood as the distance) from the starting point to the current point, and the H value represents the predicted value from the current point to the end point, which is a heuristic function. The significance of the heuristic is to guide the expansion direction of the A* algorithm so that the current point can diffuse to the target point in a more direct way. Among them, the G value takes into account the distance information, slope information and terrain fit information. The H value only considers the distance of the terrain and the fit of the terrain, because in the process of constructing the point cloud environment, the point cloud information is incomplete, so the slope information is not yet complete. Finally, under the effect of the improved A* algorithm, the present application can generate a rough path, i.e., a B-spline curve, at the global path planning level that takes into account the distance, slope and terrain fit.

[0082] The calculation formula of the G value of each node is:

[0083] G(n)=λ 1 ×g 1 +λ 2 ×g 2 +λ 3 ×g 3 (2)

[0084]

[0085] Among them, G(n) is the G value of the nth node; g 1 is the distance from the starting point to the nth node, g 2 is the slope from the starting point to the nth node, g 3 is the degree of terrain fit from the starting point to the nth node, λ 1 , 2 , 3 are weight values, a i is the three-dimensional coordinate of the i-th node; R m is an m-dimensional real number field, where m is 3; x i is the horizontal axis coordinate value of the i-th node; y i is the vertical axis coordinate value of the i-th node; z i is the Z-axis coordinate value of the i-th node; Z bi is the Z coordinate value of the ith node in the B-spline surface; Z ri is the actual Z coordinate value of the i-th node in the point cloud map.

[0086] The calculation formula of the H value of each node is:

[0087]

[0088] Where H(n) is the H value of the nth node; d is the target path node; a jis the three-dimensional vector of the jth node; R m is an m-dimensional real number field, where m is 3; R is a one-dimensional real number field; Z bj is the Z coordinate value of the jth node in the B-spline surface; Z rj is the actual Z coordinate value of the jth node in the point cloud map.

[0089] In a specific embodiment, step 4 specifically includes:

[0090] The decision variables of the non-uniform B-spline curve are:

[0091] Q m ∈R k(M-1) (7)

[0092] S(t)∈R k+pb+1 (8)

[0093] Φ(t)=S(t) T M pb+1 Q m (9)

[0094]

[0095] Among them, Q m is the control point matrix; R k(M-1) is a K(M-1)-dimensional real number field; S(t) is a node matrix; R k+pb+1 is the real number domain of K+pb+1; Φ(t) is the velocity and angular velocity of the point in the x, y, and z directions of the vehicle passing through the non-uniform B-spline curve at time t; S(t) T is the transposed matrix; M pb+1 is a constant matrix of the same order as s(t); s(t) is the three-dimensional vector of the node; s pb (t) is s(t) of the Pbth order; q m is the three-dimensional vector of the mth control point.

[0096] The decision variable shows that the decision variable of the present application is a non-uniform B-spline curve. The purpose of selecting a B-spline curve is to better fit the characteristics of the B-spline surface, and finally plan a non-uniform B-spline curve that is more suitable for the application of the B-spline surface. Among them, a non-uniform B-spline curve is uniquely determined by the q vector and the s vector, and these two items represent the control points and nodes of the non-uniform B-spline curve respectively, and the order is determined to be 3. After determining the control points, nodes and order, a non-uniform B-spline curve can be directly determined. Due to the properties of non-uniform B-splines, the smoothness and speed change curve of the B-spline curve can be determined on this basis by considering non-uniform changes.

[0097] The objective function equation of local path optimization is:

[0098] minJ(q,s)=λ s J s +λ t J t +λ d J d +λ m J m (12)

[0099]

[0100] Where J(q,s) is the evaluation value of the non-uniform B-spline curve; q is the control point of the non-uniform B-spline curve, which is M in total; s is the node of the non-uniform B-spline curve, which is N in total, where M and N are equal; J s is the smoothness of the initial B-spline curve; J t is the time to pass the initial B-spline curve; J d is the deviation degree of B-spline surface; J m is the overall terrain evaluation value of the point cloud; s , t , d , m are weight values; l (2) for y l The second derivative with respect to time, y l is the smoothness of the lth control point of the initial B-spline curve; R k is a k-dimensional real field vector, where k is 3; T l is the time to pass the lth control point of the initial B-spline curve; Indicates that J t Not related to control point q; Z rl is the actual Z coordinate value of the lth point in the point cloud map; Z bl is the Z coordinate value of the lth control point of the initial B-spline curve; R is a one-dimensional real number field; ω 1 ,ω 2 ,ω 3 All are weight values; The highest Z-axis coordinate value of the lth point in the point cloud map; The lowest Z-axis coordinate value of the lth point in the point cloud map; is the slope of the lth point in the point cloud map; is the point cloud sparsity of the lth point in the point cloud map.

[0101] Formulas (12) to (16) are the objective function equations of local path optimization, which mainly contain four major items: the smoothness of the B-spline curve, the time to pass the B-spline curve, the degree of deviation of the B-spline surface, and the overall terrain evaluation of the point cloud.

[0102] In the smoothness of the B-spline curve, the present application considers the sum of the path on the second-order derivative. The larger the sum of the second-order derivatives, the smaller the change in the smoothness of the B-spline curve in the first-order derivative, and the larger the sum of the second-order derivatives, the greater the change in the smoothness of the B-spline curve in the first-order derivative. The magnitude of this change characterizes the degree of sudden change in the smoothness of the B-spline curve at the speed level, and then determines whether the non-uniform B-spline curve generated by the present application is smooth and continuous; the time to pass the B-spline curve, that is, the time required for the vehicle to reach the destination on the B-spline curve; the degree of deviation from the B-spline surface refers to the evaluation of the degree of terrain fit after the introduction of the B-spline surface in the present application, which characterizes the degree of deviation between the actual control point and the B-spline surface; the overall terrain evaluation of the point cloud, such as terrain roughness, slope, point cloud sparsity, etc., these are all basic features of the point cloud map, and a larger penalty is given in places where the point cloud is sparse; finally, a suitable weight value λ can be selected s , t , d , m Standardize to achieve weight balance.

[0103] The constraints of the local path optimization objective function are:

[0104]

[0105] in, is the first-order derivative of the speed and angular velocity of the point passing through the non-uniform B-spline curve in the x, y, and z directions with respect to time, with a total of 6 dimensions; Φ is the speed and angular velocity of the point passing through the non-uniform B-spline curve in the x, y, and z directions, with a total of 6 dimensions; u max is a constant, which is used to constrain the six-dimensional vector of the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions; Φ(t)-Ψ| is the distance from the point on the non-uniform B-spline curve to the control point on the B-spline surface; Φ(t) is the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions at time t; Ψ is the control point on the B-spline surface; p d is a constant; is the time value of t.

[0106] The points of the non-uniform B-spline curve include control points of the non-uniform B-spline curve and nodes of the non-uniform B-spline curve.

[0107] The constraints consist of two items. One is the speed angle and speed limit of the car model, that is, the speed and angular velocity of the vehicle in the x, y, and z directions under three-dimensional rugged road conditions, with a total range of 6 dimensions. In different dimensions, this application converts this process into changes in the vehicle coordinate system through the transformation of the state equation and then defines it. The second item of the constraint is the solution space of the planned non-uniform B-spline curve. This application stipulates the value obtained by this B-spline curve on the previously generated B-spline surface, which effectively reduces the solution space of the non-uniform B-spline curve, thereby obtaining a smooth and continuous non-uniform B-spline curve. Under the constraints, the decision variable corresponding to the moment when the objective function reaches the minimum is its optimal path.

[0108] Advantages of this application:

[0109] (1) In the global path planning, the present application improves the G value and H value of the A* algorithm for path planning under the B-spline surface, so that it conforms to the characteristics of the B-spline surface response, such as slope, terrain fit, distance and other information, so that it conforms to the characteristics of three-dimensional rugged road conditions, without causing the vehicle to fall into a difficult-to-solve local optimal solution during the local planning process.

[0110] (2) In the local path planning layer of the present application, based on the optimization equation of the local path, including the local path optimization objective function equation, decision variables and constraints, the non-uniform B-spline curve is used as the decision variable, which helps to solve the required continuous curve in three-dimensional space according to the characteristics of the B-spline surface. The local path optimization objective function equation is used to determine the target to be solved. When the constraints are met, the decision variable corresponding to the moment when the objective function reaches the minimum is the optimal path.

[0111] (3) Three-dimensional path planning allows unmanned vehicles to better traverse and adapt to complex terrain, improving reliability and flexibility in these environments, and helping to improve resource utilization efficiency, reduce human risks, and optimize exploration and inspection paths.

[0112] In an exemplary embodiment, Figure 7 As shown in FIG, a complex road path planning system based on B-spline surface is provided, which is applied to Figure 8 Unmanned systems as shown. Figure 8As shown, a complete unmanned system should at least include a perception module 1, a positioning module 2, a planning module 7 and a control module 8. The planning module 7 generally includes a task control module, a global path planning module 5 and a local path planning module 6. The focus of this application is on the innovation of the planning module 7, and the construction of the global path planning module 5 and the local path planning module 6 is the main work object of this application. The task control module is currently intended to be implemented based on a state machine, which divides the vehicle's decision into multiple subtasks and makes judgments on the environment according to different road conditions.

[0113] The task control module can also be called the behavior decision module. If the behavior decision module is further considered, the interaction between humans and robots, and the interaction between the environment and robots can be taken into account. However, this behavior decision module does not consider the premise of multiple interactions. Therefore, related technologies such as behavior decision are not used. The technology used only includes the implementation of the state machine, which enables the vehicle to make some behavior changes at the right time, such as starting, braking, reaching the target point, etc. Some simple action conversions are involved.

[0114] Other modules include perception module 1, positioning module 2 and control module 8. The development of these three modules is mainly constructed using existing open source algorithms. Perception module 1 and positioning module 2 can find the corresponding slam algorithm to build, generate a point cloud map, and carry it into the actual framework, including the hardware deployment of sensors, etc., where perception module 1 can be a laser radar camera and other equipment; positioning module 2 can contain Odom odometer information. The control module 8 puts the planned complex road path into the chassis 9, and obtains its output results to achieve control. The output results include linear speed and wheel angle, etc. Existing control algorithms such as LQR and MPC algorithms are relatively mature. After completing the development of perception module 1, positioning module 2, planning module 7 and control module 8 based on the Ros communication framework, simulation verification can be carried out to verify the complex road path planning method based on B-spline surface proposed in this application.

[0115] The complex road path planning system based on B-spline surface includes:

[0116] The perception module 1 is used to obtain the perception information of the vehicle.

[0117] The positioning module 2 is used to obtain the positioning information of the vehicle.

[0118] The point cloud map generation module 3 is connected to the perception module 1 and the positioning module 2 respectively, and is used to fuse the perception information and positioning information of the vehicle to generate a point cloud map.

[0119] The B-spline surface generation module 4 is connected to the point cloud map generation module 3 and is used to determine the control points according to the point cloud map and obtain the B-spline surface by fitting.

[0120] The global path planning module 5 is connected to the point cloud map generation module 3 and the B-spline surface generation module 4 respectively, and is used to perform global path planning on the B-spline surface according to the point cloud map and the B-spline surface using the improved A* algorithm to generate an initial B-spline curve; the G value of the improved A* algorithm takes into account the distance, slope and terrain fit; the H value of the improved A* algorithm takes into account the distance and terrain fit.

[0121] The local path planning module 6 is connected to the global path planning module 5, and is used for taking the non-uniform B-spline curve as a decision variable, considering the smoothness of the initial B-spline curve, the time passing through the initial B-spline curve, the degree of deviation of the B-spline surface, and the overall terrain evaluation value of the point cloud, to construct a local path optimization objective function equation; based on the local path optimization objective function, the initial B-spline curve is optimized to generate a non-uniform B-spline curve, and the non-uniform B-spline curve is the planned complex road path.

[0122] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Fig. 9 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer 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 database of the computer device is used to store data to be processed. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a complex road path planning method for a B-spline surface is implemented.

[0123] Those skilled in the art will understand that Fig. 9The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components. In an exemplary embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps in the above-mentioned method embodiments when executing the computer program.

[0124] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0125] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0126] Those of ordinary skill 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 the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0127] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0128] 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.

[0129] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A complex road path planning method based on B-spline surface, characterized in that: The complex road path planning method based on B-spline surface includes: Obtain the vehicle's perception information and positioning information, and generate a point cloud map; Determine the control points according to the point cloud map, and obtain a B-spline surface by fitting; Based on the point cloud map and the B-spline surface, an improved A* algorithm is used to perform global path planning on the B-spline surface to generate an initial B-spline curve; the G value of the improved A* algorithm takes into account the distance, slope and terrain fit; the H value of the improved A* algorithm takes into account the distance and terrain fit; Taking the non-uniform B-spline curve as the decision variable, the smoothness of the initial B-spline curve, the time to pass the initial B-spline curve, the degree of deviation of the B-spline surface and the overall terrain evaluation value of the point cloud are considered to construct the local path optimization objective function. The initial B-spline curve is optimized based on the local path optimization objective function to generate a non-uniform B-spline curve, where the non-uniform B-spline curve is the planned complex road path.

2. The complex road path planning method based on B-spline surface according to claim 1 is characterized in that: Determining control points according to the point cloud map and fitting a B-spline surface specifically includes: The point cloud map is divided into blocks of equal proportions in the X direction and the Y direction to obtain multiple grids, each of which includes multiple coordinate points; For each grid, calculate the coordinates of each coordinate point; Determine a control point in each grid based on the coordinates of each coordinate point in each grid; The B-spline surface is fitted based on the control points in each grid.

3. The complex road path planning method based on B-spline surface according to claim 1 is characterized in that: Based on the point cloud map and the B-spline surface, an improved A* algorithm is used to perform global path planning on the B-spline surface to generate an initial B-spline curve, including: The improved A* algorithm is used to search each node in the B-spline surface and calculate the G value and H value of each node; one node corresponds to one coordinate point; Path nodes are determined based on the G value and H value of each node, and an initial B-spline curve is generated based on the path nodes.

4. The complex road path planning method based on B-spline surface according to claim 3 is characterized in that: The calculation formula of the G value of each node is: G(n)=λ1×g1+λ2×g2+λ3×g3; Among them, G(n) is the G value of the nth node; g1 is the distance from the starting point to the nth node, g2 is the slope from the starting point to the nth node, g3 is the degree of terrain fit from the starting point to the nth node, λ1, λ2, and λ3 are all weight values, and a i is the three-dimensional coordinate of the i-th node; R m is an m-dimensional real number field, where m is 3; x i is the horizontal axis coordinate value of the i-th node; y i is the vertical coordinate value of the i-th node; z i is the Z-axis coordinate value of the i-th node; Z bi is the Z coordinate value of the ith node in the B-spline surface; Z ri is the actual Z coordinate value of the i-th node in the point cloud map.

5. The complex road path planning method based on B-spline surface according to claim 3 is characterized in that: The calculation formula of the H value of each node is: Where H(n) is the H value of the nth node; d is the target path node; a j is the three-dimensional vector of the jth node; R m is an m-dimensional real number field, where m is 3; R is a one-dimensional real number field; Z bj is the Z coordinate value of the jth node in the B-spline surface; Z rj is the actual Z coordinate value of the jth node in the point cloud map.

6. The complex road path planning method based on B-spline surface according to claim 1 is characterized in that: The local path optimization objective function is: minJ(q,s)=λ s J s +λ t J t +λ d J d +λ m J m ; Where J(q,s) is the evaluation value of the non-uniform B-spline curve; q is the control point of the non-uniform B-spline curve; s is the node of the non-uniform B-spline curve; J s is the smoothness of the initial B-spline curve; J t is the time to pass the initial B-spline curve; J d is the deviation degree of the B-spline surface; J m is the overall terrain evaluation value of the point cloud; s , t , d , m are weight values; l (2) is the smoothness of the lth control point of the initial B-spline curve; R k is a k-dimensional real field vector, where k is 3; T l is the time to pass the lth control point of the initial B-spline curve; Indicates that J t Not related to control point q; Z rl is the actual Z coordinate value of the lth point in the point cloud map; Z bl is the Z coordinate value of the lth control point of the initial B-spline curve; R is a one-dimensional real number field; ω1, ω2, ω3 are all weight values; The highest Z-axis coordinate value of the lth point in the point cloud map; The lowest Z-axis coordinate value of the lth point in the point cloud map; is the slope of the lth point in the point cloud map; is the point cloud sparsity of the lth point in the point cloud map.

7. The complex road path planning method based on B-spline surface according to claim 1 is characterized in that: The constraints of the local path optimization objective function are: in, is the first-order derivative of the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions with respect to time; Φ is the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions; u max is a constant, which is used to constrain the six-dimensional vector of the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions; |Φ(t)-Ψ| is the distance from the point on the non-uniform B-spline curve to the control point on the B-spline surface; Φ(t) is the speed and angular velocity of the point on the non-uniform B-spline curve in the x, y, and z directions at time t; Ψ is the control point on the B-spline surface; p d is a constant; is the time value of t; The points of the non-uniform B-spline curve include control points of the non-uniform B-spline curve and nodes of the non-uniform B-spline curve.

8. A complex road path planning system based on B-spline surface, characterized in that: The complex road path planning system based on B-spline surface is applied to the complex road path planning method based on B-spline surface described in any one of claims 1 to 7, and the complex road path planning system based on B-spline surface includes: A perception module, used to obtain the perception information of the vehicle; A positioning module, used to obtain the positioning information of the vehicle; The point cloud map generation module is connected to the perception module and the positioning module respectively, and is used to fuse the perception information and positioning information of the vehicle to generate a point cloud map; A B-spline surface generation module, connected to the point cloud map generation module, is used to determine control points according to the point cloud map and obtain a B-spline surface by fitting; The global path planning module is connected to the point cloud map generation module and the B-spline surface generation module respectively, and is used to perform global path planning on the B-spline surface according to the point cloud map and the B-spline surface using an improved A* algorithm to generate an initial B-spline curve; the G value of the improved A* algorithm takes into account the distance, slope and terrain fit; the H value of the improved A* algorithm takes into account the distance and terrain fit; The local path planning module is connected to the global path planning module, and is used for taking the non-uniform B-spline curve as a decision variable, considering the smoothness of the initial B-spline curve, the time passing through the initial B-spline curve, the degree of deviation of the B-spline surface, and the overall terrain evaluation value of the point cloud, to construct a local path optimization objective function; based on the local path optimization objective function, the initial B-spline curve is optimized to generate a non-uniform B-spline curve, and the non-uniform B-spline curve is the planned complex road path.

9. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the complex road path planning method based on B-spline surface as described in any one of claims 1-8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the complex road path planning method based on B-spline surface described in any one of claims 1-8 is implemented.

Images