Air-ground unmanned platform cooperative path planning method in unknown cross-country scene

Abstract

The application discloses a kind of unknown cross-country scene under air-ground unmanned platform cooperative path planning method, the method is according to the local risk map of unmanned aerial vehicle constructed in bird's-eye view under current time unmanned aerial vehicle carries laser radar;According to the local risk map of unmanned aerial vehicle, based on active exploration strategy, extract the forward navigation point of current time of unmanned aerial vehicle and unmanned vehicle;According to the forward navigation point, generate the flight path of unmanned aerial vehicle and provide reference path for unmanned vehicle;Unmanned vehicle generates driving path according to reference path, combines local terrain risk perception information and vehicle kinematics characteristics optimization;Finally, repeat the above steps until unmanned vehicle and unmanned aerial vehicle successfully arrive at navigation task target terminal point;The method is a kind of robust cooperative trajectory planning method, can realize air-ground unmanned platform safely, efficiently and reliably complete cooperative navigation task.

Description

Technical Field

[0001] This invention belongs to the field of multi-agent cooperative path planning technology, and in particular relates to a cooperative path planning method for air-ground unmanned platforms in unknown off-road scenarios. Background Technology

[0002] Addressing the needs of applications such as disaster relief, battlefield support, and material transportation, autonomous intelligent navigation technology for unmanned vehicles in unknown off-road scenarios has always been a hot topic of concern both domestically and internationally. However, compared to structured scenarios such as urban traffic and factory logistics, which have high-precision global maps or prior data rules, autonomous navigation for unmanned vehicles faces challenges such as a lack of prior information and limited perception field of view in unknown off-road environments. Relying solely on a single unmanned vehicle platform makes it difficult to generate safe, efficient, and reliable navigation paths.

[0003] Currently, with the development of drone technology and multi-agent collaborative technology, multi-agent collaborative systems combining drones and unmanned vehicles (UAVs) have been widely applied. Compared to UAVs, drones possess greater mobility and flexibility, capable of traversing steep slopes, boulders, and other special obstacles in off-road scenarios. They can also carry depth sensors such as LiDAR and depth cameras to acquire off-road terrain information from a wider field of view. Therefore, by combining the advantages of both drones and UAVs, drones can act as a "third eye in the air," using their superior field of view to plan driving paths for UAVs from an overhead perspective, guiding them to their navigation objectives. Subsequently, the UAV, combining its vehicle motion characteristics with high-precision local environmental perception information, optimizes the driving path provided by the drone in real time, thereby achieving safe, efficient, and reliable autonomous navigation in unknown off-road environments. Summary of the Invention

[0004] In view of this, the present invention provides a collaborative path planning method for air-ground unmanned platforms in unknown off-road scenarios, enabling unmanned vehicles to integrate cross-perspective path planning information from drones and collaboratively reach the destination of the navigation task.

[0005] A collaborative path planning method for air-ground unmanned platforms in unknown off-road scenarios includes the following steps:

[0006] Step 1: The UAV creates a local risk map based on the current overhead lidar sensor measurement information.

[0007] Step 2: Based on the local risk map of the drone at the current moment Extract the next-time forward navigation point Gcur and calculate the reference path for the autonomous vehicle to reach the forward navigation point.

[0008] Step 3: The autonomous vehicle establishes a local risk map based on the measurement information from the surround-view LiDAR sensor at the current moment. Based on the local risk map of the autonomous vehicle and the reference path Real-time optimization of motion trajectory and control tracking to obtain the final driving path at the current moment.

[0009] Step 4: Repeat the above steps until the drone and unmanned vehicle reach the destination of the collaborative navigation mission.

[0010] Preferably, step 1 specifically includes:

[0011] The UAV uses the positioning information output by its forward-looking visual-inertial sensor and the point cloud measurement information output by its downward-looking lidar to construct a local point cloud map of the UAV at time t.

