An air-ground cooperative trajectory optimization algorithm with obstacle avoidance function

Abstract

The application is an air-ground cooperative trajectory optimization algorithm with obstacle avoidance function. The steps include: (1) the unmanned aerial vehicle receives point cloud information through laser radar, and pre-processes the point cloud information; the odometer data of the unmanned aerial vehicle is calculated, and the local occupancy grid map is updated; (2) the relative pose of the unmanned vehicle is calculated, and the accurate pose of the unmanned vehicle is obtained through the extended Kalman filter; the minimum boundary ellipse is calculated through the MBE algorithm; (3) the occupancy grid map is sent to the unmanned vehicle, and the path planning of the unmanned vehicle on the occupancy grid map is the reference path; (4) the unmanned vehicle matches the front and rear two ellipses through the Kuhn-Munkres algorithm, and according to the reference path, the path tracking and optimization are carried out through the MPC-D-CBF algorithm, and the final unmanned vehicle collision-free path is obtained. The algorithm can effectively assist the unmanned vehicle to complete the path planning when losing the GPS signal, and further improve the obstacle avoidance ability of the unmanned vehicle.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of unmanned aerial vehicle and unmanned ground vehicle air-ground collaborative system, and particularly relates to an air-ground collaborative trajectory optimization algorithm with obstacle avoidance function. BACKGROUND

[0002] In the field of modern intelligent technology, the application prospect of air-ground collaborative system is increasingly broad, especially in disaster emergency response, environmental monitoring, urban development planning and security patrol. The rapid progress of unmanned aerial vehicle (UAV) and unmanned ground vehicle (UGV) technology enables more technologies to be used for their collaborative operation, thereby achieving a result greater than the sum of its parts. In order to realize the autonomous navigation of each intelligent agent, the path planning method in the dynamic and non-structural environment is very important.

[0003] In recent years, the related research on robot autonomous navigation mainly focuses on the environmental perception and autonomous navigation of a single robot. However, concentrating both environmental perception and autonomous navigation on a single unmanned ground vehicle may cause the lack of environmental information, which affects the navigation and path planning of subsequent robots. Moreover, concentrating all sensors on the unmanned aerial vehicle or unmanned ground vehicle may reduce the load for transportation or other tasks. Some multi-robot assistance systems, such as some centralized air-ground collaborative systems, coordinate the work between different intelligent agents through a base station. However, if the base station is disturbed, the stability of the system will be affected. In the present application, the environmental perception and autonomous navigation modules are placed on the unmanned aerial vehicle and unmanned ground vehicle respectively, thereby reducing the load of the two. The distributed coordination scheme can also reduce the impact on the stability of the system after receiving interference. SUMMARY

[0004] The application aims to provide an air-ground coordination trajectory optimization algorithm with obstacle avoidance function.

[0005] The technical solution for realizing the application is as follows: an air-ground collaborative trajectory optimization algorithm with obstacle avoidance function, wherein the unmanned aerial vehicle is equipped with a laser radar and a relative pose estimation sensor, and carries an ALOAM algorithm and a DBSCAN clustering algorithm; the unmanned ground vehicle is equipped with a wheel odometer and a relative pose estimation sensor, and carries an MPC-D-CBF algorithm, a Kuhn-Munkres algorithm, an extended Kalman filter and an ego-planner planner; the algorithm comprises the following steps:

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

[0007] Step (2): According to the odometer data transmitted by the unmanned aerial vehicle and the relative pose estimation sensor data, the unmanned vehicle calculates the relative pose, and through the extended Kalman filter carried by the unmanned vehicle, the accurate pose of the unmanned vehicle is obtained; at the same time, the minimum boundary ellipse of each group of point cloud clusters is calculated through the MBE algorithm, and the minimum boundary ellipse corresponding to the accurate pose estimation of the unmanned vehicle is obtained through the extended Kalman filter and removed;

