A complex road surface path planning method, system, device and medium based on a B-spline surface

Abstract

The application discloses a complex road path planning method and system based on a B-spline surface, a device and a medium, and relates to the field of path planning.The method comprises the following steps: acquiring sensing 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 a B-spline surface; performing global path planning on the B-spline surface by using an improved A* algorithm, and generating an initial B-spline curve; taking a non-uniform B-spline curve as a decision variable, considering the smoothness of the initial B-spline curve, the time of passing through the initial B-spline curve, the deviation degree of the B-spline surface and the overall terrain evaluation value of the point cloud, and constructing a local path optimization objective function; and optimizing the initial B-spline curve based on the local path optimization objective function, and generating a non-uniform B-spline curve, i.e., a planned complex road path.The application constructs a smooth and continuous path with high road surface fitting degree, and realizes the planning of a complex road path.

Description

TECHNICAL FIELD

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

[0002] With the improvement of technology, the performance of sensors is becoming more and more mature, and along with it, the automatic driving technology is developing rapidly. The research results of path planning module in unmanned system navigation are also more and more applied to actual life, such as sweeping robots, unmanned express delivery vehicles, navigation robots in shopping malls or hotels, but the above technologies are all used in structured road surfaces, that is, two-dimensional path planning.

[0003] In the face of actual needs, more and more unmanned vehicles will be applied to complex and rugged outdoor 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 terrain, such as patrol, search and reconnaissance. Similarly, resource exploration, disaster rescue and other tasks require unmanned vehicles to plan suitable paths in rugged terrain for exploration and search tasks. However, the above two-dimensional path planning technology obviously cannot meet the needs when facing complex and rugged road surfaces. SUMMARY

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

[0005] To achieve the above purpose, the present application provides the following solutions:

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

[0007] Obtaining perception information and positioning information of a vehicle and generating a point cloud map;

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

