A path planning method for unmanned vehicle platooning not restricted by road conditions

Abstract

A method for planning a path for an unmanned vehicle formation that is not restricted by road conditions comprises the following steps: 1) establishing a global coordinate system, defining the initial position corresponding to each unmanned vehicle in the formation, and determining the target position corresponding to each unmanned vehicle in the actual formation: 1-1) defining a target point coordinate set according to the target formation and the number of unmanned vehicles in the formation; 1-2) listing each formation situation of the unmanned vehicle formation by using permutations and combinations, obtaining the theoretical formation time corresponding to each formation situation, and selecting the minimum theoretical formation time as the actual formation time; 1-3) obtaining the target position corresponding to each unmanned vehicle in the actual formation according to the driving time, average driving speed and driving distance of each unmanned vehicle in the formation process corresponding to the actual formation time; 2) using a hybrid A* algorithm to plan the driving path of each unmanned vehicle from the initial position to the target position; 3) using a speed decision method based on an S-T graph to plan the driving speed of each unmanned vehicle.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a path planning method for an unmanned vehicle formation that is not restricted by road conditions. Background Art

[0002] Most of the existing unmanned vehicle platooning methods are based on establishing a coordinate system based on lanes, and control unmanned vehicles traveling on structured roads with lanes. Due to the limitations of road conditions, they can only be used in environments with lanes, and only consider the lateral and longitudinal spacing between vehicles, and cannot be applied to field environments or other environments without lanes.

[0003] Existing obstacle avoidance measures for unmanned vehicles usually only use two measures: changing lanes or slowing down the unmanned vehicle to maintain the relative position with the obstacle vehicle unchanged, which are not flexible.

[0004] How to make unmanned vehicles not restricted by road conditions during driving, and be able to adapt to most environments without lanes, as well as increase the flexibility of unmanned vehicles' obstacle avoidance measures, have always been issues that the field of unmanned driving technology has long been eager to solve. Summary of the invention

[0005] The purpose of the present invention is to address the corresponding deficiencies of the prior art and provide a path planning method for unmanned vehicle formations that is not restricted by road conditions, thereby increasing the flexibility of the unmanned vehicle's obstacle avoidance measures during driving and being not restricted by road conditions. The method can be used not only for automatic control of unmanned vehicles on structured roads with lanes, but also for automatic control of unmanned vehicles in environments without lanes.

[0006] The purpose of the present invention is to adopt the following scheme to achieve: a method for unmanned vehicle formation path planning that is not restricted by road conditions, comprising the following steps:

[0007] 1) Establish a global coordinate system, define the initial position of each unmanned vehicle in the formation, and determine the target position of each unmanned vehicle in the actual formation:

[0008] 1-1) Define the target point coordinate set according to the target formation and the number of unmanned vehicles;

[0009] 1-2) using permutations and combinations to list each formation situation of the unmanned vehicle formation, and obtaining the theoretical formation time corresponding to each formation situation, wherein the theoretical formation time is the theoretical time required to form a target formation during the unmanned vehicle formation process, and the minimum theoretical formation time is selected as the actual formation time;

[0010] 1-3) Determine the target position corresponding to each unmanned vehicle during the actual formation according to the driving time, average driving speed, and driving distance of each unmanned vehicle during the formation corresponding to the actual formation time;

[0011] 2) A hybrid A* algorithm is used to plan the driving path of each unmanned vehicle from the initial position to the target position;

[0012] 3) The speed decision method based on ST graph is used to plan the driving speed of each unmanned vehicle.

[0013] Preferably, the expression of the theoretical formation time is as follows:

[0014]

[0015] Where T is the theoretical formation time, p1 is the initial position of the first unmanned vehicle, p2 is the initial position of the second unmanned vehicle, and p n is the initial position of the nth unmanned vehicle, o i is the coordinate of the i-th target point, o j is the coordinate of the jth target point, o k is the coordinate of the kth target point, The first unmanned vehicle reaches the target point o from the initial position p1 i The required travel time, The second unmanned vehicle reaches the target point o from the initial position p2 j The required travel time, For the nth unmanned vehicle from the initial position p n Arrive at the target point o k The required driving time, i,j,k∈{1,2,...,n}, and i≠j≠k, n is the number of unmanned vehicles.

