Air-ground cooperative trajectory optimization algorithm with obstacle avoidance function

Abstract

The invention relates to an air-ground cooperative trajectory optimization algorithm with an obstacle avoidance function. Comprising the following steps: (1) an unmanned aerial vehicle receives point cloud information through a laser radar and preprocesses the point cloud information; calculating to obtain odometer data of the unmanned aerial vehicle, and calculating and updating a local occupied grid map; (2) the unmanned vehicle calculates to obtain a relative pose, and an accurate pose of the unmanned vehicle is obtained through an extended Kalman filter; calculating a minimum boundary ellipse through an MBE algorithm; (3) the occupied grid map is sent to the unmanned vehicle, and the unmanned vehicle carries out path planning on the occupied grid map, namely a reference path; and (4) the unmanned vehicle matches the ellipses of the front frame and the rear frame through a Kuhn-Munkres algorithm, path tracking and optimization are performed through an MPC-D-CBF algorithm according to the reference path, and a final unmanned vehicle collision-free path is obtained. The algorithm can effectively assist the unmanned vehicle losing the GPS signal to complete path planning, and the obstacle avoidance capability of the unmanned vehicle is further improved.

Description

Technical Field

[0001] The present invention belongs to the field of air-ground collaborative systems of unmanned aerial vehicles and unmanned vehicles, and specifically relates to an air-ground collaborative trajectory optimization algorithm with obstacle avoidance function. Background Art

[0002] In the field of modern intelligent technology, the application prospects of air-ground collaborative systems are becoming increasingly broad, especially in disaster emergency response, environmental monitoring, urban development planning, and security patrols. With the rapid advancement of unmanned aerial vehicle (UAV) and unmanned vehicle (UGV) technology, more technologies can be used for collaborative operations between the two, making the sum of one greater than the other. In order to achieve autonomous navigation of each intelligent body, map perception and path planning methods in dynamic and unstructured environments are very important.

[0003] In recent years, the research on robot autonomous navigation has mainly focused on the environmental perception and autonomous navigation of a single robot. However, concentrating environmental perception and autonomous navigation on a single unmanned vehicle may cause the lack of environmental information, affecting the navigation and path planning of subsequent robots. Moreover, concentrating sensors on drones or unmanned vehicles will reduce the load allocated to transportation or other tasks. Some multi-robot assistance systems, such as some centralized air-ground collaborative systems, coordinate the work between different intelligent bodies through base stations, but if the base stations are interfered with, it will affect the stability of the system. The present invention places the environmental perception and autonomous navigation modules on drones and unmanned vehicles respectively, reducing the load of both. The distributed coordination scheme can also reduce the impact on the stability of the system after receiving interference. Summary of the invention

[0004] The purpose of the present invention is to provide an air-ground coordinated trajectory optimization algorithm with obstacle avoidance function.

[0005] The technical solution to achieve the purpose of the present invention is: an air-ground collaborative trajectory optimization algorithm with obstacle avoidance function, the unmanned aerial vehicle is equipped with a laser radar and a relative posture estimation sensor, and is equipped with an ALOAM algorithm and a DBSCAN clustering algorithm, the unmanned vehicle is equipped with a wheel odometer and a relative posture estimation sensor, and is equipped with an MPC-D-CBF algorithm, a Kuhn–Munkres algorithm, an extended Kalman filter, and an ego-planner planner; the algorithm includes the following steps:

[0006] Step (1): The drone receives point cloud information through the laser radar and pre-processes the point cloud information through the DBSCAN clustering algorithm; the drone's own odometer data is calculated through the ALOAM algorithm, and the local occupancy grid map is updated through the Raycast algorithm;

[0007] Step (2): Based on the odometer data and relative pose estimation sensor data transmitted by the drone, the unmanned vehicle calculates the relative pose, and obtains the precise pose of the unmanned vehicle through the extended Kalman filter carried by the unmanned vehicle; at the same time, the minimum bounding ellipse of each point cloud cluster is calculated through the MBE algorithm, and the minimum bounding ellipse corresponding to the unmanned vehicle is estimated and eliminated through the precise pose obtained by the extended Kalman filter;

