Unmanned vehicle-unmanned aerial vehicle air-ground cooperation system and method based on laser radar SLAM
By using lidar SLAM algorithm and extended Kalman filter in the UAV-UAV air-to-ground collaborative system, combined with Raycast and ego-planner planner, the system's dependence on GPS signals is solved, and the unmanned vehicle is realized. Accurate positioning and dynamic obstacle avoidance improve task efficiency and coverage.
Patent Information
- Application Number
- CN202411144821.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-20
- Publication Date
- 2024-12-03
AI Technical Summary
The existing UAV-UAV air-ground collaboration system relies on a centralized cluster control system and GPS signals, is susceptible to interference and cannot work properly in areas with GPS missing. The increase in sensors leads to a decrease in payload, affecting task efficiency.
Adopting a distributed air-to-ground collaborative path planning system based on lidar SLAM, through mutual communication between drones and unmanned vehicles, the lidar SLAM algorithm is used to calculate odometer data, and an extended Kalman filter and ego-planner planner are constructed. , realize accurate positioning and path planning of unmanned vehicles, and combine Raycast algorithm and collision ellipse prediction for path optimization.
It realizes precise positioning and dynamic obstacle avoidance of unmanned vehicles after losing GPS signals, improves task efficiency and coverage, reduces dependence on payloads, and enhances system stability and task completion capabilities.
Smart Images