[0012] A local point cloud map of the UAV at time t is generated based on the set resolution. Rasterization is performed, where each grid c i height value z i This will be set to the maximum height of the point cloud within that grid; for grid c in the local grid map of the UAV. i Calculate two terrain feature parameters of the raster: the height jump value h. i and slope value s i ;

[0013] Based on the local raster map of the UAV after calculating the terrain feature parameters, calculate c for each raster. i Risk value r i The formula is as follows:

[0014]

[0015] Where ω1 and ω2 are weights that sum to 1, and h crit s crit The threshold values ​​for each terrain parameter are used; the risk value of each grid is calculated according to the calculation formula to obtain the local risk map of the UAV at time t.

[0016] Preferably, in step 1, the height jump value h i The calculation methods include:

[0017] Calculate the height difference Δh between the current grid cell and its neighboring grid cells within a defined distance range d. i For all height differences Δh i Sort the cells from largest to smallest, and take the largest value as the height jump value of the current grid cell.

[0018] Preferably, in step 1, the slope value s i The calculation methods include:

[0019] Principal component analysis was used in the raster c. i Surrounding Fit Local Plane π i Obtain the normal vector n of the plane. i Calculate the normal vector n i The angle between the grid and the Z-axis of the global coordinate system is used as the grid slope value s. i .

[0020] Preferably, step 2 specifically includes:

[0021] Local risk map of the drone at time t Local risk map at time t-1 By comparison, a grid generated from the lidar measurement data newly acquired by the UAV at time t is obtained. This yields the sub-map of new drone risks at time t. And Each grid cell is stored in the detection queue Q. t ;

[0022] For queue Q t Each grid The following criteria are used to determine whether a grid is a valid edge grid: If the grid risk value meets the following conditions... r safe To set a threshold, and to ensure that none of its four adjacent grid cells (top, bottom, left, and right) exist in the local risk map. In the middle, it is considered a grid. It is a valid edge grid and is denoted as Traverse queue Q t This involves selecting all grid cells to obtain the set of valid edge grid cells. Then for the set The corresponding valid edge points are obtained using the Euclidean distance clustering method. This yields the complete set of valid edge points.

[0023] For the set of valid edge points Each edge point in Construct the cost function as shown below to calculate the cost and its value E. i :

[0024]

[0025] Where, ω r ω d ω h These are adjustable weight parameters, R, for different scenarios.i To form edge points Edge grid set The sum of the risk values ​​of each grid cell, d i The current location and edge points of the autonomous vehicle The distance, h i For edge points With the endpoint G of the collaborative navigation mission final The distance, N is The number of mid-edge points;

[0026] For the set of valid edge points Each edge point in According to its value E i Sort the points by size and select the edge points with the smallest cost value as the look-ahead navigation points G of the unmanned air-to-ground platform at time t. cur For drones, the existing PID algorithm is used to generate flight paths and control them to the forward navigation point G. cur .

[0027] Preferably, in step 2, when a valid edge grid is detected... Using a breadth-first search method, in the local risk map In accordance with the above determination conditions, the search for other valid edge grids in the vicinity begins from the grid cell. Once a valid edge grid cell is obtained, the search continues to find other valid edge grid cells in the vicinity, starting from the grid cell.

[0028] Preferably, in step 2, the local risk map of the UAV at time t is used. Forward Navigation Point G cur Location of driverless car The reference path for autonomous vehicle lane lookup navigation points is calculated using the risk-constrained A-scan path planning method. In the A* algorithm, the cost function G(i) and heuristic function H(i) of the grid are expressed as follows:

[0029]

[0030] H(i)=λ||C i -G cur || 2

[0031] In the formula, r i Here, λ represents the risk value of the current grid cell, and λ is a factor parameter that adjusts the risk cost and distance cost. 2 This indicates calculating the Euclidean distance between two points; C i and C i+1 Represents grid c i and c i+1 The coordinates.

[0032] Preferably, in step 3, the local risk map of the unmanned vehicle and the reference path are used... Real-time optimization of motion trajectory and control tracking to obtain the final driving path at the current moment. Specific methods include:

[0033] Based on the reference trajectory provided by the drone at time t From the reference trajectory Extract the set of trajectory control points {Q} i}, and use third-order B-spline curves to control the set of points {Q} i} is optimized, and its expression is:

[0034]

[0035] Among them B i.k (t) is the basis function of the Bézier curve, and M is the number of control points extracted from the trajectory;

[0036] For the control point set {Q i Construct a quadratic programming optimization problem:

[0037]

[0038] in, Let λ represent the optimized set of control points. s and λ d The weighted parameters f of the balanced optimization objective d The smoothness cost function is expressed as follows:

[0039]

[0040] Among them, f s The risk cost function is derived from the local risk map of the autonomous vehicle. The query returned the result.

[0041] The calculation based on the above optimization method Obtain the driving trajectory of the autonomous vehicle as it arrives at the current forward navigation point. The PID algorithm is used to control the unmanned vehicle to follow the trajectory, and finally complete the cooperative path planning for the current moment.

[0042] The present invention has the following beneficial effects:

[0043] This invention addresses the challenge of generating safe, efficient, and reliable navigation paths for a single unmanned vehicle (UAV) in unknown off-road environments due to limited perception perspective and range. It proposes a collaborative path planning method between an UAV and an UAV. This method leverages the advantages of UAVs in mobility and perception range, designing an active exploration method for the UAV and a path calculation process for the UAV. Furthermore, it optimizes the trajectory of the UAV by combining local environmental perception information and vehicle kinematics characteristics, resulting in a robust collaborative trajectory planning method. Finally, by repeatedly executing the active exploration and path planning steps, the air-to-ground unmanned platform can safely, efficiently, and reliably complete collaborative navigation tasks. Attached Figure Description

[0044] Figure 1 This is a flowchart of the collaborative path planning method for unmanned air-ground platforms in unknown off-road scenarios according to the present invention.

[0045] Figure 2 This is a schematic diagram of the simulated off-road scenario and the simulated unmanned air-to-ground platform of the present invention. Detailed Implementation

[0046] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0047] like Figure 1 As shown, this invention relates to a collaborative path planning method for air-to-ground unmanned platforms in unknown off-road scenarios, and the specific technical process includes:

[0048] Step 1: The UAV creates a local risk map based on the current overhead lidar sensor measurement information.

[0049] Step 2: Based on the local risk map of the drone at the current moment Extracting the next-moment forward navigation point G for drones and unmanned vehicles cur Calculate the reference path for the autonomous vehicle to reach the forward navigation point.

[0050] Step 3: The autonomous vehicle establishes a local risk map based on the measurement information from the surround-view LiDAR sensor at the current moment. Based on the local risk map and reference path of the autonomous vehicle Real-time optimization of motion trajectory and control tracking to obtain the final driving path at the current moment.

[0051] Step 4: Repeat the above steps until the drone and unmanned vehicle reach the destination of the collaborative navigation mission.

[0052] Further, step 1 specifically involves: using the positioning information output by the UAV's forward-looking visual inertial sensor and the point cloud measurement information output by the downward-looking lidar to establish a local point cloud map of the UAV at time t.

[0053] Furthermore, based on the set high resolution of 0.4m, a local point cloud map of the UAV at time t was generated. Rasterization is performed, where each grid c i height value z i This will be set to the maximum height of the point cloud within that grid. For grid c in the local grid map of the UAV... i Calculate two terrain feature parameters of the raster: the height jump value h. i and slope value s i .

[0054] Furthermore, calculate the height difference Δh between the current grid and its neighboring grids within a distance range d. i For all height differences Δh i Sort the cells from largest to smallest, and take the largest value as the height jump value for the current grid cell.

[0055] h i =max{Δh i}

[0056] Furthermore, principal component analysis was used in the raster c i Surrounding Fit Local Plane π i Obtain the normal vector n of the plane. i Calculate the normal vector n i The angle between the grid and the Z-axis of the global coordinate system is used as the grid slope value s. i .