[0008] Step (3): Send the occupancy grid map of the drone in step (1) to the unmanned vehicle. The unmanned vehicle restores the map and uses the ego-planner on board to perform path planning on the occupancy grid map to obtain a reference path for the unmanned vehicle.

[0009] Step (4): The unmanned vehicle receives the minimum bounding ellipse information from step (2), matches the two ellipses in the previous and next frames using the Kuhn-Munkres algorithm, and performs path tracking and optimization using the MPC-D-CBF algorithm based on the reference path planned in step (3) to obtain the final collision-free path for the unmanned vehicle.

[0010] Furthermore, in step (1), “preprocessing the point cloud information by using the DBSCAN clustering algorithm” is specifically as follows:

[0011] Set the parameter domain size Eps of the DBSCAN clustering algorithm and the minimum number of points MinPts required to form a cluster. DBSCAN starts from any point p in the point cloud set of each frame of the input, and retrieves all density-reachable points from p. If p is a core point, that is, the number of points in the domain of p is greater than MinPts, a cluster is formed; otherwise, the point is marked as a noise point, and the next point is traversed until all points are traversed, and a set of n clusters A is obtained by integration. obs ={O1, O2, ..., O n} and several noise points.

[0012] Furthermore, in step (2), “obtaining the precise position and posture of the unmanned vehicle through the extended Kalman filter carried by the unmanned vehicle” is specifically:

[0013] In the prediction stage, the extended Kalman filter (EKF) uses the angular velocity and linear velocity provided by the wheel odometer of the unmanned vehicle as control input; initializes the covariance matrix R and Gaussian noise matrix Q, and calculates the prior covariance matrix P k|k-1 and posture information prediction value;

[0014] In the update phase, the extended Kalman filter (EKF) fuses the drone odometer data and relative pose estimation sensor data to construct a measurement matrix Z containing pose information measurements.

[0015] By calculating the state transfer matrix K, the extended Kalman filter EKF combines the error between the measured value and the predicted value to finally obtain the filtered posterior state estimate and the covariance matrix P k|k ;

[0016] Iterate and continuously calculate the precise position and posture of the unmanned vehicle.

[0017] Furthermore, in step (2), "calculating the minimum bounding ellipse of each point cloud cluster by the MBE algorithm, estimating the minimum bounding ellipse corresponding to the unmanned vehicle by the accurate pose obtained by the extended Kalman filter and eliminating it" is specifically as follows:

[0018] For each group of n clusters of point cloud information in each frame in step (1), the minimum bounding ellipse is calculated for each cluster respectively: for a given point set, i.e., a cluster, the points in the cluster are traversed, the selected point p is added to the initially empty point set P, and the minimum bounding ellipse Di of the points other than point p is calculated. If point p is on the boundary of the minimum bounding ellipse Di, then the minimum bounding ellipse Di is also the minimum bounding ellipse including point p, and point p is added to the boundary set R; if not, the minimum bounding ellipse Di is updated to surround point p by calling the recursive boundary function b_minidisk; the specific expression is as follows:

[0019]

[0020] The recursive relation in the recursive boundary function b_minidisk is:

[0021]

[0022] Where t(n) is the expected number of steps to process n points, and c is the constant time required to calculate the minimum circle;

[0023] When the number of points in the point set is reduced to 0, or the number of points in the boundary set R reaches (d+3)d / 2, the termination condition of the traversal is met, and Di at this time is the minimum boundary ellipse D;

[0024] Calculate the set of minimum bounding ellipses D corresponding to all clusters of a set of point cloud information;

[0025] The precise position and posture of the unmanned vehicle obtained by the extended Kalman filter is combined to eliminate the minimum bounding ellipse corresponding to the unmanned vehicle.

[0026] Furthermore, step (3) specifically includes the following steps:

[0027] Step (31): Generate a B-spline curve Φ that only satisfies the terminal constraint and does not consider obstacle avoidance, i.e., a trajectory;

[0028] Step (32): Iterate each collision segment and use the A* algorithm to generate a collision-free path Г;

[0029] 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 the collision-free path Г 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 Q pointing to point p i，j The unit vector of

[0030] Step (34): Define the distance field d according to the (p, v) pair i，j Perform the calculation:

[0031] d i，j =(Q i -p i，j )·v i，j

[0032] If d i，j is greater than 0, it means point p i，j There are no obstacles, and the path planning is successful.

[0033] Furthermore, in step (4), "matching the ellipses of the previous and next frames by using the Kuhn-Munkres algorithm" is specifically as follows:

[0034] For the two sets of minimum bounding ellipses obtained from the previous and next frames in step (2), the Kuhn–Munkres algorithm is used to convert the data of the minimum bounding ellipses of the previous and next frames into a state vector and solve the corresponding allocation matrix R ij , for a given positive integer n×n matrix R=(r ij ), find the transformation j1 ， ...,j n Among the integers 1, ..., n such that Maximum; as follows:

[0035] argmin u1+...+u n +v1+...+ν n

[0036] u i +ν j ≧r ij

[0037] where u1, ..., u n with v1, ..., v nIt is a non-negative integer. After finding the corresponding matching relationship, the corresponding boundary ellipse is numbered and the boundary ellipses of the previous and next frames are matched.

[0038] Furthermore, in step (4), "path tracking and optimization by using the MPC-D-CBF algorithm to obtain the final collision-free path for the unmanned vehicle" is specifically as follows:

[0039] After receiving the accurate position and posture of the unmanned vehicle obtained by the extended Kalman filter in step (2), the MPC algorithm is used to control the unmanned vehicle to track the reference path obtained in step (3). Combined with the minimum boundary ellipse of the obstacle obtained in step (2), the running trajectory of the obstacle and the unmanned vehicle after N steps at the current moment is predicted. By designing a control boundary function, the unmanned vehicle is kept away from the obstacle to obtain a collision-free safe trajectory.

[0040] Furthermore, in step (4), the specific steps of "using the MPC-D-CBF algorithm to track and optimize the path to obtain the final collision-free path for the unmanned vehicle" are as follows:

[0041] Step (41): Establish the discrete-time dynamics model of the unmanned vehicle:

[0042]

[0043] Step (42): Determine the objective function and constraints of the MPC control algorithm:

[0044]

[0045] x k+1 =f(x k ,u k ), k = 0, ..., N-1, (1)

[0046]

[0047] x t|t =x t , (3)

[0048]

[0049] Formula (1) is the equation constraint of the dynamic model of the unmanned vehicle, and formula (3) is the initial condition of the algorithm.

[0050] Tracking global path x d t：t+N , The various cost items in are: Terminal cost Stage Cost Where P, Q, R, S are the corresponding weight matrices;

[0051] Step (43): Combined with the CBF algorithm, the control boundary function h(x) is designed. For the state x in the safety set, And there exists an extended K∞-like function γ that satisfies:

[0052]

[0053] The minimum ellipse is used to represent the drone, the unmanned vehicle and the obstacle, and the control boundary function is finally designed:

[0054]

[0055] where d safe For safe distance

[0056] l i Represents the distance from the center of the smallest ellipse of obstacle i to the intersection of the line connecting it and the center of the unmanned vehicle ellipse at its boundary:

[0057]

[0058] Step (44): Add D-CBF related inequality constraints (5) to the MPC control algorithm:

[0059] Δh(x k ,u k )≥-γh(x k ), k = 0, ..., N-1

[0060] Where Δh(x k ,u k ):=h(x k+1 )-h(x k ) (5).

[0061] Compared with the prior art, the present invention has the following significant advantages:

[0062] The UAV side of the present invention uses the ALOAM algorithm to locate and map the UAV, and at the same time calculates the minimum bounding ellipse of obstacles in the map, and transplants the perception part of the MPC-D-CBF algorithm to the UAV side to make up for the lack of perception information of a single unmanned vehicle; the information is transmitted to the unmanned vehicle side, and the extended Kalman filter carried by the unmanned vehicle combines the UAV posture and its own odometer information to achieve accurate positioning of the unmanned vehicle, and the ego-planner generates a reference path in the occupancy grid map shared by the UAV. Finally, the MPC-D-CBF algorithm carried by the unmanned vehicle combines the reference path and the minimum bounding ellipse to generate a collision-free planning path for the unmanned vehicle, converts the trajectory tracking problem of the unmanned vehicle into an MPC optimization problem, and adds a control boundary function as a constraint condition, which further improves the obstacle avoidance ability of the unmanned vehicle facing static and dynamic obstacles, and uses the UAV perception to make up for the problem of insufficient field of view of a single unmanned vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 This is a flow chart of the air-ground collaborative trajectory optimization algorithm of the present invention.

[0064] Figure 2 1 is a comparison diagram of the paths before and after optimization of the present invention; (a) is before optimization, and (b) is after optimization. DETAILED DESCRIPTION

[0065] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the examples are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, various equivalent forms of modifications to the present invention by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0066] An air-ground coordinated trajectory optimization algorithm with obstacle avoidance function is used for unmanned aerial vehicles to assist blind unmanned vehicles in path planning and dynamic obstacle avoidance without relying on GPS signals. The drone is responsible for the main environmental perception and reference trajectory planning, and the unmanned vehicle is responsible for combining map information with the reference trajectory for trajectory optimization. The specific steps are as follows:

[0067] 1. The drone equipped with the ALOAM algorithm estimates the odometer pose of the drone through the ALOAM algorithm. The pose parameters and odometer data of the drone are calculated through the point cloud data obtained by the laser radar. At the same time, the obstacle area is processed through the DBSCAN clustering algorithm. In order to find a cluster, DBSCAN starts from any point p in the input point cloud set and retrieves all density-reachable points from p (judged by the parameter domain size Eps and the minimum number of points required to form a cluster MinPts). If p is a core point (the number of points in the domain of p is greater than MinPts), a cluster is formed; otherwise, the point is marked as a noise point, and the next point is traversed until all points are traversed. Finally, after calculation, a group of n clusters A is obtained. obs ={O1, O2, ..., O n} and several noise points.

[0068] 2. For the lidar 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, and the corresponding expansion operation is performed on the corresponding points in the expanded occupied grid map.

[0069] At the same time, for each frame of point cloud information obtained by the DBSCAN algorithm, the minimum bounding ellipse is calculated for each cluster: for a given point set, that is, a cluster, traverse the points in the cluster, add the selected point p to the initially empty point set P, and calculate the minimum bounding ellipse Di of the points other than point p. If point p is on the boundary of the minimum bounding ellipse Di, then the minimum bounding ellipse Di is also the minimum bounding ellipse including p, and point p is added to the boundary set R; if not, the minimum bounding ellipse Di is updated by calling the recursive boundary function b_minidisk to enclose point p; the specific expression is as follows:

[0070]

[0071] When analyzing the time complexity of an algorithm, the recurrence relation can be expressed as:

[0072]

[0073] Where t(n) is the expected number of steps to process n points and c is the constant time required to compute the smallest circle.

[0074] When the number of points in the point set is reduced to 0, or the number of points in the boundary set reaches (d+3)d / 2, the minimum boundary ellipse can be directly calculated. Finally, the minimum boundary ellipse corresponding to all obstacles in the environment detected by the drone is calculated.

[0075] 3. The unmanned vehicle receives the odometer data from the drone and obtains the relative position data between the drone and the unmanned vehicle. Through the extended Kalman filter on board, the accurate position estimate of the unmanned vehicle is calculated.