[0016] Preferably, the theoretical formation time is calculated by using the relationship between the driving time required for each unmanned vehicle to reach each target point from the initial position and the average driving speed and driving distance of the unmanned vehicle.

[0017] Preferably, the theoretical formation time is calculated according to the following formula:

[0018]

[0019] In the formula, p i is the initial position of the i-th unmanned vehicle, o j is the coordinate of the jth target point, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The distance moved, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The average speed during the process, For the i-th unmanned vehicle from the initial position pi Arrive at the target point o j The required driving time, i,j∈{1,2,...,n}, n is the number of unmanned vehicles.

[0020] Preferably, the expression of the actual formation time is as follows:

[0021]

[0022] Where, T min is the actual formation time, n is the number of unmanned vehicles, T1 is the theoretical formation time corresponding to the first formation situation, and T2 is the theoretical formation time corresponding to the second formation situation. For the The theoretical formation time corresponding to the various formation situations.

[0023] Preferably, the hybrid A* algorithm is used to plan the driving path of each unmanned vehicle from the initial position to the target position according to the following steps:

[0024] 2-1) Use grids to divide the plane where the unmanned vehicle is located into a grid map;

[0025] 2-2) Setting up a node database, wherein the node database is provided with a plurality of node data tables for storing node information;

[0026] 2-3) Determine the initial node and target node of the unmanned vehicle in the grid map according to the initial position, target position and heading angle of the unmanned vehicle, and store them in the node database;

[0027] 2-4) determining the grid positions of multiple obstacle avoidance path subnodes of the initial node in the grid map;

[0028] 2-5) calculating the comprehensive cost function values ​​of multiple obstacle avoidance path sub-nodes of the node, and storing the node information of the node and each obstacle avoidance path sub-node of the node in the node data table corresponding to the node;

[0029] 2-6) taking the node with the smallest comprehensive cost function value in the node data table described in step 2-5) as the next path node, and clearing the other node information in the node data table;

[0030] 2-7) Determine the grid position of each obstacle avoidance path sub-node of the next path node in the grid map;

[0031] 2-8) Repeat steps 2-5) to 2-7) until the next path node is the target node, and obtain the node information of each node between the initial node and the target node;

[0032] 2-9) Fit each node between the initial node and the target node to obtain the driving path of the unmanned vehicle.

[0033] Preferably, the node information includes the ID of the node, the ID of the parent node of the node, the comprehensive cost function value, the heuristic function value, and the cumulative cost function value.

[0034] Preferably, the obstacle avoidance path sub-node of each node is obtained by permuting and combining the speed and deflection angle of the unmanned vehicle at the node, and there is no obstacle between the obstacle avoidance path sub-node of each node and the node.

[0035] Preferably, the comprehensive cost function value of each node is calculated according to the following steps:

[0036] b-1) Calculate the cost function value from the current node to the parent node according to the following formula:

[0037] cost(x parent ,x)=dist(x parent ,x)×(1+a·backcost+b·mapcost)+switchcost

[0038] In the formula, switchcost is the cost function value of the unmanned vehicle switching the driving direction, backcost is the driving cost function value of the unmanned vehicle retreating from the current node to the parent node, a is the weight coefficient of the driving cost function value of the unmanned vehicle retreating from the current node to the parent node, mapcost is the terrain cost function value of the current node, b is the weight coefficient of the terrain cost function value of the current node, dist(x parent ,x) is the path length from the current node to the parent node, x parent is the ID of the parent node of the current node, cost(x parent ,x) is the cost function value from the current node to the parent node, x is the ID of the current node;

[0039] b-2) Calculate the cumulative cost function value of the current node according to the following formula:

[0040] g(x)=g(x parent )+cost(x parent ,x)

[0041] In the formula, cost(x parent ,x) is the cost function value from the current node to the parent node, g(x) is the cumulative cost function value of the current node, x is the ID of the current node, x parent is the ID of the parent node of the current node, g(x parent ) is the cumulative cost function value of the parent node of the current node;

[0042] b-3) Calculate the heuristic function value of the current node according to the following formula:

[0043] h(x)=max{h RS ,h dis}

[0044] In the formula, h RS is the length of the RS curve from the current node to the target node, h dis is the basic A* path length from the current node to the target node, h(x) is the heuristic function value of the current node, and x is the ID of the current node;