[0057] Furthermore, based on the local raster map of the UAV after the terrain feature parameters have been calculated, the c of each raster is calculated. i Risk value r i The formula is as follows:

[0058]

[0059] Where ω1 and ω2 are weights that sum to 1, and h crit s crit These are the threshold values ​​for various terrain parameters. The risk value for each grid cell is calculated using the formula to obtain the local risk map of the UAV at time t.

[0060] Furthermore, step 2 specifically includes the following sub-steps:

[0061] 2.1 Local risk map of the drone at time t Local risk map at time t-1 By comparison, a grid generated from the lidar measurement data newly acquired by the UAV at time t is obtained. This yields the sub-map of new drone risks at time t. And Each grid cell is stored in the detection queue Q. t .

[0062] For grid The following criteria are used to determine whether a grid is a valid edge grid: If the grid risk value meets the following conditions... r safe To set a threshold, and to ensure that none of its four adjacent grid cells (top, bottom, left, and right) exist in the local risk map. In the middle, it is considered a grid. It is a valid edge grid and is denoted as

[0063] Furthermore, when a valid edge grid is detected... Using a breadth-first search method, in the local risk map In this process, based on the above-mentioned criteria, other valid edge grids are searched for starting from the current grid cell, resulting in a set of valid edge grids. After each determination, the raster markers have been checked; subsequently, for the set... The corresponding valid edge points are obtained using the Euclidean distance clustering method. After clustering is completed, the effective edge grid set will be... All grid cells in the queue are marked as detected and retrieved from queue Q. t Remove from queue Q. t When empty, obtain the final set of valid edge points.

[0064] Furthermore, for the set of valid edge points Each edge point in Construct the cost function as shown below to calculate the cost and its value E. i .

[0065]

[0066] Where, ω r ω d ω h These are adjustable weight parameters, R, for different scenarios. i To form edge points Edge grid set The sum of the risk values ​​of each grid cell, d i The current location and edge points of the autonomous vehicle The distance, h i For edge points With the endpoint G of the collaborative navigation mission final The distance, N is The number of edge points.

[0067] Furthermore, for the set of effective edge points Each edge point in According to its value E i Sort by size and select the edge point with the smallest cost. G serves as the forward navigation point for the unmanned air-to-ground platform at time t. cur For drones, the existing PID algorithm is used to generate flight paths and control them to the forward navigation point G. cur .

[0068] 2.2, Based on the local risk map of the UAV at time t Forward Navigation Point G cur Location of driverless car The reference path for autonomous vehicle lane lookup navigation points is calculated using the risk-constrained A-scan path planning method. In the A* algorithm, the cost function G(i) and heuristic function H(i) of the i-th grid can be expressed as:

[0069]

[0070] H(i)=λ||C i -G cur || 2

[0071] In the formula, r i Here, λ represents the risk value of the current grid cell, and λ is a factor parameter that adjusts the risk cost and distance cost. 2 This indicates calculating the Euclidean distance between two points; C i and C i+1 Represents grid c i and c i+1 The coordinates.

[0072] Furthermore, step 3 specifically includes the following sub-steps:

[0073] 3.1 The autonomous vehicle uses an open-source laser SLAM algorithm to build a local point cloud map of the autonomous vehicle at time t using point cloud measurement information output by the surround-view laser radar.

[0074] Furthermore, based on the set small resolution of 0.1m, a local point cloud map of the unmanned vehicle at time t was generated. The data is rasterized, and the raster height values ​​are set according to the same construction process as the UAV local map in step 1. The terrain risk feature parameters of the raster are calculated to finally obtain the local risk map of the UAV.

[0075] 3.2, Based on the reference trajectory provided by the UAV at time t From the reference trajectory Extract the set of trajectory control points {Q} i}, and use third-order B-spline curves to control the set of points {Q} i} is optimized, and its expression is:

[0076]