[0076] For the two sets of minimum bounding ellipses obtained from the previous and next frames in step 2, use the Kuhn–Munkre s The algorithm transforms the registration problem into an optimization problem, transforms the data of the minimum bounding ellipse of the previous and next frames into a state vector, and solves the corresponding distribution matrix R ij , transform the problem into a dual problem, that is, given an n×n matrix of positive integers R = (r ij ), find the transformation j1,...,j n Among the integers 1, ..., n such that Maximum. This dual linear programming problem is described as: satisfying the following relationship:

[0077] argmin u1+...+u n +v1+...+v n

[0078] u i +v j ≧r ij

[0079] where u1, ..., u n with υ1, ..., υ n It is a non-negative integer. After finding the corresponding matching relationship, the corresponding boundary ellipse is numbered and the boundary ellipses of the previous and next frames are matched.

[0080] Finally, combined with the precise position and posture of the unmanned vehicle obtained by the extended Kalman filter, the minimum bounding ellipse corresponding to the unmanned vehicle is additionally extracted to prevent it from affecting the results of trajectory optimization.

[0081] 4. After receiving the occupancy grid map transmitted by the drone side, the precise position of the unmanned vehicle in the map is obtained through step 3. The ego-planner planner takes the current position of the unmanned vehicle as the starting point and the pre-set target as the end point to plan the initial reference path, which is described by the B sampling strip interpolation curve and the trajectory is smoothed.

[0082] 5. For the unmanned vehicle path planned by the ego-planner, the MPC-D-CBF algorithm is used for path tracking and trajectory optimization to obtain the final collision-free safety trajectory. The discrete time dynamics model is used to describe the unmanned vehicle:

[0083]

[0084] The MPC control problem is given by the receding horizon optimization:

[0085]

[0086] x k+1 =f(x k ,u k ), k = 0, ..., N-1, (1)

[0087]

[0088] x t|t =x t , (3)

[0089]

[0090] Formula (1) is the equation constraint of the dynamic model of the unmanned vehicle, and Formula (3) is the initial condition of the algorithm. In order to track the global path x d t：t+N , The various cost items in are: Terminal cost Stage Cost Where P, Q, R, S are the corresponding weight matrices.

[0091] Combined with the CBF algorithm, in order to keep the state of the unmanned vehicle in the safe set, it is necessary to design a control boundary function h(x). For the state x in the safe set, And there exists an extended K∞-like function γ that satisfies:

[0092]

[0093] The minimum ellipse is used to represent the drone, the unmanned vehicle and the obstacle, and the control boundary function is finally designed:

[0094]

[0095] where d safe is the safe distance, l i Represents the distance from the center of the smallest ellipse of obstacle i to the intersection of the line connecting it and the center of the unmanned vehicle ellipse at its boundary:

[0096]

[0097] Combining D-CBF with MPC, we add D-CBF related inequality constraints to MPC (5):

[0098] △h(x k ,u k )≥-γh(x k ), k = 0, ..., N-1

[0099] Where Δh(x k ,u k ):=h(x k+1 )-h(x k ) (5)

[0100] In summary, the present invention designs an air-ground coordinated trajectory optimization algorithm with obstacle avoidance function, which is based on the air-ground coordination system and does not rely on GPS signals. The drone provides obstacle information and precise positioning in the map to the unmanned vehicle through its own SLAM algorithm and relative posture sensor, and the unmanned vehicle plans a reference path through its own planner. On the basis of the reference path, the designed MPC-D-CBF algorithm controls the unmanned vehicle to follow the reference path, and calculates the distance between the minimum ellipse related to the obstacle and the minimum ellipse of the unmanned vehicle itself, so that the state of the unmanned vehicle is kept in the safe set of the CBF function to optimize the reference path. Therefore, the algorithm can effectively assist the unmanned vehicle that has lost the GPS signal to complete the path planning, further improve the obstacle avoidance ability of the unmanned vehicle, and provide important strategic guarantees for it.

Claims