Figure CN119065387A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of blind unmanned vehicle positioning and navigation, and specifically relates to an unmanned vehicle-unmanned aerial vehicle air-ground collaboration system and method based on laser radar SLAM. Background Art
[0002] Air-ground collaboration systems have broad application prospects in modern intelligent technologies, especially in disaster relief, environmental monitoring, urban planning, security patrol and other fields. With the rapid development of unmanned aerial vehicles (UAVs) and unmanned vehicles (UGVs), these mobile platforms can significantly improve mission efficiency and coverage through collaborative work. In order to achieve efficient air-ground collaboration, environmental perception and positioning and navigation technologies are key, among which the SLAM algorithm based on LiDAR has become one of the core technologies.
[0003] In today's drone-unmanned vehicle air-ground collaborative systems, some air-ground systems rely on centralized cluster control system base stations to coordinate the work between different intelligent entities. If the base station is interfered with, it will affect the system's journey stability. Some air-ground collaborative systems rely on GPS signals to realize the positioning of intelligent entities, and cannot work normally in areas where GPS is missing. In order to achieve the accuracy of collaborative tasks, some systems are equipped with more sensors, resulting in a reduction in the payload of drones and drones themselves for transportation or other tasks. Summary of the invention
[0004] The purpose of the present invention is to provide a distributed air-ground collaborative path planning system that does not rely on GPS signals.
[0005] The technical solution to achieve the purpose of the present invention is: an unmanned vehicle-unmanned aerial vehicle air-ground collaborative system based on laser radar SLAM, including a unmanned aerial vehicle and an unmanned vehicle that communicate with each other;
[0006] The drone is equipped with a laser radar and a relative pose estimation sensor. The drone receives point cloud information through the laser radar, calculates the drone odometer data through the laser radar 3D SLAM algorithm, pre-processes the radar point cloud set, and then generates occupancy grid map data through the Raycast algorithm.
[0007] The unmanned vehicle is equipped with a wheel odometer and a relative posture estimation sensor, receives drone data, uses the wheel odometer data of the unmanned vehicle as the control quantity, and uses the unmanned vehicle odometer data calculated by the drone odometer data and the relative posture data of the two as the measurement quantity. An extended Kalman filter is constructed to calculate the unmanned vehicle posture data, and the path planning is performed in the shared occupancy grid map through the ego-planner planner. The collision is predicted through the collision ellipse and the path is optimized.
[0008] A method for collaborative path planning based on the above system includes the following steps:
[0009] Step (1): The drone receives point cloud information through radar, calculates the drone's own odometer data through the lidar SLAM algorithm, pre-processes the radar point cloud set, and then calculates and updates the local occupancy grid map through the Raycast algorithm;
[0010] Step (2): The unmanned vehicle receives the odometer data from the drone, and then combines it with the relative posture sensor to obtain the relative posture of the drone and the unmanned vehicle. The wheel odometer data of the unmanned vehicle is used as the control quantity, and the unmanned vehicle odometer data calculated by the drone odometer data and the relative posture data of the two is used as the measurement quantity. An extended Kalman filter is constructed to calculate the posture data of the unmanned vehicle.
[0011] Step (3), the UAV sends the constructed local occupancy grid data to the unmanned vehicle, and the unmanned vehicle obtains the local occupancy grid map through the same parameter processing, and uses the ego-planner planner to use an aggressive strategy to treat the unknown grid as an unoccupied state and perform path planning;
[0012] Step (4): The unmanned vehicle receives the covariance matrix of the extended Kalman filter in step (2), constructs a probability ellipse based on the eigenvalues and eigenvectors of the matrix, propagates uncertainty along the path planned in step (3) in the occupancy grid map, and performs collision prediction. When an impending collision is detected, the path planned in step (3) is optimized to complete the obstacle avoidance task.
[0013] Furthermore, the laser radar SLAM algorithm steps in step (1) are:
[0014] The point cloud message is received from the laser radar carried by the drone, and after preprocessing the point cloud message, the point cloud set P k For point i in , let S be the set of consecutive points of i returned by the lidar in the same scan, and calculate the value of c used to evaluate the smoothness of the local surface:
[0015]
[0016] Then select the feature point with the largest c, i.e., the edge point, and the feature point with the smallest c, i.e., the plane point;
[0017] Assume t k is the start time of scanning k. After the scanning is completed, the point cloud set P received by the laser radar k is reprojected to timestamp t k The projected point cloud is recorded as The point cloud set P corresponding to the next timestamp k+1Estimate the motion of the laser radar together; extract the feature points by calculating the evaluation value of formula (1) and find P k+1 The edge point set E (k+1) With the plane point set H (k+1) , Secondly, find the edge line (j, l) corresponding to each edge point; calculate the distance from the point to the edge line:
[0018]
[0019] For each plane point, find the corresponding plane (j, l, m) and calculate the distance from the point to the plane:
[0020]
[0021] Assume that the angular velocity and linear velocity of the laser radar are uniform in a scan, and perform linear interpolation on the calculated pose; Assume for [t (k+1) ,t] The pose conversion between the two frames corresponds to the transformation matrix. For point i, get the corresponding timestamp t i , [t (k+1) , t i ]For the transformation matrix It is calculated by the following formula:
[0022]
[0023] Get the distance calculation formula:
[0024]
[0025] Combining equations (5) and (6) we get the nonlinear function:
[0026]
[0027] The nonlinear function in formula (7) is optimized and iterated continuously to minimize d:
[0028]
[0029] The above frame point cloud The P obtained by scanning this frame k+1 , and the transformation matrix obtained from the previous iteration cycle As input, then, from P k+1 Extract the edge point set E k+1 and the plane point set H k+1 , for each edge point / plane point, in the previous frame point cloud Find the corresponding edge line and plane segment in, establish the nonlinear function (7), and optimize it. If the iteration converges, output If the iteration reaches the end of the scan, the output and
[0030] Furthermore, the "calculating and updating the local occupancy grid map by using the Raycast algorithm" in step (1) is specifically as follows:
[0031] According to the odometer data predicted by the UAV SLAM algorithm, the point cloud set preprocessed in the previous step is laser mapped, and the local occupied grid map is calculated according to the set grid map update parameters. The grid map coordinates of the occupied grid and the grid map coordinates of the position grid are extracted, and the map information data of the updated local map are sent to the unmanned vehicle.
[0032] Furthermore, the extended Kalman filter structure constructed in step (2) is:
[0033] In the prediction stage, the extended Kalman filter receives the data from the wheel odometer of the unmanned vehicle as the control variable of the extended Kalman filter, and initializes the covariance matrix R and the Gaussian noise matrix Q to calculate the prior covariance matrix P k|k-1 In the update phase, the odometer data and relative posture of the drone are received to obtain the measurement matrix Z of the extended Kalman filter, the state transfer matrix K is calculated, and the filtered posterior state matrix is obtained by combining the measurement and prediction errors. With the covariance matrix P k|k .
[0034] Furthermore, the extended Kalman filter receives data from the wheel odometer of the unmanned vehicle including angular velocity and linear velocity.
[0035] Furthermore, the ego-planner operation process in step (3) is as follows:
[0036] Step (31): Generate a B-spline curve Φ that only satisfies the terminal constraint without considering obstacle avoidance;
[0037] Step (32): Iterate each collision segment and use the A* algorithm to generate a collision-free path Γ;
[0038] Step (33): For each control point Q on the trajectory Φ i The tangent vector R i , through the control point Q i Tangent vector R i The vertical plane Ψ intersects with Γ to form a line I, which intersects the obstacle surface j at point p i,j , and obtain the repulsive force direction vector v i,j, v is the unit vector from Q to p;
[0039] Step (34): Define the distance field d according to the (p, v) pairi,j Perform the calculation:
[0040] d i,j =(Q i -p i,j )·v i,j
[0041] If d i,j If it is greater than 0, it means that point p is not within the obstacle and the path planning is successful.
[0042] Compared with the prior art, the present invention has the following significant advantages:
[0043] The present invention introduces a drone with a SLAM algorithm to realize the drone's own posture estimation, shares the constructed occupancy grid map with the blind unmanned vehicle, and then realizes the precise positioning of the unmanned vehicle through the extended Kalman filter combined with the drone's posture data. Finally, two different unmanned vehicle path planning strategies are introduced. The planning path of the ego-planner planner based on the aggressive strategy is combined with the collision detection of the conservative strategy to predict the collision of the unmanned vehicle's forward path, thereby realizing the path planning of the unmanned vehicle, and being able to accurately position the unmanned vehicle and avoid obstacles, providing an important strategic guarantee for the completion of tasks such as path planning and dynamic obstacle avoidance after the unmanned vehicle loses the GPS signal. BRIEF DESCRIPTION OF THE DRAWINGS
[0044] Figure 1 This is a block diagram of the air-ground collaborative system of the present invention.
[0045] Figure 2 This is a flow chart of the unmanned vehicle trajectory planning of the present invention. DETAILED DESCRIPTION
[0046] The present invention is further described in detail below in conjunction with the accompanying drawings.
[0047] An air-ground collaborative system based on LiDAR SLAM is used to assist blind unmanned vehicles in path planning and dynamic obstacle avoidance without relying on GPS signals. The specific steps are as follows:
[0048] 1. The drone equipped with the laser radar SLAM algorithm estimates the odometer pose of the drone through the SLAM algorithm. The point cloud data obtained by the laser radar is pre-processed by denoising and removing invalid points, and the processed point cloud data is feature extracted. The start timestamp t k The corresponding point cloud P k , projecting it to timestamp t k+1 On, use Indicates that it is compared with the timestamp t k+1 The received point cloud P k+1Compare feature points, calculate the UAV’s posture parameters and odometer data, and send them to the unmanned vehicle.
[0049] 2. For the laser radar point cloud data obtained by the drone, the Raycast algorithm is used to receive the pre-processed point cloud message, and then each point in the point cloud set is traversed. The point cloud in the sphere with the radar as the center and the maximum mapping length as the radius is mapped, and the point cloud is mapped to the ray between it and the camera. If the distance between the point and the center of the sphere is less than the maximum mapping length, it means that the point observed in the radar observation is the edge of the obstacle, and the point is hit. Then, it is determined whether the two mappings are the point clouds of the same frame. For each ray, step mapping is performed to obtain the grid state, and the hit grid is updated. The grid state stored in the original grid map is expressed as the logarithm of the ratio odd(s):
[0050]
[0051] Where p(s=1) represents the probability that the grid is occupied, and p(s=0) represents the probability that the grid is not occupied. For the updated odd(s|z) after observation, we have:
[0052]
[0053] The model of the measured quantity is It is divided into two models: measurement hit and measurement miss. The state of the updated grid log(odd(s|z)) is obtained. If it is greater than the preset occupation state threshold, the point state is occupied, otherwise it is unoccupied. It is further determined whether log(odd(s|z)) is in the preset update interval. If it is in the interval, the corresponding point in the expanded occupied grid map is expanded upward, otherwise, the corresponding point is expanded downward in the expanded occupied grid map:
[0054] 3. The unmanned vehicle receives the odometer data from the drone and obtains the relative position data between the drone and the unmanned vehicle. The extended Kalman filter is used to generate the relative position data with the timestamp t k Corresponding to the odometer data u of the unmanned vehicle k As control input, it includes angular velocity and linear velocity:
[0055] u k =[v k ω k ] T =[p xk p yk θ k ] T
[0056] Calculate the odometer and covariance matrix of the unmanned vehicle predicted by the extended Kalman filter:
[0057]
[0058]
[0059] Combine the UAV odometer and the UAV relative position data of the unmanned vehicle to obtain the measurement data of the unmanned vehicle’s position z k :
[0060]
[0061] When the UAV observes the unmanned vehicle for the first time, the EKF is initialized. The initialization steps are as follows:
[0062]
[0063]
[0064] P k|k =(IK k H k ) k|k-1
[0065] Finally, the unmanned vehicle odometer data after EKF filtering is output.
[0066] 4. After receiving the map information sent by the drone, the unmanned vehicle odometer data after EKF filtering obtained in step 3 is combined to process the received map information to obtain the corresponding local occupancy grid map, which is synchronously updated to the unmanned vehicle's global occupancy grid map. For the unknown occupied grids, an aggressive planning strategy is adopted to set all unknown grid states to unoccupied (free), and the ego-planner is used for preliminary path planning.
[0067] 5. At the same time, a conservative strategy is adopted to propagate the uncertainty of the unmanned vehicle to the grid of unknown state, and the covariance matrix P calculated by EKF in step 3 is received. xy , calculate the eigenvalue λ 1,2 With the eigenvector [v 1 v 2 ]:
[0068] Q -1 P xy Q=Λ=diag(λ 1 ,λ 2 )
[0069] Q=[v 1 v 2 ]
[0070] Compute a Gaussian ellipse:
[0071]
[0072]
[0073] Finally, traverse the cells occupied by the ellipse along the planned trajectory to find the collision ellipse center PPC and collision time TPC of the UGV. Optimize the trajectory obtained in step 4.
[0074] In summary, the present invention designs a path planning system based on air-ground collaboration that does not rely on GPS signals. The system introduces a drone with a SLAM algorithm to realize the drone's own path planning and posture estimation, and shares the constructed occupancy grid map with the blind unmanned vehicle. The position of the unmanned vehicle is accurately located by combining the drone's posture data with the extended Kalman filter. In addition, the system combines two different unmanned vehicle guidance strategies: one is the ego-planner planner based on the radical strategy, which is responsible for path planning; the other is to plan the path based on the former, perform collision detection through a conservative strategy, and predict the collision risk of the unmanned vehicle's forward path. Therefore, the system can effectively realize the path planning, dynamic obstacle avoidance and other tasks of the unmanned vehicle after losing the GPS signal, providing it with important strategic guarantees.
Claims
1. The unmanned vehicle-unmanned aerial vehicle air-ground collaborative system based on LiDAR SLAM is characterized by: including drones and autonomous vehicles that communicate with each other; The drone is equipped with a laser radar and a relative pose estimation sensor. The drone receives point cloud information through the laser radar, calculates the drone odometer data through the laser radar 3D SLAM algorithm, pre-processes the radar point cloud set, and then generates occupancy grid map data through the Raycast algorithm. The unmanned vehicle is equipped with a wheel odometer and a relative posture estimation sensor, receives drone data, uses the wheel odometer data of the unmanned vehicle as the control quantity, and uses the unmanned vehicle odometer data calculated by the drone odometer data and the relative posture data of the two as the measurement quantity. An extended Kalman filter is constructed to calculate the unmanned vehicle posture data, and the path planning is performed in the shared occupancy grid map through the ego-planner planner. The collision is predicted through the collision ellipse and the path is optimized.
2. A method for collaborative path planning based on the system of claim 1, characterized in that: The steps include: Step (1): The drone receives point cloud information through radar, calculates the drone's own odometer data through the lidar SLAM algorithm, pre-processes the radar point cloud set, and then calculates and updates the local occupancy grid map through the Raycast algorithm; Step (2): The unmanned vehicle receives the odometer data from the drone, and then combines it with the relative posture sensor to obtain the relative posture of the drone and the unmanned vehicle. The wheel odometer data of the unmanned vehicle is used as the control quantity, and the unmanned vehicle odometer data calculated by the drone odometer data and the relative posture data of the two is used as the measurement quantity. An extended Kalman filter is constructed to calculate the posture data of the unmanned vehicle. Step (3), the UAV sends the constructed local occupancy grid data to the unmanned vehicle, and the unmanned vehicle obtains the local occupancy grid map through the same parameter processing, and uses the ego-planner planner to use an aggressive strategy to treat the unknown grid as an unoccupied state and perform path planning; Step (4): The unmanned vehicle receives the covariance matrix of the extended Kalman filter in step (2), constructs a probability ellipse based on the eigenvalues and eigenvectors of the matrix, propagates uncertainty along the path planned in step (3) in the occupancy grid map, and performs collision prediction. When an impending collision is detected, the path planned in step (3) is optimized to complete the obstacle avoidance task.
3. The method according to claim 2, characterized in that The steps of the laser radar SLAM algorithm in step (1) are: The point cloud message is received from the laser radar carried by the drone, and after preprocessing the point cloud message, the point cloud set P k For point i in , let S be the set of consecutive points of i returned by the lidar in the same scan, and calculate the value of c used to evaluate the smoothness of the local surface: Then select the feature point with the largest c, i.e., the edge point, and the feature point with the smallest c, i.e., the plane point; Assume t k is the start time of scanning k. After the scanning is completed, the point cloud set P received by the laser radar k is reprojected to timestamp t k The projected point cloud is recorded as The point cloud set P corresponding to the next timestamp k+1 Estimate the motion of the laser radar together; extract the feature points by calculating the evaluation value of formula (1) and find P k+1 The edge point set E (k+1) With the plane point set H (k+1) , Secondly, find the edge line (j, l) corresponding to each edge point; calculate the distance from the point to the edge line: For each plane point, find the corresponding plane (j, 1, m) and calculate the distance from the point to the plane: Assume that the angular velocity and linear velocity of the laser radar are uniform in one scan, and perform linear interpolation on the calculated pose; Assume for [t (k+1) ,t] The pose conversion between the two frames corresponds to the transformation matrix. For point i, get the corresponding timestamp t i , [t (k+1) , t i ]For the transformation matrix It is calculated by the following formula: Get the distance calculation formula: Combining equations (5) and (6) we get the nonlinear function: The nonlinear function in formula (7) is optimized and iterated continuously to minimize d: The above frame point cloud This frame scan obtains P k+1 , and the transformation matrix obtained from the previous iteration cycle As input, then, from P k+1 Extract the edge point set E k+1 and the plane point set H k+1 , for each edge point / plane point, in the previous frame point cloud Find the corresponding edge line and plane segment in, establish the nonlinear function (7), and optimize it. If the iteration converges, output If the iteration reaches the end of the scan, the output and 4. The method according to claim 3, characterized in that In step (1), "by Ra yc The ast algorithm calculates and updates the local occupancy grid map" specifically as follows: According to the odometer data predicted by the UAV SLAM algorithm, the point cloud set preprocessed in the previous step is laser mapped, and the local occupied grid map is calculated according to the set grid map update parameters. The grid map coordinates of the occupied grid and the grid map coordinates of the position grid are extracted, and the map information data of the updated local map are sent to the unmanned vehicle.
5. The method according to claim 4, characterized in that The extended Kalman filter structure constructed in step (2) is: In the prediction stage, the extended Kalman filter receives the data from the wheel odometer of the unmanned vehicle as the control variable of the extended Kalman filter, and initializes the covariance matrix R and the Gaussian noise matrix Q to calculate the prior covariance matrix P k|k-1 In the update phase, the odometer data and relative posture of the drone are received to obtain the measurement matrix Z of the extended Kalman filter, the state transfer matrix K is calculated, and the filtered posterior state matrix is obtained by combining the measurement and prediction errors. With the covariance matrix P k|k .
6. The method according to claim 5, characterized in that The extended Kalman filter receives data from the wheel odometer of the unmanned vehicle, including angular velocity and linear velocity.
7. The method according to claim 6, characterized in that The ego-planner operation process in step (3) is as follows: Step (31): Generate a B-spline curve Φ that only satisfies the terminal constraint without considering obstacle avoidance; Step (32): Iterate each collision segment and use the A* algorithm to generate a collision-free path Γ; Step (33): For each control point Q on the trajectory Φ i The tangent vector R i , through the control point Q i Tangent vector R i The vertical plane Ψ intersects with Γ to form a line I, which intersects the obstacle surface j at point p i,j , and obtain the repulsive force direction vector v i,j , v is the unit vector from Q to p; Step (34): Define the distance field d according to the (p, v) pair i,j Perform the calculation: d i,j =(Q i -p i,j )·v i,j If d i,j If it is greater than 0, it means that point p is not within the obstacle and the path planning is successful.
Citation Information
Cited By
Air-ground cooperative unmanned system high-reliability positioning navigation method
CN119860777A
Track scanning method and device of track scanning system and track scanning system
CN122519347A
Track scanning method, device and system of track scanning system
CN122519347B