[0077] Among them B i，k (t) is the basis function of the Bézier curve, and M is the number of control points extracted from the trajectory.

[0078] Furthermore, for the control point set {Q} i Construct a quadratic programming optimization problem:

[0079]

[0080] in, Let λ represent the optimized set of control points. s and λ d The weighted parameters f of the balanced optimization objective d The smoothness cost function, used to generate smoother motion control trajectories, is expressed as follows:

[0081]

[0082] f s The risk cost function is used to improve the safety of motion control trajectory and can be obtained from the local risk map of the autonomous vehicle. The query returned the results.

[0083] 3.3, calculated based on the above optimization method Obtain the driving trajectory of the autonomous vehicle as it arrives at the current forward navigation point. The PID algorithm is used to control the unmanned vehicle to follow the trajectory, and finally complete the cooperative path planning for the current moment.

[0084] Example:

[0085] This invention uses, for example Figure 2The simulations shown depict off-road terrain and unmanned platforms in simulated airspace. At each moment, the local risk map coverage area for the UAV is 50m x 50m, and for the unmanned vehicle it is 10m x 10m. The resolution of the UAV's local risk map is 0.4m, and for the unmanned vehicle it is 0.1m. Furthermore, the weights ω1 and ω2 used for calculating the local risk value are 0.5 and 0.5 respectively, and the weight ω used for calculating the cost of edge points is... r ω d ω h The weights λ are 1 / 3, 1 / 3, and 1 / 3 respectively, used for autonomous vehicle trajectory optimization. s , λ d The values ​​are 0.5 and 0.5 respectively. The quantitative comparison results between existing methods and the proposed air-to-ground unmanned platform cooperative path planning method are as follows:

[0086] This invention 103.6m 61.3 Existing methods 152.3m 142.1

[0087] Simulation results show that the proposed method for collaborative path planning of unmanned aerial platforms in unknown off-road scenarios can reduce the total path length to 103.6m and the total path risk value to 61.3. This method utilizes the advantages of UAVs to actively explore and guide the path of unmanned vehicles from an aerial perspective, enabling unmanned vehicles to complete navigation tasks safely, efficiently and reliably in the field.

[0088] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for collaborative path planning between air and ground unmanned platforms in unknown off-road scenarios, characterized in that, Includes the following steps: Step 1: The UAV creates a local risk map based on the current overhead lidar sensor measurement information. Specifically, it includes: The UAV uses the positioning information output by its forward-looking visual-inertial sensor and the point cloud measurement information output by its downward-looking lidar to construct a local point cloud map of the UAV at time t. ; A local point cloud map of the UAV at time t is generated based on the set resolution. Rasterization is performed, where each grid cell height value This will be set to the maximum height of the point cloud within that grid; for grids in a local grid map of a drone. Calculate two terrain feature parameters of the raster: height jump value. and slope value ; Based on the local raster map of the UAV after the terrain feature parameters have been calculated, each raster is calculated. Risk value The formula is as follows: in, , The weights are those that sum to 1. , The threshold values ​​for each terrain parameter are used; the risk value of each grid is calculated according to the calculation formula to obtain the local risk map of the UAV at time t. ; Step 2: Based on the local risk map of the drone at the current moment Extract the next moment's forward navigation points Calculate the reference path for the autonomous vehicle to reach the forward navigation point. Specifically, it includes: Local risk map of the drone at time t Local risk map at time t-1 By comparison, a grid generated from the lidar measurement data newly acquired by the UAV at time t is obtained. Thus, the newly added risk sub-map of the drone at time t is obtained. and will Each grid cell is stored in the detection queue. ; For queues Each grid The following criteria are used to determine whether a grid is a valid edge grid: If the grid risk value meets the following conditions... , To set a threshold, and to ensure that none of its four adjacent grid cells (top, bottom, left, and right) exist in the local risk map. In the middle, it is considered a grid. It is a valid edge grid and is denoted as ; Traverse the queue This involves selecting all grid cells to obtain the set of valid edge grid cells. Then for the set The corresponding valid edge points are obtained using the Euclidean distance clustering method. This allows us to obtain the complete set of valid edge points. ; For the set of valid edge points Each edge point in Construct the cost function as shown below to calculate the cost and its value. : in, , , These are adjustable weight parameters for different scenarios. To form edge points Edge grid set The sum of the risk values ​​of each grid cell. The current location and edge points of the autonomous vehicle distance, For edge points With the endpoint of the collaborative navigation mission distance, for The number of mid-edge points; For the set of valid edge points Each edge point in According to its value Sort the points by size and select the edge points with the smallest cost value as the look-ahead navigation points for the unmanned air-to-ground platform at time t. For drones, the existing PID algorithm is used to generate flight paths and control them to forward navigation points. ; Step 3: The autonomous vehicle establishes a local risk map based on the measurement information from the surround-view LiDAR sensor at the current moment. Based on the local risk map of the unmanned vehicle and the reference path The motion trajectory is optimized in real time and controlled and tracked to obtain the final driving path at the current moment. ; Step 4: Repeat the above steps until the drone and unmanned vehicle reach the destination of the collaborative navigation mission.