1. An air-ground collaborative trajectory optimization algorithm with obstacle avoidance function, characterized in that: The drone is equipped with a laser radar and a relative pose estimation sensor, and is equipped with the ALOAM algorithm and the DBSCAN clustering algorithm. The unmanned vehicle is equipped with a wheel odometer and a relative pose estimation sensor, and is equipped with the MPC-D-CBF algorithm, the Kuhn-Munkres algorithm, the extended Kalman filter, and the ego-planner planner. The algorithm includes the following steps: Step (1): The drone receives point cloud information through the laser radar and pre-processes the point cloud information through the DBSCAN clustering algorithm; the drone's own odometer data is calculated through the ALOAM algorithm, and the local occupancy grid map is updated through the Raycast algorithm; Step (2): Based on the odometer data and relative pose estimation sensor data transmitted by the drone, the unmanned vehicle calculates the relative pose, and obtains the precise pose of the unmanned vehicle through the extended Kalman filter carried by the unmanned vehicle; at the same time, the minimum bounding ellipse of each point cloud cluster is calculated through the MBE algorithm, and the minimum bounding ellipse corresponding to the unmanned vehicle is estimated and eliminated through the precise pose obtained by the extended Kalman filter; Step (3): Send the occupancy grid map of the drone in step (1) to the unmanned vehicle. The unmanned vehicle restores the map and uses the ego-planner on board to perform path planning on the occupancy grid map to obtain a reference path for the unmanned vehicle. Step (4): The unmanned vehicle receives the minimum bounding ellipse information from step (2), matches the two ellipses in the previous and next frames using the Kuhn-Munkres algorithm, and performs path tracking and optimization using the MPC-D-CBF algorithm based on the reference path planned in step (3) to obtain the final collision-free path for the unmanned vehicle.

2. The algorithm according to claim 1, characterized in that In step (1), "preprocessing point cloud information by using DBSCAN clustering algorithm" is specifically as follows: Set the parameter domain size Eps of the DBSCAN clustering algorithm and the minimum number of points MinPts required to form a cluster. DBSCAN starts from any point p in the point cloud set of each frame of the input, and retrieves all density-reachable points from p. If p is a core point, that is, the number of points in the domain of p is greater than MinPts, a cluster is formed; otherwise, the point is marked as a noise point, and the next point is traversed until all points are traversed, and a set of n clusters A is obtained by integration. obs ={O1, O2, ..., O n } and several noise points.

3. The algorithm according to claim 2, characterized in that In step (2), "obtaining the precise position and posture of the unmanned vehicle through the extended Kalman filter carried by the unmanned vehicle" is specifically: In the prediction stage, the extended Kalman filter (EKF) uses the angular velocity and linear velocity provided by the wheel odometer of the unmanned vehicle as control input; initializes the covariance matrix R and Gaussian noise matrix Q, and calculates the prior covariance matrix P k|k-1 and posture information prediction value; In the update phase, the extended Kalman filter (EKF) fuses the drone odometer data and relative pose estimation sensor data to construct a measurement matrix Z containing pose information measurements. By calculating the state transfer matrix K, the extended Kalman filter EKF combines the error between the measured value and the predicted value to finally obtain the filtered posterior state estimate and the covariance matrix P k|k ; Iterate and continuously calculate the precise position and posture of the unmanned vehicle.

4. The algorithm according to claim 3, characterized in that In step (2), "calculate the minimum bounding ellipse of each point cloud cluster by using the MBE algorithm, estimate the minimum bounding ellipse corresponding to the unmanned vehicle by using the accurate pose obtained by the extended Kalman filter and remove it" is specifically as follows: For each group of n clusters of point cloud information in step (1), the minimum bounding ellipse is calculated for each cluster respectively: for a given point set, i.e., a cluster, the points in the cluster are traversed, the selected point p is added to the initially empty point set P, and the minimum bounding ellipse Di of the points other than point p is calculated. If point p is on the boundary of the minimum bounding ellipse Di, then the minimum bounding ellipse Di is also the minimum bounding ellipse including point p, and point p is added to the boundary set R; if not, the minimum bounding ellipse Di is updated to surround point p by calling the recursive boundary function bminidisk; the specific expression is as follows: The recursive relation in the recursive boundary function bminidisk is: Where t(n) is the expected number of steps to process n points, and c is the constant time required to calculate the minimum circle; When the number of points in the point set is reduced to 0, or the number of points in the boundary set R reaches (d+3)d / 2, the termination condition of the traversal is met, and Di at this time is the minimum boundary ellipse D; Calculate the set of minimum bounding ellipses D corresponding to all clusters of a set of point cloud information; The precise position and posture of the unmanned vehicle obtained by the extended Kalman filter is combined to eliminate the minimum bounding ellipse corresponding to the unmanned vehicle.