[0045] b-4) Calculate the comprehensive cost function value of the current node according to the following formula:

[0046] f(x)=g(x)+h(x)

[0047] Where h(x) is the heuristic function value of the current node, g(x) is the cumulative cost function value of the current node, x is the ID of the current node, and f(x) is the comprehensive cost function value of the current node.

[0048] Preferably, the specific steps of planning the driving speed of each unmanned vehicle using the speed decision method based on the ST graph are as follows:

[0049] 3-1) Setting an upper threshold of the speed of the unmanned vehicle, and presetting the speed of each unmanned vehicle as the upper threshold of the speed of the unmanned vehicle;

[0050] 3-2) Select any unmanned vehicle as the first unmanned vehicle;

[0051] 3-3) According to the driving path and driving speed of the unmanned vehicle, the driving time required for the unmanned vehicle to travel is obtained;

[0052] 3-4) constructing an ST graph corresponding to the unmanned vehicle in a rectangular coordinate system with the driving time of the unmanned vehicle as the horizontal axis and the driving distance of the unmanned vehicle as the vertical axis;

[0053] 3-5) Assume that the driving time of the unmanned vehicle is composed of a number of equal intervals, and based on the ST diagram corresponding to the unmanned vehicle, record the position of the unmanned vehicle corresponding to each interval in the global coordinate system to obtain the time-position relationship curve of the unmanned vehicle;

[0054] 3-6) Select another unmanned vehicle and repeat steps 3-2) to 3-4) to obtain the ST graph corresponding to the unmanned vehicle;

[0055] 3-7) Assume that the driving distance of the unmanned vehicle is composed of a number of equal-length intervals, record the position of the unmanned vehicle corresponding to each interval in the global coordinate system, and obtain the time-position relationship curve of the unmanned vehicle based on the ST graph corresponding to the unmanned vehicle;

[0056] 3-8) adjusting the driving speed of the unmanned vehicle within the range of the upper speed threshold of the unmanned vehicle until the time-position relationship curves of the unmanned vehicles do not intersect;

[0057] 3-9) Repeat steps 3-3) to 3-5) for each unmanned vehicle after adjusting the driving speed, and obtain a time-position relationship curve corresponding to each unmanned vehicle after adjusting the driving speed;

[0058] 3-10) Repeat steps 3-6) to 3-9) until the driving speed of each unmanned vehicle is adjusted.

[0059] The advantage of the present invention is that it increases the flexibility of the obstacle avoidance measures of the unmanned vehicle during driving, is not restricted by road conditions, and can be used not only for the automatic control of the unmanned vehicle on structured roads with lanes, but also for the automatic control of the unmanned vehicle in an environment without lanes. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 is a flow chart of the present invention;

[0061] Figure 2 A flow chart of the present invention using a hybrid A* algorithm to plan the driving path of each unmanned vehicle from an initial position to a target position;

[0062] Figure 3 This is a flow chart of the present invention using the ST diagram-based speed decision method to plan the driving speed of each unmanned vehicle;

[0063] Figure 4 Schematic diagram of the grid position of each obstacle avoidance path sub-node in the grid map in the present invention. DETAILED DESCRIPTION

[0064] like Figures 1 to 4 As shown, a path planning method for an unmanned vehicle formation that is not restricted by road conditions includes the following steps:

[0065] 1) Establish a global coordinate system, define the initial position of each unmanned vehicle in the formation, and determine the target position of each unmanned vehicle in the actual formation:

[0066] 1-1) Define the target point coordinate set according to the target formation and the number of unmanned vehicles;

[0067] In this embodiment, the initial position coordinate set corresponding to each unmanned vehicle in the formation is defined as P = [p1, p2, ..., p n ], define the target point coordinate set as O = [o1, o2, ..., o n ], n is the number of unmanned vehicles;

[0068] 1-2) Each formation situation of the unmanned vehicle formation is listed by permutations and combinations, and the theoretical formation time corresponding to each formation situation is obtained, and the minimum theoretical formation time is selected as the actual formation time. The theoretical formation time is the theoretical time required to form the target formation during the unmanned vehicle formation process. The expression of the theoretical formation time is as follows:

[0069]