[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 considers distance, slope and terrain fitting degree; the H value of the improved A* algorithm considers distance and terrain fitting degree;

[0010] The non-uniform B-spline curve is taken as a decision variable, and an initial B-spline curve is considered in terms of smoothness of the initial B-spline curve, time passing through the initial B-spline curve, deviation degree of the B-spline surface, and overall terrain evaluation value of the point cloud, so as to construct a local path optimization objective function equation;

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

[0012] Optionally, a control point is determined according to the point cloud map, and a B-spline surface is fitted, specifically including:

[0013] The point cloud map is equally divided in X and Y directions to obtain a plurality of grids, and each grid includes a plurality of coordinate points;

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

[0015] Based on the coordinates of each coordinate point in each grid, a control point in each grid is determined;

[0016] Based on the control point in each grid, a B-spline surface is fitted.

[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, an improved A* algorithm is used to search each node in the B-spline surface, and G value and H value of each node are calculated; one node corresponds to one coordinate point;

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

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

[0021] G(n)=λ1×g1+λ2×g2+λ3×g3;

[0022]

[0023] wherein 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 terrain fitting degree from the starting point to the nth node, λ1, λ2, and λ3 are weight values, a i is the three-dimensional coordinate of the ith node; R m is an m-dimensional real number field, where m is 3; x i is the horizontal axis coordinate value of the ith node; yi is the longitudinal coordinate value of the ith node; z i is the Z-axis coordinate value of the ith 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 ith 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 the m-dimensional real field, where m is 3; R is a one-dimensional real 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] min J(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, a total of N; s is the node of the non-uniform B-spline curve, a total of N; J s is the smoothness of the initial B-spline curve; J t is the time through the initial B-spline curve; J d is the B-spline surface deviation; J m is the overall terrain evaluation value of the point cloud; λ s , λ t , λ d , λ m are weight values; y l (2) is the smoothness of the lth control point of the initial B-spline curve; R k is the k-dimensional real field vector, where k is 3; T l is the time through the lth control point of the initial B-spline curve; denotes the Jt Irrelevant to the control point q; Z rl is the actual Z coordinate value of the lth point of 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 field; ω1, ω2, ω3 are weight values; is the highest Z-axis coordinate value of the lth point of the point cloud map; is the lowest Z-axis coordinate value of the lth point of the point cloud map; is the slope of the lth point of the point cloud map; is the point cloud sparsity of the lth point of the point cloud map.

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

[0032]

[0033] wherein, is the first derivative of the speed and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, z three directions with respect to time, a total of 6 dimensions; Φ is the speed and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, z three directions, a total of 6 dimensions; u max is a constant, used to constrain the six-dimensional vector of the speed and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, z three 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 vehicle passing through the point of the non-uniform B-spline curve in the x, y, z three 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 point of the non-uniform B-spline curve includes the control point of the non-uniform B-spline curve and the node of the non-uniform B-spline curve.

[0035] In a second aspect, the present application provides a complex road surface path planning system based on a B-spline surface, which is applied to the complex road surface path planning method based on a B-spline surface in any one of the above embodiments. The complex road surface path planning system based on a B-spline surface comprises:

[0036] A perception module is configured to obtain perception information of a vehicle.

[0037] A positioning module is configured to obtain positioning information of the vehicle.

[0038] A point cloud map generation module is connected with the perception module and the positioning module respectively, and is configured to fuse the perception information and the positioning information of the vehicle to generate a point cloud map.

[0039] The B-spline surface generation module is connected with the point cloud map generation module, and is configured to determine control points according to the point cloud map, and fit a B-spline surface;

[0040] The global path planning module is connected with the point cloud map generation module and the B-spline surface generation module, and is configured to perform global path planning on the B-spline surface according to the point cloud map and the B-spline surface by using an improved A* algorithm, and generate an initial B-spline curve; the G value of the improved A* algorithm considers distance, slope and terrain fitting degree; and the H value of the improved A* algorithm considers distance and terrain fitting degree.

[0041] The local path planning module is connected with the global path planning module, and is configured to take a non-uniform B-spline curve as a decision variable, consider smoothness of the initial B-spline curve, time for passing through the initial B-spline curve, B-spline surface deviation degree and point cloud overall terrain evaluation value, and construct a local path optimization objective function equation; and perform optimization on the initial B-spline curve based on the local path optimization objective function, and generate a non-uniform B-spline curve, which is a planned complex road surface 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, and the processor executes the computer program to implement the B-spline surface-based complex road surface path planning method according to any one of the above.

[0043] In a fourth aspect, the present application provides a computer readable storage medium having a computer program stored thereon, and the computer program is executed by a processor to implement the B-spline surface-based complex road surface path planning method according to any one of the above.

[0044] According to the embodiments provided in the present application, the following technical effects are disclosed:

[0045] The present application provides a B-spline surface-based complex road surface path planning method, system, device and medium, the global path planning is performed on the B-spline surface by the G value and the H value of the improved A* algorithm, since the G value and the H value consider distance information, slope information and terrain fitting degree 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 determined by the local path optimization objective function, a non-uniform B-spline curve is generated, a smooth and continuous path with high road surface fitting degree is constructed, and the planning of the complex road surface path is completed. BRIEF DESCRIPTION OF DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the accompanying drawings needed in the embodiments will be briefly introduced as follows. Obviously, the accompanying drawings in the following description only only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

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

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

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

[0050] Figure 4 A point cloud map generated in step 1 in the embodiment; Figure 3

[0051] A convex hull property schematic diagram in which the B-spline surface is wrapped by the control points with slope; Figure 5

[0052] A convex hull property schematic diagram in which the B-spline surface is wrapped by the control points without slope; Figure 6

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

[0054] A complete unmanned system structure schematic diagram; Figure 8

[0055] A structure schematic diagram of a computer device provided in an embodiment of the present application. Figure 9 BRIEF DESCRIPTION OF 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] ​With reference to the drawings, the technical solutions in the embodiments of the present application will be clearly and completely described below, obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort belong to the scope of protection of the present application.

[0059] For the existing path planning, Carnegie Mellon University proposes a way of constructing an initial solution of a plane map and solving a locally planned path by constructing an optimal control equation. The basic principle is that the terrain under the three-dimensional rugged road conditions has great uncertainty, so under the premise of having a two-dimensional plane trajectory library, an optimization equation is constructed, a gradient descent method is used to optimize and solve near the initial path, and a state equation is used to estimate the state according to the chassis model of the vehicle, and finally a path under the three-dimensional road conditions is backstepped. However, there is a problem, because the optimal control is selected in the two-dimensional plane, after the three-dimensional rugged road conditions are disturbed by the uncertain factors, the actual obtained curve is not continuous, but is a curve subject to environmental factors.

[0060] Dalian University of Technology proposes a passability evaluation method based on B-spline surface. The basic principle is that after generating the related point cloud original data, the control points of the point cloud data are selected, the B-spline surface is generated, and the B-spline surface is used to solve the slope information of the terrain, which is used 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, robots and the like need to perform three-dimensional path planning in actual exploration and inspection tasks, such as in mines, oil fields, forests and the like, however, ordinary two-dimensional path planning cannot well adapt to the change of terrain. Here, the field exploration environment is taken as an example for analysis, as shown in Figure 1 and Figure 2 . Figure 1 and Figure 2 shows a complex road surface in a field desert environment. The complex road surface has the characteristics of unstructured road surface, rugged hard road surface, and high complexity. The application occasion of the present application is mainly to face the three-dimensional rugged road conditions similar to the above, and the path planning in this case is a problem we need to solve.

[0062] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the drawings and specific embodiments.

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

[0064] Step 1: Obtain the perception information and positioning information of the vehicle, and generate a point cloud map, as shown in Figure 4

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

[0066] Step 3: 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 considers distance, slope, and terrain fitting degree; the H value of the improved A* algorithm considers distance and terrain fitting degree.

[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 through the initial B-spline curve, the B-spline surface deviation degree, and the overall terrain evaluation value of the point cloud, a local path optimization objective function is constructed.

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

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

[0070] Step 21: The point cloud map is equally divided in X and Y directions to obtain a plurality of grids, each grid including a plurality of coordinate points.

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

[0072] Step 23: Based on the coordinates of each coordinate point in each grid, the control points in each grid are determined.

[0073] Step 24: Based on the control points in each grid, the B-spline surface is fitted, and the specific formula is:

[0074]

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

[0076] As shown in Figure 5 and Figure 6 ​As shown in FIG. 1, wherein Figure 5 FIG. 1 is a schematic diagram of convex hull property of B-spline surface wrapped by control points with slope; Figure 6 FIG. 1 is a schematic diagram of convex hull property of B-spline surface wrapped by control points with slope; B-spline surface is uniquely determined by control points, nodes and order, while B-spline curve presents convex hull property wrapped by control points in the case of being determined by control points, which is one of the main values of the cost item of the A* algorithm constructed in the global planning layer of the present application, and is also one of the main optimization items of the objective function constructed in the local planning optimization problem of the present application.

[0077] In the case of rough road with large slope, the gap between the B-spline surface and the control points (i.e. actual point cloud values) is larger, while in the case of road with gentle slope, the gap between the B-spline surface and the control points is smaller. This feature can be used to represent the degree of terrain fitting between the path obtained by path planning and the actual road surface, and is the main optimization item introduced in the present application. This index well represents the degree of fitting between the path and the terrain, which facilitates the subsequent control of the vehicle to meet the requirements of certain continuity and fitting degree on this basis, and is one of the differences between the path planning method of the present application and other path planning methods based on B-spline surface.

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

[0079] Step 31: based on the point cloud map and the B-spline surface, searching each node in the B-spline surface by using the improved A* algorithm, calculating the G value and the H value of each node; one node corresponds to one coordinate point.

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

[0081] Specifically, in the search process of the A* algorithm, each node needs to be expanded. The G value represents the cost (or penalty, which can be understood as distance) from the starting point to the current point, and the H value represents the predicted value from the current point to the target point, which is a heuristic function. The significance of the heuristic function is to guide the expansion direction of the A* algorithm, so that the current point can reach the target point in a more direct way. The G value considers distance information, slope information and terrain fitting degree information. The H value only considers the distance and the fitting degree of the terrain, because the point cloud information is incomplete in the construction process of the point cloud environment, and the slope information is also incomplete. Finally, under the action of the improved A* algorithm, the present application can generate a rough path considering distance, slope and terrain fitting degree, i.e. a B-spline curve, at the global path planning level.

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

[0083] G(n) = λ1 x g1 + λ2 x g2 + λ3 x g3 (2)

[0084]

[0085] wherein 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 terrain fitting degree from the starting point to the nth node, λ1, λ2, λ3 are weight values, a i is the three-dimensional coordinate of the ith node; R m is the m-dimensional real number field, where m is 3; x i is the horizontal axis coordinate value of the ith node; y i is the vertical axis coordinate value of the ith node; z i is the Z-axis coordinate value of the ith 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 ith node in the point cloud map.

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

[0087]

[0088] wherein 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 the 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 comprises:

[0090] The decision variable of the non-uniform B-spline curve is:

[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] S(t) = [1 s(t) s 2 (t)... s pb (t)] T(10)

[0095] Q m = [q m-pb q m-pb+1 q m-pb+2 ...q m ] T (11)

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

[0097] The decision variable indicates that the decision variable of the application is a non-uniform B-spline curve. The purpose of selecting the B-spline curve is to better fit the characteristics of the B-spline surface, and finally plan a non-uniform B-spline curve that better meets the application of the B-spline surface. A non-uniform B-spline curve is uniquely determined by a q vector and an s vector, which respectively represent the control points and nodes of the non-uniform B-spline curve, 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. And due to the nature of the non-uniform B-spline, the smoothness and speed change curve of the B-spline curve can be determined by considering the non-uniform change on this basis.

[0098] The local path optimization objective function equation is:

[0099] min J(q, s) = λ s J s + λ t J t + λ d J d + λ m J m (12)

[0100]

[0101] wherein 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, a total of M; s is the node of the non-uniform B-spline curve, a total of N, wherein the number of M and N is equal; J sis the smoothness of the initial B-spline curve; J t is the time through the initial B-spline curve; J d is the B-spline surface deviation; J m is the point cloud overall terrain evaluation value; λ s , λ t , λ d , λ m are all weight values; y l (2) is the y l second derivative of time, y l is the smoothness of the initial B-spline curve; R k is a k-dimensional real number field vector, where k is 3; T l is the time through the initial B-spline curve; J is the time through the initial B-spline curve; J t is not related to the control point q; Z rl is the actual Z coordinate value of the point cloud map; Z bl is the Z coordinate value of the initial B-spline curve; R is a one-dimensional real number field; ω1, ω2, ω3 are all weight values; is the highest Z axis coordinate value of the point cloud map; is the lowest Z axis coordinate value of the point cloud map; is the slope of the point cloud map; is the point cloud sparsity of the point cloud map.

[0102] Equations (12) to (16) are local path optimization objective function equations, mainly containing four terms, the smoothness of the B-spline curve, the time through the B-spline curve, the B-spline surface deviation, and the point cloud overall terrain evaluation.

[0103] In the smoothness of the B-spline curve, the present application considers the sum of the second derivatives, and the greater the sum of the second derivatives, the smaller the change of the smoothness of the B-spline curve in the first derivative, and the greater the sum of the second derivatives, the greater the change of the smoothness of the B-spline curve in the first derivative. The size of this change represents the degree of sudden change of the smoothness of the B-spline curve in the speed level, and further determines whether the non-uniform B-spline curve generated by the present application is smooth and continuous; the time through the B-spline curve is the time required for the vehicle to reach the destination on the B-spline curve; the B-spline surface deviation is an evaluation of the terrain fitting degree after the introduction of the B-spline surface by the present application, which represents the deviation degree of the actual control point from the B-spline surface; the point cloud overall terrain evaluation, such as the terrain roughness, the slope, and the point cloud sparsity, are all basic features possessed by the point cloud map. In places with high point cloud sparsity, a greater penalty is given; finally, appropriate weight values λ s , λt d m Standardization is carried out to achieve weight balance.

[0104] The constraint condition of the local path optimization objective function is:

[0105]

[0106] wherein, is the first derivative of the speed and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y and z directions with respect to time, and there are 6 dimensions; Φ is the speed and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y and z directions, and there are 6 dimensions; u max is a constant for constraining the six-dimensional vector of the speed and angular velocity of the vehicle passing through the point of 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 vehicle passing through the point of 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.

[0107] The point of the non-uniform B-spline curve includes the control point of the non-uniform B-spline curve and the node of the non-uniform B-spline curve.

[0108] The constraint condition consists of two items, one is the speed and angular limit of the automobile model, that is, the speed and angular velocity of the vehicle in the x, y and z directions under three-dimensional rugged road conditions, and there are 6 dimensions. In different dimensions, the application converts this process into changes in the vehicle coordinate system through the conversion of the state equation and then defines it. The second item of the constraint term is the solution space of the planned non-uniform B-spline curve, and the application specifies the value of the 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 constraint condition, when the objective function reaches the minimum time, the corresponding decision variable is the optimal path.

[0109] Advantages of the application:

[0110] (1) In the global path planning, the application improves the G value and H value of the A* algorithm for the B-spline surface path planning, so as to conform to the characteristics of the B-spline surface reaction, such as slope, terrain fitting degree, distance and other information, so as to conform to the characteristics of the three-dimensional rugged road conditions, and not to make the vehicle fall into a local optimal solution in the local planning process.

[0111] ​​(2) In the local path planning layer of the present application, based on the optimization equation for constructing a local path, including a local path optimization objective function equation, decision variables and constraint conditions, taking a non-uniform B-spline curve as a decision variable, it is helpful to solve the required continuous curve according to the characteristics of the B-spline surface in three-dimensional space, and through the local path optimization objective function equation, the target to be solved is determined, and when the objective function reaches the minimum time, the corresponding decision variable is the optimal path under the condition of meeting the constraint condition.

[0112] (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 risk, and optimize exploration and inspection paths.

[0113] In an exemplary embodiment, as shown in Figure 7 , a complex road surface path planning system based on B-spline surfaces is provided, which is applied to Figure 8 the unmanned system as shown. As shown in Figure 8 , a complete set of unmanned systems at least includes 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 the present application is the innovation of the planning module 7, and the construction of the global path planning module 5 and the local path planning module 6 in it is the main work object of the present application. The task control module currently proposed is based on a state machine implementation, which divides the vehicle's decision into multiple sub-tasks and makes judgments according to different road conditions.

[0114] The task control module can also be called a behavior decision module. If the behavior decision module is further considered, the interaction between people and robots and the interaction between the environment and robots can be considered, but the behavior decision module does not consider the interaction under the premise of multiple interactions. Therefore, behavior decision and related technologies are not used, and the only technology used is the implementation of a state machine, which enables the vehicle to perform some behavior changes at the right time, such as starting, braking, reaching the target point, etc. Some simple action transitions are involved.

[0115] Other modules include a perception module 1, a positioning module 2 and a control module 8, the development of the three modules is mainly applied to the construction of existing open source algorithms. The perception module 1 and the positioning module 2 can find the corresponding slam algorithm to construct, generate a point cloud map, and be carried into the actual framework, including the hardware deployment of the sensor, etc., wherein the perception module 1 can be a laser radar camera and the like; the positioning module 2 can contain odom odometer information. The control module 8 puts the planned complex road path into the chassis 9 and obtains the output result to realize control, the output result includes linear speed and wheel rotation angle, etc., and existing control algorithms such as LQR and MPC algorithm are relatively mature. After the development of the perception module 1, the positioning module 2, the planning module 7 and the control module 8 based on the Ros communication framework is completed, simulation verification can be carried out to verify the complex road path planning method based on the B-spline surface proposed in the application.

[0116] The complex road path planning system based on B-spline surface comprises:

[0117] The perception module 1 is used for acquiring the perception information of the vehicle.

[0118] The positioning module 2 is used for acquiring the positioning information of the vehicle.

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

[0120] The B-spline surface generation module 4 is connected with the point cloud map generation module 3, and is used for determining control points according to the point cloud map to fit a B-spline surface.

[0121] The global path planning module 5 is connected with the point cloud map generation module 3 and the B-spline surface generation module 4 respectively, and is used for generating an initial B-spline curve by using an improved A* algorithm to perform global path planning on the B-spline surface according to the point cloud map and the B-spline surface; the G value of the improved A* algorithm considers distance, slope and terrain fitting degree; the H value of the improved A* algorithm considers distance and terrain fitting degree.

[0122] The local path planning module 6 is connected with 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 of passing through the initial B-spline curve, the deviation degree of the B-spline surface and the overall terrain evaluation value of the point cloud to construct a local path optimization objective function equation; the initial B-spline curve is optimized based on the local path optimization objective function to generate a non-uniform B-spline curve, and the non-uniform B-spline curve is the planned complex road path.

[0123] In an exemplary embodiment, a computer device is provided, which can be a server or a terminal, and an internal structure diagram thereof can be as shown in FIG. 1. Figure 9 The computer device includes a processor, a memory, an input / output interface (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 configured 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 running the operating system and the computer program in the non-volatile storage medium. The database of the computer device is configured to store to-be-processed data. The input / output interface of the computer device is configured to exchange information between the processor and external devices. The communication interface of the computer device is configured to communicate with external terminals through a network connection. The computer program is executed by the processor to implement a complex road surface path planning method for a B-spline surface.

[0124] Those skilled in the art can understand that Figure 9 the structure shown in FIG. 1 is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. The specific computer device can 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, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps in the above method embodiments.

[0125] In an exemplary embodiment, a computer readable storage medium is provided, which stores a computer program. The computer program is executed by a processor to implement the steps in the above method embodiments.

[0126] 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 for analysis, stored data, displayed data, etc.) involved in the present application are all information and data authorized by the user or authorized by all parties, and the collection, use and processing of the relevant data need to comply with relevant regulations.

[0127] 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. The computer program can be stored in a non-volatile computer readable storage medium, and when the computer program is executed, the processes of the above-mentioned embodiments of the methods can be included. Any reference to 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 storage, high-density embedded non-volatile memory, resistive memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration but not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0128] The database involved in the embodiments provided in the present application can include at least one of a relational database and a non-relational database. The non-relational database can include a distributed database based on a blockchain, etc., without being limited thereto. The processor involved in the embodiments provided in the present application can be a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., without being limited thereto.

[0129] The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above embodiments are not described, however, as long as the combinations of the technical features do not exist contradictory, they should be considered as the scope of the present application.

[0130] The principles and implementation modes of the present application are described by applying specific examples in the present application. The above-mentioned embodiments are only used to help understand the method and its core idea of the present application; at the same time, for those skilled in the art, according to the idea of the present application, the specific implementation mode and application range will be changed. In conclusion, the content of the present application should not be understood as a limitation.

Claims

1. A complex road path planning method based on B-spline surface, characterized in that, The complex road path planning method based on the B-spline surface comprises the following steps: Obtaining the sensing information and positioning information of a vehicle and generating a point cloud map; Determining control points according to the point cloud map and fitting a B-spline surface; 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 considers distance, slope and terrain fitting degree; the H value of the improved A* algorithm considers distance and terrain fitting degree; Taking a non-uniform B-spline curve as a decision variable, considering the smoothness of the initial B-spline curve, the time of passing through the initial B-spline curve, the deviation degree of the B-spline surface and the overall terrain evaluation value of the point cloud, a local path optimization objective function is constructed; Based on the local path optimization objective function, the initial B-spline curve is optimized to generate a non-uniform B-spline curve, which is the planned complex road path.

2. The complex road path planning method based on B-spline surface according to claim 1, wherein, Determining control points according to the point cloud map and fitting a B-spline surface comprises the following steps: The point cloud map is equally divided in X and Y directions to obtain a plurality of grids, each of which includes a plurality of coordinate points; For each grid, the coordinates of each coordinate point are calculated; Based on the coordinates of each coordinate point in each grid, control points in each grid are determined; Based on the control points in each grid, a B-spline surface is fitted.

3. The B-spline surface based complex road path planning method of claim 1, wherein, 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, which comprises the following steps: The improved A* algorithm is used to search each node in the B-spline surface to calculate the G value and the H value of each node; one node corresponds to one coordinate point; Based on the G value and the H value of each node, path nodes are determined, 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, wherein, The calculation formula of the G value of each node is: G(n) = λ1×g1+λ2×g2+λ3×g3; Wherein, 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 terrain fitting degree from the starting point to the nth node, λ1, λ2, λ3 are weight values, a i is the three-dimensional coordinate of the ith node; R m is the three-dimensional coordinate of the ith node; R i is the horizontal axis coordinate value of the ith node; y i is the horizontal axis coordinate value of the ith node; y i is the Z-axis coordinate value of the ith node; Z bi is the Z-axis coordinate value of the ith node; Z ri is the Z-axis coordinate value of the ith node; Z 5. The complex road path planning method based on B-spline surface according to claim 3, wherein, The calculation formula of the H value of each node is: wherein 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 the m-dimensional real field, where m is 3; R is the one-dimensional real 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 B-spline curve based complex road path planning method of claim 1, wherein, The local path optimization objective function is: min J(q,s) = λ s J s +λ t J t +λ d J d +λ m J m ; wherein 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 passing through the initial B-spline curve; J d is the deviation degree of the B-spline surface; J m is the point cloud overall terrain evaluation value; λ s , λ t , λ d , λ m are all weight values; y l (2) is the smoothness of the initial B-spline curve; R k is a k-dimensional real number field vector, where k is 3; T l is the time passing through the initial B-spline curve; l is the index of the control point; R is a one-dimensional real number field; ω1, ω2, ω3 are all weight values; J represents the deviation of the initial B-spline curve; J t is irrelevant to the control point q; Z rl is the actual Z coordinate value of the lth point of 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; is the highest Z axis coordinate value of the lth point of the point cloud map; is the lowest Z axis coordinate value of the lth point of the point cloud map; is the slope of the lth point of the point cloud map; is the point cloud sparsity of the lth point of the point cloud map.

7. The B-spline surface based complex road path planning method of claim 1, wherein, The constraint condition of the local path optimization objective function is: wherein, is the first derivative of the velocity and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, z directions with respect to time; Φ is the velocity and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, z directions; u max is a constant for constraining the six-dimensional vector of the velocity and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, 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 velocity and angular velocity of the vehicle passing through the point of the non-uniform B-spline curve in the x, y, 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 and nodes of the non-uniform B-spline curve.

8. A complex road path planning system based on B-spline surfaces, characterized by, The complex road path planning system based on the B-spline surface is applied to the complex road path planning method based on the B-spline surface in any one of claims 1-7, and the complex road path planning system based on the B-spline surface comprises: A sensing module for obtaining the sensing information of a vehicle; A positioning module for obtaining the positioning information of a vehicle; A point cloud map generation module connected with the sensing module and the positioning module respectively for fusing the sensing information and the positioning information of the vehicle to generate a point cloud map; A B-spline surface generation module connected with the point cloud map generation module for determining control points according to the point cloud map and fitting a B-spline surface. The global path planning module is connected with the point cloud map generation module and the B-spline surface generation module respectively, and is used for generating an initial B-spline curve by performing global path planning on the B-spline surface according to the point cloud map and the B-spline surface by using an improved A* algorithm; the G value of the improved A* algorithm considers distance, slope and terrain fitting degree; and the H value of the improved A* algorithm considers distance and terrain fitting degree. The local path planning module is connected with the global path planning module, and is used for constructing a local path optimization objective function by taking a non-uniform B-spline curve as a decision variable and considering smoothness of the initial B-spline curve, time for passing through the initial B-spline curve, B-spline surface deviation degree and point cloud overall terrain evaluation value; and the initial B-spline curve is optimized based on the local path optimization objective function to generate a non-uniform B-spline curve, and the non-uniform B-spline curve is a planned complex road surface path.

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

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the B-spline curve-based complex road surface path planning method in any one of claims 1-7.

Images