5. The algorithm according to claim 4, characterized in that Step (3) specifically includes the following steps: Step (31): Generate a B-spline curve Φ that only satisfies the terminal constraint and does not consider obstacle avoidance, i.e., a trajectory; 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 the collision-free path Γ 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 Q pointing to point p i，j The unit vector of 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 is greater than 0, it means point p i，j There are no obstacles, and the path planning is successful.

6. The algorithm according to claim 5, characterized in that The specific steps of "matching the ellipses of the previous and next frames by using the Kuhn-Munkres algorithm" in step (4) are: For the two sets of minimum bounding ellipses obtained from the previous and next frames in step (2), the Kuhn-Munkres algorithm is used to convert the data of the minimum bounding ellipses of the previous and next frames into a state vector and solve the corresponding allocation matrix R ij , for a given positive integer n×n matrix R=(r ij ), find the transformation j1,...,j n Among the integers 1, ..., n such that Maximum; as follows: argmin u1+…+u n +v1+…+v n u i +v j ≥r ij where u1, ..., u n with v1, ..., v n It is a non-negative integer. After finding the corresponding matching relationship, the corresponding boundary ellipse is numbered and the boundary ellipses of the previous and next frames are matched.

7. The algorithm according to claim 6, characterized in that In step (4), "using the MPC-D-CBF algorithm to track and optimize the path to obtain the final collision-free path for the unmanned vehicle" is specifically: After receiving the accurate position and posture of the unmanned vehicle obtained by the extended Kalman filter in step (2), the MPC algorithm is used to control the unmanned vehicle to track the reference path obtained in step (3). Combined with the minimum boundary ellipse of the obstacle obtained in step (2), the running trajectory of the obstacle and the unmanned vehicle after N steps at the current moment is predicted. By designing a control boundary function, the unmanned vehicle is kept away from the obstacle to obtain a collision-free safe trajectory.

8. The algorithm according to claim 7, characterized in that The specific steps of "using the MPC-D-CBF algorithm to track and optimize the path to obtain the final collision-free path for the unmanned vehicle" in step (4) are as follows: Step (41): Establish the discrete-time dynamics model of the unmanned vehicle: Step (42): Determine the objective function and constraints of the MPC control algorithm: s.t.x k+1 =f(x k ,u k ),k=0,…,N-1, (1) x t|t =x t , (3) Formula (1) is the equation constraint of the dynamic model of the unmanned vehicle, and formula (3) is the initial condition of the algorithm. Tracking global path x d t：t+N , The various cost items in are: Terminal cost Stage Cost Where P, Q, R, S are the corresponding weight matrices; Step (43): Combined with the CBF algorithm, the control boundary function h(x) is designed. For the state x in the safety set, And there exists an extended K∞-like function γ that satisfies: The minimum ellipse is used to represent the drone, the unmanned vehicle and the obstacle, and the control boundary function is finally designed: where d safe For safe distance l i Represents the distance from the center of the smallest ellipse of obstacle i to the intersection of the line connecting it and the center of the unmanned vehicle ellipse at its boundary: Step (44): Add D-CBF related inequality constraints (5) to the MPC control algorithm: Δh(x k ,u k )≥-γh(x k ),k=0,…,N-1 Where Δh(x k ,u k ):=h(x k+1 )-h(x k ) (5).

Images