[0070] Where T is the theoretical formation time, p1 is the initial position of the first unmanned vehicle, p2 is the initial position of the second unmanned vehicle, and p n is the initial position of the nth unmanned vehicle, o i is the coordinate of the i-th target point, o j is the coordinate of the jth target point, o k is the coordinate of the kth target point, The first unmanned vehicle reaches the target point o from the initial position p1 i The required travel time, The second unmanned vehicle reaches the target point o from the initial position p2 j The required travel time, For the nth unmanned vehicle from the initial position p n Arrive at the target point o k The required driving time, i,j,k∈{1,2,...,n}, and i≠j≠k, n is the number of unmanned vehicles.

[0071] In this embodiment, the theoretical formation time is calculated by using the relationship between the driving time required for each unmanned vehicle to reach each target point from the initial position and the average driving speed and driving distance of the unmanned vehicle. The theoretical formation time is calculated according to the following formula:

[0072]

[0073] In the formula, p i is the initial position of the i-th unmanned vehicle, o j is the coordinate of the jth target point, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The distance moved, For the i-th unmanned vehicle from the initial position p iArrive at the target point o j The average speed during the process, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The required driving time, i,j∈{1,2,...,n}, n is the number of unmanned vehicles.

[0074] 1-3) According to the driving time, average driving speed and driving distance of each unmanned vehicle in the formation process corresponding to the actual formation time, the target position corresponding to each unmanned vehicle in the actual formation is obtained;

[0075] The expression of the actual formation time is as follows:

[0076]

[0077] Where, T min is the actual formation time, n is the number of unmanned vehicles, T1 is the theoretical formation time corresponding to the first formation situation, and T2 is the theoretical formation time corresponding to the second formation situation. For the The theoretical formation time corresponding to the various formation situations.

[0078] 2) A hybrid A* algorithm is used to plan the driving path of each unmanned vehicle from the initial position to the target position;

[0079] In this embodiment, the hybrid A* algorithm is used to plan the driving path of each unmanned vehicle from the initial position to the target position according to the following steps, so that the driving path of each unmanned vehicle can be planned:

[0080] 2-1) Use grids to divide the plane where the unmanned vehicle is located into a grid map;

[0081] 2-2) Setting up a node database, wherein the node database is provided with a plurality of node data tables for storing node information;

[0082] 2-3) Determine the initial node and target node of the unmanned vehicle in the grid map according to the initial position, target position and heading angle of the unmanned vehicle, and store them in the node database;

[0083] In this embodiment, the state quantity of the unmanned vehicle at the node is defined to include {x, y, θ}, where (x, y) represents the node position coordinates of the unmanned vehicle in the grid map, and θ represents the heading angle.

[0084] 2-4) determining the grid positions of multiple obstacle avoidance path subnodes of the initial node in the grid map;

[0085] The obstacle avoidance path sub-node of each node is obtained by the arrangement and combination of the speed and deflection angle of the unmanned vehicle at the node, and there is no obstacle between the obstacle avoidance path sub-node of each node and the node.

[0086] In this embodiment, the estimated speed of the unmanned vehicle is set to {v, -v}, where v represents the unmanned vehicle moving forward and -v represents the unmanned vehicle moving backward; the estimated steering wheel angle of the unmanned vehicle is set to {φ, 0, -φ}, and the estimated driving time of the unmanned vehicle is set to △t, that is:

[0087] During the estimated driving time △t of the unmanned vehicle at the node, the node state quantity of each child node of the node is calculated using the control quantity (v, φ), (v, 0) (v, -φ) (-v, φ) (-v, 0) (-v, -φ) by permutation and combination, and the child nodes with obstacles on the path between the node and the node are eliminated, so as to obtain each obstacle avoidance path child node and the grid position of each obstacle avoidance path child node in the grid map;

[0088] In this embodiment, before determining the child nodes of each node, the Reeds-Shepp curve between the current node and the target node can be planned first. If there is no obstacle on the Reeds-Shepp curve between the current node and the target node, the Reeds-Shepp curve is directly selected as the subsequent path of the current node. If there is an obstacle on the Reeds-Shepp curve between the current node and the target node, the grid positions of multiple obstacle avoidance path child nodes of the current node in the grid map are determined by the above method.

[0089] In order to improve the calculation efficiency, an RS curve test constant can be preset in advance. If the ID value of the current node can be divided by the RS curve test constant, the Reeds-Shepp curve between the current node and the target node is planned first. If the ID value of the current node cannot be divided by the RS curve test constant, there is no need to plan the Reeds-Shepp curve. The grid positions of multiple obstacle avoidance path sub-nodes of the current node in the grid map are directly determined by the above method.