[0008] Step (3): Send the occupancy grid map of the unmanned aerial vehicle in step (1) to the unmanned vehicle, restore the map, use the ego-planner planner carried, and plan the path on the occupancy grid map to obtain the reference path of the unmanned vehicle;

[0009] Step (4): The unmanned vehicle receives the minimum boundary ellipse information in step (2), matches the two frames of ellipses through the Kuhn-Munkres algorithm, and according to the reference path planned in step (3), performs path tracking and optimization through the MPC-D-CBF algorithm to obtain the final collision-free path of the unmanned vehicle.

[0010] Further, the "preprocessing of point cloud information through DBSCAN clustering algorithm" in step (1) is specifically:

[0011] The parameters of the DBSCAN clustering algorithm, the field size Eps and the minimum number of points required to form a cluster MinPts, are set. DBSCAN starts from any point p in the input point cloud set of each frame, retrieves all density reachable points from p, and if p is a core point, i.e. the number of points in the field 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 group of n clusters A obs ={O1,O2,...,O n} and some noise points are integrated.

[0012] Further, the "obtaining accurate pose of unmanned vehicle through extended Kalman filter carried by unmanned vehicle" in step (2) 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 the control input; the covariance matrix R and the Gaussian noise matrix Q are initialized, and the prior covariance matrix P k|k-1 and the predicted value of the pose information are calculated;

[0014] In the update stage, the extended Kalman filter EKF fuses the odometer data of the unmanned aerial vehicle and the relative pose estimation sensor data to construct a measurement matrix Z containing the measured value of the pose information;

[0015] By calculating the state transition matrix K, the extended Kalman filter (EKF) combines the error between the measurement and the prediction to finally obtain the filtered posterior state estimation and the covariance matrix P k|k ;

[0016] The precise pose of the unmanned vehicle is continuously calculated through iteration.

[0017] Further, the "calculating the minimum bounding ellipse of each group of point cloud clusters by the MBE algorithm, obtaining the minimum bounding ellipse corresponding to the precise pose estimation of the unmanned vehicle by the extended Kalman filter, and removing" in step (2) specifically comprises:

[0018] For each group, i.e., each frame of point cloud information, n clusters, the minimum bounding ellipse of each cluster is calculated 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, the minimum bounding ellipse Di of the points except 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 p, point p is added to the boundary set R; if not, the recursive boundary function b_minidisk is called to update the minimum bounding ellipse Di to enclose point p; the specific expression is as follows:

[0019]

[0020] The recursive relationship 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 a constant time required for calculating 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, i.e., the termination condition of traversal is met, Di at this time is the minimum bounding ellipse D;

[0024] The set of minimum bounding ellipses D corresponding to all clusters of a group of point cloud information is calculated.

[0025] Combined with the precise pose of the unmanned vehicle obtained by the extended Kalman filter, the minimum bounding ellipse corresponding to the unmanned vehicle is removed.

[0026] Further, step (3) specifically comprises the following steps:

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

[0028] Step (32): Iterate through 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 tangent vector R i Passing through control point Q i Tangent vector R i The vertical plane Ψ intersects with the collision-free path Г to form line I, and line I intersects with the obstacle surface j at point p. i,j At the same time, the repulsive force direction vector v is obtained. i,j v is the point p that Q points to. i,j , unit vector;

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

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

[0032] If d i，j If the value is greater than 0, it indicates that point p i,j The path planning was successful as the path was not within the obstacles.

[0033] Furthermore, step (4) of "matching the ellipses between the two frames using the Kuhn-Munkres algorithm" specifically means:

[0034] For the two sets of minimum boundary ellipses obtained in the two frames before and after step (2), the Kuhn–Munkres algorithm is used to convert the data of the minimum boundary ellipses in the two frames before and after into state vectors, and the corresponding allocation matrix R is solved. ij For a given n×n matrix R = (r ij Find the transformations j1, ..., j n In integers 1, ..., n, such that Maximum; details are as follows:

[0035] argmin u1+...+u n +v1+…+v n

[0036] u i +v j ≧r ij

[0037] Where u1,...,u n With v1,...,v nFor non-negative integer, find the corresponding matching relationship, and number the corresponding boundary ellipse. Match the boundary ellipses of the previous and next two frames.

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

[0039] After receiving the accurate pose of the unmanned vehicle obtained by the extended Kalman filter in step (2), the reference path obtained in step (3) is used to control the unmanned vehicle to track the reference path by the MPC algorithm, and the minimum boundary ellipse of the obstacle obtained in step (2) is used to predict the running track of the obstacle and the unmanned vehicle after N steps at the current time, so as to keep the distance between the unmanned vehicle and the obstacle by designing the control boundary function, and obtain the collision-free safe track.

[0040] Further, the step (4) "path tracking and optimization by MPC-D-CBF algorithm to obtain the final collision-free path of the unmanned vehicle" is specifically as follows:

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

[0042]

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

[0044]

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

[0046]

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

[0048]

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

[0050] Track the global path x d t：t+N , Each cost term in (1) is respectively: terminal cost stage cost Where P, Q, R and S are the corresponding weight matrices.

[0051] Step (43): Design the control boundary function h(x) combined with the CBF algorithm, for the state x in the safe set, there is And there is an extended class K∞ function γ, which satisfies:

[0052]

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

[0054]

[0055] Where d safe is the safety distance

[0056] l i represents the distance from the center of the minimum ellipse of the obstacle i to the intersection point of its boundary and the line connecting the center of the unmanned vehicle ellipse:

[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 application has the following advantages:

[0062] The UAV side of the present application uses the ALOAM algorithm to locate and map the UAV, and calculates the minimum boundary ellipse of the obstacles in the map, 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, transmits the information to the unmanned vehicle side, and the extended Kalman filter carried by the unmanned vehicle combines the pose of the UAV and the odometer information of the unmanned vehicle to achieve accurate positioning of the unmanned vehicle, 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 boundary 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 the control boundary function as a constraint condition, further improving the obstacle avoidance ability of the unmanned vehicle in the face of static and dynamic obstacles, and using the UAV perception to make up for the lack of field of view of a single unmanned vehicle. Attached Figure Description

[0063] Figure 1 This is a flowchart of the air-ground cooperative trajectory optimization algorithm of the present invention.

[0064] Figure 2 This is a comparison diagram of the paths before and after optimization according to the present invention; the left side is before optimization, and the right side is after optimization. Detailed Implementation

[0065] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the examples are for illustrative purposes only and are not intended to limit the scope of the invention. After reading the present invention, any modifications of the present invention in various equivalent forms by those skilled in the art fall within the scope defined by the appended claims.

[0066] An air-ground coordinated trajectory optimization algorithm with obstacle avoidance capabilities is proposed. This algorithm enables unmanned vehicles (UAVs) to assist blind drivers in path planning and dynamic obstacle avoidance without relying on GPS signals. The UAV is responsible for environmental perception and reference trajectory planning, while the UAV is responsible for trajectory optimization by combining map information and the reference trajectory. The specific steps are as follows:

[0067] 1. A drone equipped with the ALOAM algorithm performs odometry pose estimation using ALOAM. Point cloud data obtained from LiDAR is used to calculate the drone's pose parameters and odometry data. Simultaneously, the DBSCAN clustering algorithm is used to process obstacle regions. To find a cluster, DBSCAN starts from any point p in the input point cloud set and searches for all density-reachable points from p (determined by the neighborhood size Eps and the minimum number of points (MinPts) required to form a cluster). If p is a core point (the number of points in p's neighborhood is greater than MinPts), a cluster is formed; otherwise, the point is marked as noise, and the process continues until all points have been traversed. Finally, after calculation, a set of n clusters A is obtained. obs ={O1, O2, ..., O n} and several noise points.

[0068] 2. For the lidar point cloud data acquired by the UAV, the Raycast algorithm is used to receive the preprocessed point cloud message, and then to traverse each point in the point cloud set and perform the corresponding expansion operation on the corresponding points in the expanded grid map.

[0069] Meanwhile, for each of the n clusters of point cloud information of each frame obtained by the DBSCAN algorithm, a 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, the minimum bounding ellipse Di of the points except 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 p, 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:

[0070]

[0071] In the analysis of the time complexity of the algorithm, the recursive relationship 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 calculate the minimum 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 bounding ellipse can be directly calculated. Finally, the minimum bounding ellipses corresponding to all the obstacles in the environment detected by the UAV are calculated.

[0075] 3. The unmanned vehicle receives the odometer data of the UAV and obtains the relative pose data of the UAV and the unmanned vehicle, and calculates the accurate unmanned vehicle pose estimation through the extended Kalman filter.

[0076] For the two sets of minimum bounding ellipses obtained in step 2, the Kuhn-Munkres algorithm is used to convert the registration problem into an optimization problem, and the data of the minimum bounding ellipses of the two frames are converted into state vectors to solve the corresponding assignment matrix R ij , which converts the problem into a dual problem, i.e. given a positive integer n x n matrix R = (r ij ), find the transformation j1,..., j n In integers 1,..., n such that is maximum. This dual linear programming problem is described as follows:

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

[0078] u i +v j ≧r ij

[0079] where u1,...,u n and v1,...,v n are non-negative integers, find the corresponding matching relationship, and number the corresponding boundary ellipse. Match the boundary ellipses of the two frames.

[0080] Finally, the precise pose of the unmanned vehicle obtained by the extended Kalman filter is combined to extract the corresponding minimum boundary ellipse of the unmanned vehicle to prevent affecting the results of trajectory optimization.

[0081] 4. After receiving the occupancy grid map transmitted by the unmanned aerial vehicle side, the precise pose of the unmanned vehicle in the map is obtained by step 3, and the ego-planner planner is used to plan an initial reference path with the current unmanned vehicle position as the starting point and the pre-set target as the ending point. The path is described by a B-spline curve and is smoothed.

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

[0083] is used to describe the unmanned vehicle:

[0084]

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

[0086]

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

[0088]

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

[0090]

[0091] (1) is the dynamics model equation constraint of the unmanned vehicle, and (3) is the initial condition of the algorithm. In order to track the global path x d t:t+N , Each cost term in (2) is respectively: terminal cost stage cost where P, Q, R, and S are the corresponding weight matrices.

[0092] In combination 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, there is And there is an extended class K∞ function γ, which satisfies:

[0093]

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

[0095]

[0096] Where d safe is the safety distance, l i represents the distance from the center of the minimum ellipse of the obstacle i to the intersection point of its boundary and the line connecting the center of the unmanned vehicle ellipse:

[0097]

[0098] In combination with D-CBF and MPC, the inequality constraint (5) related to D-CBF is added to MPC:

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

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

[0101] In summary, the application 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 unmanned aerial vehicle provides obstacle information in the map and accurate positioning for the unmanned vehicle through the SLAM algorithm and relative pose sensor carried by itself. The planner of the unmanned vehicle plans a reference path. On the basis of the reference path, the MPC-D-CBF algorithm designed 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, and the reference path is optimized. Therefore, the algorithm can effectively assist the unmanned vehicle to complete path planning when losing GPS signals, further improve the obstacle avoidance ability of the unmanned vehicle, and provide important strategic guarantee.

Claims

1. An air-ground cooperative trajectory optimization algorithm with obstacle avoidance function, characterized in that, The unmanned aerial vehicle is equipped with a laser radar and a relative pose estimation sensor, and carries an ALOAM algorithm and a DBSCAN clustering algorithm, the unmanned vehicle is equipped with a wheel odometer and a relative pose estimation sensor, and carries an MPC-D-CBF algorithm, a Kuhn-Munkres algorithm, an extended Kalman filter and an ego-planner planner; the algorithm comprises the following steps: Step (1): the unmanned aerial vehicle receives point cloud information through the laser radar, and pre-processes the point cloud information through the DBSCAN clustering algorithm; the odometer data of the unmanned aerial vehicle is calculated through the ALOAM algorithm, and the local occupancy grid map is calculated and updated through the Raycast algorithm; Step (2): according to the odometer data and the relative pose estimation sensor data transmitted by the unmanned aerial vehicle, the relative pose of the unmanned vehicle is calculated, and the accurate pose of the unmanned vehicle is obtained through the extended Kalman filter carried by the unmanned vehicle; at the same time, the minimum boundary ellipse of each group of point cloud clusters is calculated through the MBE algorithm, and the minimum boundary ellipse corresponding to the accurate pose estimation of the unmanned vehicle is obtained through the extended Kalman filter and removed; Step (3): the occupancy grid map of the unmanned aerial vehicle in step (1) is sent to the unmanned vehicle, the unmanned vehicle restores the map, and uses the ego-planner planner carried to plan a path on the occupancy grid map, to obtain a reference path of the unmanned vehicle; Step (4): the unmanned vehicle receives the minimum boundary ellipse information in step (2), matches the front and rear two frames of ellipses through the Kuhn-Munkres algorithm, plans a reference path according to step (3), and performs path tracking and optimization through the MPC-D-CBF algorithm, to obtain a final collision-free path of the unmanned vehicle.

2. The algorithm of claim 1, wherein, In step (1), the "pre-processing of the point cloud information through the DBSCAN clustering algorithm" specifically comprises: The DBSCAN clustering algorithm is set with a parameter field size Eps and a minimum number of points required to form a cluster MinPts. DBSCAN starts from an arbitrary point p in the point cloud set of each input frame, retrieves all density-reachable points from p, and if p is a core point, i.e., the number of points in the field of p is greater than MinPts, a cluster is formed; otherwise, the point is marked as a noise point, and the next point is continued to be traversed until all points are traversed, and a group of n clusters A is obtained obs = {O1, O2,..., On} n and a number of noise points.

3. The algorithm of claim 2, wherein, In step (2), the "accurate pose of the unmanned vehicle obtained through the extended Kalman filter carried by the unmanned vehicle" specifically comprises: 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 the Gaussian noise matrix Q, and calculates the prior covariance matrix P k|k-1 and the predicted pose information In the update phase, the extended Kalman filter EKF fuses the odometer data and the relative pose estimation sensor data of the unmanned aerial vehicle, and constructs a measurement matrix Z containing the measured value of the pose information; By computing the state transition matrix K, the extended Kalman filter EKF combines the error between the measurement and the prediction to finally obtain the filtered posterior state estimate and the covariance matrix P k|k ; The accurate pose of the unmanned vehicle is continuously calculated through iteration.

4. The algorithm of claim 3, wherein, In step (2), the "minimum boundary ellipse of each group of point cloud clusters calculated through the MBE algorithm, and the minimum boundary ellipse corresponding to the accurate pose estimation of the unmanned vehicle obtained through the extended Kalman filter and removed" specifically comprises: For each group, i.e. each frame of point cloud information in step (1), the minimum boundary ellipse of each cluster is calculated: for a given point set, i.e. a cluster, the selected point p is added to the initially empty point set P, the minimum boundary ellipse Di of the points except point p is calculated, if point p is on the boundary of the minimum boundary ellipse Di, then the minimum boundary ellipse Di is also the minimum boundary ellipse including p, and point p is added to the boundary set R; if not, the recursive boundary function b_minidisk is called to update the minimum boundary ellipse Di to enclose point p; the specific expression is as follows: The recursive relationship in the recursive boundary function b_minidisk is: Wherein, t(n) is the expected number of steps of processing n points, c is a constant time required for calculating 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, that is, the termination condition of traversal is met, Di at this time is the minimum boundary ellipse D; Calculate the set of minimum boundary ellipses D corresponding to all clusters of the point cloud information; Combined with the accurate pose of the unmanned vehicle obtained by the extended Kalman filter, the minimum boundary ellipse corresponding to the unmanned vehicle is removed.

5. The algorithm of claim 4, wherein, Step (3) specifically comprises the following steps: Step (31): generate a B-spline curve Ф that only satisfies the terminal constraint and does not consider obstacle avoidance, that is, a trajectory; Step (32): iterate each collision segment, and generate a collision-free path Г using the A* algorithm; Step (33): For each control point Q on the trajectory Ф i tangent vector R i Passing through control point Q i Tangent vector R i The vertical plane Ψ intersects with the collision-free path Г to form line I, and line I intersects with the obstacle surface j at point p. i，j At the same time, the repulsive force direction vector v is obtained. i，j v is the point p that Q points to. i，j , unit vector; Step (34): Define distance field d from (p, v) pair i，j Compute: d i，j = (Q i -p i,j ) · v i，j If d i，j is greater than 0, it means that the point p i，j is not inside the obstacle, and the path planning is successful.

6. The algorithm of claim 5, wherein, The "matching the front and rear two frames of ellipses by Kuhn-Munkres algorithm" in step (4) is specifically: For the two sets of minimum boundary ellipse obtained in step (2), using Kuhn-Munkres algorithm, the data of the minimum boundary ellipse of the two frames is converted into state vectors, and the corresponding assignment matrix R is solved ij , for a given n x n matrix R = (r ij ) of positive integers, find a permutation j n 1,..., j n such that max; in particular as follows: argmin u1+...+u n +v1+...+v n u i +v j ≧r ij where u1,...,u n and v1,...,v n are non-negative integers, find the corresponding matching relationship, and number the corresponding boundary ellipse. Match the boundary ellipses of the previous and next two frames.

7. The algorithm of claim 6, wherein, The "path tracking and optimization by MPC-D-CBF algorithm to obtain the final collision-free path of the unmanned vehicle" in step (4) is specifically: After receiving the accurate pose of the unmanned vehicle obtained by the extended Kalman filter in step (2), the reference path obtained in step (3) is used to control the unmanned vehicle to track the reference path by the MPC algorithm, and the minimum boundary ellipse of the obstacle obtained in step (2) is combined to predict the running trajectory of the obstacle and the unmanned vehicle after N steps at the current time, so that the unmanned vehicle and the obstacle maintain a distance by designing a control boundary function, and a collision-free safe trajectory is obtained.

8. The algorithm of claim 7, wherein, The "path tracking and optimization by MPC-D-CBF algorithm to obtain the final collision-free path of the unmanned vehicle" in step (4) is specifically as follows: Step (41): establish a discrete-time dynamic model of the unmanned vehicle: Step (42): determine the objective function and constraint conditions 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) Equation (1) is the dynamic model equation constraint of the unmanned vehicle, equation (3) is the initial condition of the algorithm, Tracking global path x d t：t+N , Each of the cost terms in (1) is given as follows: Stage cost where P, Q, R, S are the corresponding weight matrices. Step (43): Design the control boundary function h(x) in combination with the CBF algorithm, for the state x in the safe set, there is and there is an extended class K∞ function γ that satisfies: The minimum ellipse is used to represent the unmanned aerial vehicle, the unmanned vehicle and the obstacle, and finally the control boundary function is designed: where d safe is a safety distance l i distance of the center of the smallest ellipse representing obstacle i to the intersection of its boundary with the line connecting the centers of the ellipses of the unmanned vehicle: Step (44): add the inequality constraint (5) related to D-CBF to the MPC control algorithm: Δh(x k , u k ) ≥ -γh(x k ), k = 0,..., N - 1 where Ah(x k , u k ):= h(x k+1 )- h(x k ) (5).

Images