2. The method for collaborative path planning of unmanned air-ground platforms in unknown off-road scenarios as described in claim 1, characterized in that, In step 1, the height jump value The calculation methods include: Calculate the height difference between the current grid cell and its neighboring grid cells within a defined distance range d. For all height differences Sort the cells from largest to smallest, and take the largest value as the height jump value of the current grid cell.

3. The method for collaborative path planning of unmanned air-ground platforms in unknown off-road scenarios as described in claim 1, characterized in that, In step 1, the slope value The calculation methods include: Principal component analysis was used in the raster. Surrounding Fit Local Plane To obtain the normal vector of the plane Calculate the normal vector The angle between the grid and the Z-axis of the global coordinate system is used as the grid slope value. .

4. The method for collaborative path planning of unmanned air-ground platforms in an unknown off-road scenario as described in claim 1, characterized in that, In step 2, when a valid edge grid is detected... Using a breadth-first search method, in the local risk map In accordance with the above determination conditions, the search for other valid edge grids in the vicinity begins from the grid cell. Once a valid edge grid cell is obtained, the search continues to find other valid edge grid cells in the vicinity, starting from the grid cell.

5. The method for collaborative path planning of unmanned air-ground platforms in unknown off-road scenarios as described in claim 1, characterized in that, In step 2, the local risk map of the UAV at time t is used. Forward navigation points Location of driverless car The risk-constrained A-Star path planning method is used to calculate the reference path for the forward navigation points of the autonomous vehicle lane. Among them, the cost function of the raster in the A* algorithm. With heuristic functions Represented as: In the formula, The current risk value of the raster. To adjust the factor parameters of risk cost and distance cost, This indicates the calculation of the Euclidean distance between two points; and Represents a grid and The coordinates.

6. The method for collaborative path planning of unmanned air-ground platforms in an unknown off-road scenario as described in claim 1, characterized in that, In step 3, based on the local risk map and reference path of the unmanned vehicle... The motion trajectory is optimized in real time and controlled and tracked to obtain the final driving path at the current moment. Specific methods include: Based on the reference trajectory provided by the drone at time t From the reference trajectory Extract the set of trajectory control points And use third-order B-spline curves to control the set of points. To optimize, its expression is: in is the basis function of the Bézier curve, and M is the number of control points extracted from the trajectory; For the set of control points Construct a quadratic programming optimization problem: in, This represents the optimized set of control points. and Weighted parameters for balancing the optimization objective. The smoothness cost function is expressed as follows: in, The risk cost function is derived from the local risk map of the autonomous vehicle. The query returned the result. The calculation based on the above optimization method Obtain the driving trajectory of the autonomous vehicle as it reaches the forward navigation point at the current moment. The PID algorithm is used to control the unmanned vehicle to follow the trajectory, and finally complete the cooperative path planning at the current moment.

Images