[0090] The RS curve test constant is obtained by simulation experiments or calibration experiments to ensure that the time required for planning the path is reduced to the maximum extent.

[0091] 2-5) calculating the comprehensive cost function values ​​of multiple obstacle avoidance path sub-nodes of the node, and storing the node information of the node and each obstacle avoidance path sub-node of the node in the node data table corresponding to the node;

[0092] 2-6) taking the node with the smallest comprehensive cost function value in the node data table described in step 2-5) as the next path node, and clearing the other node information in the node data table;

[0093] 2-7) Determine the grid position of each obstacle avoidance path sub-node of the next path node in the grid map;

[0094] 2-8) Repeat steps 2-5) to 2-7) until the next path node is the target node, and obtain the node information of each node between the initial node and the target node, wherein the node information includes the ID of the node, the parent node ID of the node, the heuristic function value, the cumulative cost function value, and the comprehensive cost function value.

[0095] In this embodiment, the comprehensive cost function value of each node is calculated according to the following steps:

[0096] b-1) Calculate the cost function value from the current node to the parent node according to the following formula:

[0097] cost(x parent ,x)=dist(x parent ,x)×(1+a·backcost+b·mapcost)+switchcost

[0098] In the formula, switchcost is the cost function value of the unmanned vehicle switching the driving direction, backcost is the driving cost function value of the unmanned vehicle retreating from the current node to the parent node, a is the weight coefficient of the driving cost function value of the unmanned vehicle retreating from the current node to the parent node, mapcost is the terrain cost function value of the current node, b is the weight coefficient of the terrain cost function value of the current node, dist(x parent ,x) is the path length from the current node to the parent node, x parent is the ID of the parent node of the current node, cost(x parent ,x) is the cost function value from the current node to the parent node, x is the ID of the current node;

[0099] In this embodiment, the terrain cost function value mapcost of the current node is preset to be 0 when there is no obstacle and to be ∞ when there is an obstacle;

[0100] The cost function value switchcost for the unmanned vehicle to switch the driving direction is preset, so that the unmanned vehicle can maintain the driving direction as much as possible during planning, and avoid the unmanned vehicle frequently switching between forward and backward states;

[0101] The preset driving cost function value backcost of the unmanned vehicle backing from the current node to the parent node enables the unmanned vehicle to drive forward as much as possible to avoid the occurrence of reversing behavior. In this embodiment, the backcost is 0 when the unmanned vehicle drives forward, and the backcost is 1 when the unmanned vehicle drives backward.

[0102] b-2) Calculate the cumulative cost function value of the current node according to the following formula:

[0103] g(x)=g(x parent )+cost(x parent ,x)

[0104] In the formula, cost(x parent ,x) is the cost function value from the current node to the parent node, g(x) is the cumulative cost function value of the current node, x is the ID of the current node, x parent is the ID of the parent node of the current node, g(x parent ) is the cumulative cost function value of the parent node of the current node;

[0105] b-3) Calculate the heuristic function value of the current node according to the following formula:

[0106] h(x)=max{h RS ,h dis}

[0107] In the formula, h RS is the length of the RS curve from the current node to the target node, h dis is the basic A* path length from the current node to the target node, h(x) is the heuristic function value of the current node, and x is the ID of the current node;

[0108] In this embodiment, the heuristic function h(x) of the hybrid A* algorithm can reflect the length of a kinematically feasible path from the current node to the target node that avoids obstacles under ideal conditions.

[0109] The RS curve complies with the vehicle kinematic constraints but ignores obstacles.

[0110] The basic A* algorithm takes obstacle avoidance into account, but kinematic feasibility cannot be guaranteed.

[0111] Therefore, the basic A* path length is the path searched by the A* algorithm, which does not satisfy the kinematic constraints, so the accuracy is not high and it is not suitable for direct use. Therefore, selecting the larger value of the two can ensure that the heuristic function value can be close to the value under the ideal state.

[0112] b-4) Calculate the comprehensive cost function value of the current node according to the following formula:

[0113] f(x)=g(x)+h(x)

[0114] Where h(x) is the heuristic function value of the current node, g(x) is the cumulative cost function value of the current node, x is the ID of the current node, and f(x) is the comprehensive cost function value of the current node.

[0115] 2-9) Fit each node between the initial node and the target node to obtain the driving path of the unmanned vehicle.

[0116] In this embodiment, a B-spline function is used to fit each node between the initial node and the target node.

[0117] 3) The speed decision method based on ST graph is used to plan the driving speed of each unmanned vehicle.

[0118] In this embodiment, the specific steps of using the speed decision method based on the ST graph to plan the driving speed of each unmanned vehicle are as follows:

[0119] 3-1) Setting an upper threshold of the speed of the unmanned vehicle, and presetting the speed of each unmanned vehicle as the upper threshold of the speed of the unmanned vehicle;

[0120] In this embodiment, the upper threshold value of the unmanned vehicle's driving speed is obtained through simulation experiments or calibration experiments based on the current vehicle condition of the unmanned vehicle, the road condition of the road section and the curvature limit of the driving path.

[0121] 3-2) Select any unmanned vehicle as the first unmanned vehicle;

[0122] 3-3) According to the driving path and driving speed of the unmanned vehicle, the driving time required for the unmanned vehicle during the driving process is obtained. The driving path of the unmanned vehicle in this step is the driving path planned by the hybrid A* algorithm in step 2);

[0123] 3-4) constructing an ST graph corresponding to the unmanned vehicle in a rectangular coordinate system with the driving time of the unmanned vehicle as the horizontal axis and the driving distance of the unmanned vehicle as the vertical axis;

[0124] 3-5) Assume that the driving time of the unmanned vehicle is composed of a number of equal intervals, and based on the ST diagram corresponding to the unmanned vehicle, record the position of the unmanned vehicle corresponding to each interval in the global coordinate system to obtain the time-position relationship curve of the unmanned vehicle;

[0125] 3-6) Select another unmanned vehicle and repeat steps 3-2) to 3-4) to obtain the ST graph corresponding to the unmanned vehicle;

[0126] 3-7) Assume that the driving distance of the unmanned vehicle is composed of a number of equal-length intervals, record the position of the unmanned vehicle corresponding to each interval in the global coordinate system, and obtain the time-position relationship curve of the unmanned vehicle based on the ST graph corresponding to the unmanned vehicle;

[0127] 3-8) adjusting the driving speed of the unmanned vehicle within the range of the upper speed threshold of the unmanned vehicle until the time-position relationship curves of the unmanned vehicles do not intersect;

[0128] 3-9) Repeat steps 3-3) to 3-5) for each unmanned vehicle after adjusting the driving speed, and obtain a time-position relationship curve corresponding to each unmanned vehicle after adjusting the driving speed;

[0129] 3-10) Repeat steps 3-6) to 3-9) until the driving speed of each unmanned vehicle is adjusted.

[0130] 4) After each unmanned vehicle reaches the corresponding target position, an undirected graph G = (V, E) can be used to represent the formation of the unmanned vehicles during the formation maintenance process, and the formation difference function based on the normalized Laplace matrix can be used to maintain the formation.

[0131] In this embodiment, the following steps are followed to maintain the formation of the unmanned vehicles during driving:

[0132] 4-1) In the global coordinate system, let the real-time position vector of the i-th unmanned vehicle be pi = (xi, yi), and the real-time position vector of the j-th unmanned vehicle be p j =(x j ,y j ), the real-time position vectors of N unmanned vehicles constitute a point set V = {p1,…,p i ,p j ,…,p N};

[0133] 4-2) Connect to the position p of the i-th unmanned vehicle i =(x i ,y i ) and the position p of the jth unmanned vehicle j =(x j ,y j ) of the vector e ij =(x j -x i ,y j -y i ) constitute the edge set E;

[0134] 4-3) Vector e ij The weight w ij Calculated using the following formula:

[0135] w ij =||e ij ||,i≤n,j≤n,i≠n

[0136] 4-4) According to the point set, edge set E, and edge weight w of the undirected graph G ij , get the degree matrix D and adjacency matrix L of the undirected graph;

[0137] 4-5) Since the normalized Laplace matrix is ​​geometrically invariant, during the geometric enlargement and reduction and rotation of the unmanned vehicle team, the symmetric normalized Laplace matrix of the undirected graph G is calculated according to the following formula:

[0138]

[0139] Where D is the degree matrix of the undirected graph, and L is the adjacency matrix of the undirected graph.

[0140] 4-6) Calculate the difference between the current formation and the target formation and the gradient of the real-time position vector of the i-th unmanned vehicle according to the following formula to obtain the optimal position of each unmanned vehicle:

[0141]

[0142] Where f is the difference between the current formation and the target formation, is the normalized Laplace matrix of the current unmanned vehicle formation, is the normalized Laplace matrix of the unmanned vehicle target formation.

[0143] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification made to the present invention by those skilled in the art without departing from the spirit of the present invention shall fall within the protection scope of the present invention.

Claims

1. A path planning method for unmanned vehicle formation that is not restricted by road conditions, characterized in that: The following steps are involved: 1) Establish a global coordinate system, define the initial position of each unmanned vehicle in the formation, and determine the target position of each unmanned vehicle in the actual formation: 1-1) Define the target point coordinate set according to the target formation and the number of unmanned vehicles in the formation; 1-2) Use permutations and combinations to list each formation of the unmanned vehicle formation, obtain the theoretical formation time corresponding to various formation situations, and select the smallest theoretical formation time as the actual formation time; 1-3) Determine the target position corresponding to each unmanned vehicle during the actual formation; 2) The hybrid A* algorithm is used to plan the driving path of each unmanned vehicle from the initial position to the target position: 2-1) Divide the plane where the unmanned vehicle is located into a grid map; 2-2) Setting up a node database, wherein the node database is provided with a plurality of node data tables for storing node information; 2-3) Determine the initial node and target node of the unmanned vehicle in the grid map according to the initial position, target position and heading angle of the unmanned vehicle; 2-4) Determine the grid positions of multiple obstacle avoidance path sub-nodes of the initial node; 2-5) calculating the comprehensive cost function values ​​of multiple obstacle avoidance path sub-nodes of the node, and storing the node information of the node and each obstacle avoidance path sub-node of the node in the node data table; 2-6) The node with the smallest comprehensive cost function value in the node data table is used as the next path node, and the other node information in the node data table is cleared; 2-7) Determine the grid position of each obstacle avoidance path sub-node of the next path node; 2-8) Repeat steps 2-5) to 2-7) until the next path node is the target node, and obtain the node information of each node between the initial node and the target node; 2-9) Fitting each node between the initial node and the target node to obtain the driving path of the unmanned vehicle; The comprehensive cost function value of each node is calculated according to the following steps: b-1) Calculate the cost function value from the current node to the parent node according to the following formula: cost(x parent ,x)=dist(x parent ,x)×(1+a·backcost+b@mapcost)+switchcost In the formula, switchcost is the cost function value of the unmanned vehicle switching the driving direction, backcost is the driving cost function value of the unmanned vehicle retreating from the current node to the parent node, a is the weight coefficient of the driving cost function value of the unmanned vehicle retreating from the current node to the parent node, mapcost is the terrain cost function value of the current node, b is the weight coefficient of the terrain cost function value of the current node, dist(x parent ,x) is the path length from the current node to the parent node, x parent is the ID of the parent node of the current node, cost(x parent ,x) is the cost function value from the current node to the parent node, x is the ID of the current node; b-2) Calculate the cumulative cost function value of the current node according to the following formula: g(x)=g(x parent )+cost(x parent ,x) In the formula, cost(x parent ,x) is the cost function value from the current node to the parent node, g(x) is the cumulative cost function value of the current node, x is the ID of the current node, x parent is the ID of the parent node of the current node, g(x parent ) is the cumulative cost function value of the parent node of the current node; b-3) Calculate the heuristic function value of the current node according to the following formula: h(x)=max{h RS ,h dis } In the formula, h RS is the length of the RS curve from the current node to the target node, h dis is the basic A* path length from the current node to the target node, h(x) is the heuristic function value of the current node, and x is the ID of the current node; b-4) Calculate the comprehensive cost function value of the current node according to the following formula: f(x)=g(x)+h(x) In the formula, h(x) is the heuristic function value of the current node, g(x) is the cumulative cost function value of the current node, x is the ID of the current node, and f(x) is the comprehensive cost function value of the current node; 3) The speed decision method based on the ST graph is used to plan the driving speed of each unmanned vehicle: 3-1) Setting an upper threshold of the speed of the unmanned vehicle, and presetting the speed of each unmanned vehicle as the upper threshold of the speed of the unmanned vehicle; 3-2) Select any unmanned vehicle as the first unmanned vehicle; 3-3) According to the driving path and driving speed of the unmanned vehicle, the driving time required for the unmanned vehicle to travel is obtained; 3-4) constructing an ST graph corresponding to the unmanned vehicle in a rectangular coordinate system with the driving time of the unmanned vehicle as the horizontal axis and the driving distance of the unmanned vehicle as the vertical axis; 3-5) Assume that the driving time of the unmanned vehicle is composed of a number of equal intervals, and based on the ST diagram corresponding to the unmanned vehicle, record the position of the unmanned vehicle corresponding to each interval in the global coordinate system to obtain the time-position relationship curve of the unmanned vehicle; 3-6) Select another unmanned vehicle and repeat steps 3-2) to 3-4) to obtain the ST graph corresponding to the unmanned vehicle; 3-7) Assume that the driving distance of the unmanned vehicle is composed of a number of equal-length intervals, record the position of the unmanned vehicle corresponding to each interval in the global coordinate system, and obtain the time-position relationship curve of the unmanned vehicle based on the ST graph corresponding to the unmanned vehicle; 3-8) adjusting the driving speed of the unmanned vehicle within the range of the upper speed threshold of the unmanned vehicle until the time-position relationship curves of the unmanned vehicles do not intersect; 3-9) Repeat steps 3-3) to 3-5) for each unmanned vehicle after adjusting the driving speed, and obtain a time-position relationship curve corresponding to each unmanned vehicle after adjusting the driving speed; 3-10) Repeat steps 3-6) to 3-9) until the driving speed of each unmanned vehicle is adjusted.

2. The unmanned vehicle formation path planning method not restricted by road conditions according to claim 1 is characterized in that: The expression of the theoretical formation time is as follows: Where T is the theoretical formation time, p1 is the initial position of the first unmanned vehicle, p2 is the initial position of the second unmanned vehicle, and p n is the initial position of the nth unmanned vehicle, o i is the coordinate of the i-th target point, o j is the coordinate of the jth target point, o k is the coordinate of the kth target point, The first unmanned vehicle reaches the target point o from the initial position p1 i The required travel time, The second unmanned vehicle reaches the target point o from the initial position p2 j The required travel time, For the nth unmanned vehicle from the initial position p n Arrive at the target point o k The required driving time, i,j,k∈{1,2,...,n}, and i≠j≠k, n is the number of unmanned vehicles.

3. The unmanned vehicle formation path planning method not restricted by road conditions according to claim 1 or 2, characterized in that: The theoretical formation time is calculated by using the relationship between the driving time required for each unmanned vehicle to reach each target point from the initial position and the average driving speed and driving distance of the unmanned vehicle.

4. The unmanned vehicle formation path planning method not restricted by road conditions according to claim 3 is characterized in that: The theoretical formation time is calculated according to the following formula: In the formula, p i is the initial position of the i-th unmanned vehicle, o j is the coordinate of the jth target point, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The distance moved, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The average speed during the process, For the i-th unmanned vehicle from the initial position p i Arrive at the target point o j The required driving time, i,j∈{1,2,...,n}, n is the number of unmanned vehicles.

5. The unmanned vehicle formation path planning method not restricted by road conditions according to claim 1 is characterized in that: The expression of the actual formation time is as follows: Where, T min is the actual formation time, n is the number of unmanned vehicles, T1 is the theoretical formation time corresponding to the first formation situation, and T2 is the theoretical formation time corresponding to the second formation situation. For the The theoretical formation time corresponding to the various formation situations.

6. The unmanned vehicle formation path planning method not restricted by road conditions according to claim 1 is characterized in that: The node information includes the ID of the node, the parent node ID of the node, the comprehensive cost function value, the heuristic function value, and the cumulative cost function value.

7. The unmanned vehicle formation path planning method not restricted by road conditions according to claim 1 is characterized in that: The obstacle avoidance path sub-node of each node is obtained by the arrangement and combination of the speed and deflection angle of the unmanned vehicle at the node, and there is no obstacle between the obstacle avoidance path sub-node of each node and the node.

Images