Unmanned aerial vehicle obstacle avoidance method based on obstacle state prediction

Abstract

The invention discloses an unmanned aerial vehicle obstacle avoidance method based on obstacle state prediction. The method comprises the following steps: acquiring an environment depth image by using a depth camera and performing point cloud conversion; cutting and filtering the original point cloud information; clustering the filtered point cloud data to obtain the position and size information of a single obstacle; tracking and matching the same obstacle in continuous frames of the camera, and recording historical state information; designing an adaptive Kalman filtering algorithm, and predicting a future motion state based on the historical state information of the obstacle; based on the predicted motion state of the obstacle, an unmanned aerial vehicle speed obstacle avoidance algorithm in a three-dimensional space is designed; and re-planning a flight path based on the calculated speed direction for avoiding the obstacle.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) perception and obstacle avoidance, and in particular to a UAV obstacle avoidance method based on obstacle state prediction. Background Art

[0002] With the rapid development of drone technology, drones have been widely used in many fields such as industry, agriculture, logistics, and security. However, with the complexity of application scenarios, drones often face complex environments and dynamic obstacles during flight, such as buildings, trees, pedestrians, etc. The uncertainty and dynamics of these obstacles pose great challenges to the safe flight of drones.

[0003] Traditional obstacle avoidance methods for drones usually make obstacle avoidance decisions based on real-time sensor data and preset rules. However, these methods often rely on the construction of a global map and the instantaneous perception of obstacles, and are unable to effectively handle moving obstacles or predict the future state of obstacles, resulting in low obstacle avoidance efficiency and even possible obstacle avoidance failure.

[0004] Therefore, real-time monitoring of the movement status of obstacles within the field of view during the flight of the drone and making reasonable predictions of the movement status of the obstacles will be very helpful for the drone to make evasive actions in advance and ensure flight safety. Summary of the invention

[0005] The purpose of the present invention is to propose a UAV obstacle avoidance method based on obstacle state prediction in order to ensure that the UAV can accurately avoid obstacles, especially dynamic obstacles, during flight. The present invention is divided into two stages. The first stage is to detect obstacles and predict their motion states, and the second stage is to perform obstacle avoidance according to the predicted obstacle motion states.

[0006] The present invention adopts the following technical scheme:

[0007] The camera can be an RGB-D depth camera, a binocular camera, or other camera that can obtain depth information of the environment.

[0008] Furthermore, the acquired depth image is converted into a point cloud.

[0009] Furthermore, the converted original point cloud is preprocessed to obtain a target point cloud suitable for calculation.

[0010] The point cloud preprocessing operation includes point cloud clipping and point cloud filtering.

[0011] Furthermore, a clustering operation is performed on the processed target point cloud.

[0012] The point cloud clustering method may be a K-means clustering method or a DBSCAN clustering method.

[0013] Furthermore, the center position and size information of each cluster are calculated.

[0014] The method for calculating the center position of each cluster is:

[0015]

[0016] Among them, p x 、p y 、p z Respectively represent the coordinate values ​​of the cluster center position on the x, y, and z axes, x i ,y i 、z i Represent the coordinate values ​​of each point cloud in the cluster on the x, y, and z axes respectively.

[0017] The method for calculating each cluster size information is as follows:

[0018] Assume that the minimum coordinate value of all points in each cluster is (x min ,y min ,z min ), the maximum coordinate value is (x max ,y max ,z max ), calculate a cubic anchor box that can contain all point clouds of the cluster, and the size information of the anchor box is:

[0019] Length: l x =|x max -x min |

[0020] Width: l y =|y max -y min |

[0021] Height: l z =|z max -z min |

[0022] The anchor frame size information is the size information of the cluster.

[0023] Furthermore, the clustered obstacles are distinguished into static obstacles and dynamic obstacles.

[0024] The specific method of distinguishing static and dynamic obstacles is: setting a distance threshold d min and sliding window threshold k min , for the same obstacle, when passing k min After the frame, if the displacement distance exceeds d min, it is marked as a dynamic object, otherwise it is regarded as a static object and the moving speed of the static object is set to 0.

[0025] Furthermore, the location and size information of the cluster of the same dynamic object in each frame is recorded.

[0026] For each frame, the set of all dynamic object clusters is represented as C = {c 1 ,c 2 ,...,c m}, each cluster represents an obstacle and contains the location and size information of each point in the cluster.

[0027] Calculate the cost of the current cluster at time t and all clusters in the previous frame, expressed as Euclidean distance:

[0028]

[0029] Among them, i represents the index of a cluster in the current frame, j represents the index of all clusters in the previous frame, and △t represents the time interval between the current frame and the previous frame.

[0030] Construct the cost matrix Cost = (c ij ), the Hungarian algorithm is used to solve the optimal match in the cost matrix Cost, and a new index is assigned to the current cluster that does not match any existing cluster.

[0031] For clusters matched to the same index in adjacent frames, the location and anchor box size information of the cluster in each frame are recorded.

[0032] Furthermore, for each dynamic obstacle, an adaptive Kalman filter is used to predict the future motion state based on its historical position information.

[0033] The adaptive Kalman filter is specifically implemented as follows:

[0034] The state variable in the adaptive Kalman filter algorithm is selected as x = [p x ,p y ,p z ,v x ,v y ,v z ], which is expressed as position and velocity information in the x, y, and z axis directions.

[0035] The state equation and measurement equation describing the motion of dynamic obstacles are:

[0036] Equation of state:

[0037] x k+1 =Ax k +vk

[0038] Measurement equation:

[0039] z k =Hx k +w k

[0040] Among them, A is the state transfer matrix, H is the measurement matrix, k is the discrete state quantity, v k is the process noise, w k To measure the noise, v k 、w k All satisfy the Gaussian distribution:

[0041] v k ~N(0,Q)

[0042] w k ~N(0,R)

[0043] Q represents the process noise covariance matrix, and R represents the measurement noise covariance matrix.

[0044] The prediction and update equations of the adaptive Kalman filter can be expressed as:

[0045] Prediction steps:

[0046]

[0047] Update steps:

[0048]

[0049] Among them, P represents the error covariance matrix, E represents the residual, and K represents the Kalman gain.

[0050] The specific method of adaptively updating the process noise covariance matrix Q and the measurement noise covariance matrix R is as follows: Assuming that each state variable is independent of each other, the residual vector is taken as:

[0051]

[0052] Residual covariance matrix:

[0053]

[0054] The diagonal elements of the residual covariance matrix form the vector:

[0055]

[0056] The diagonal element vector of the process noise covariance matrix:

[0057]

[0058] The diagonal element vector of the measurement noise covariance matrix:

[0059]

[0060] Residual threshold vector:

[0061]

[0062] Perform the update:

[0063]

[0064] in, represents the position residual in the x direction, represents the position residual in the y direction, represents the position residual in the z direction, represents the velocity residual in the x direction, represents the velocity residual in the y direction, represents the velocity residual in the z direction. represents the variance of the position measurement error in the x direction,

[0065] represents the variance of the position measurement error in the y direction, represents the variance of the position measurement error in the z direction, represents the variance of the velocity measurement error in the x direction, represents the variance of the velocity measurement error in the y direction, Represents the variance of the position measurement error in the z direction. represents the position process noise variance in the x direction, represents the position process noise variance in the y direction, represents the position process noise variance in the z direction, represents the velocity process noise variance in the x direction, Represents y

[0066] The speed process noise variance in the direction, represents the velocity process noise variance in the z direction. represents the position measurement noise variance in the x direction, represents the position measurement noise variance in the y direction,

[0067] represents the position measurement noise variance in the z direction, represents the velocity measurement noise variance in the x direction,

[0068] represents the velocity measurement noise variance in the y direction, represents the velocity measurement noise variance in the z direction.

[0069] Represents the position residual threshold in the x direction, Represents the position residual threshold in the y direction, Indicates the position residual threshold in the z direction. represents the velocity residual threshold in the x direction, represents the velocity residual threshold in the y direction, Represents the velocity residual threshold in the z direction, and the value of i is p x 、p y ,

[0070] p z , the value of j is v x 、v y 、v z , η is the custom coefficient.

[0071] Furthermore, obstacle checks are performed within the drone's field of view.

[0072] Furthermore, if a dynamic obstacle is detected, dynamic obstacle avoidance is performed.

[0073] Furthermore, based on the predicted obstacle status information, the speed obstacle method is used to plan the avoidance speed.

[0074] Based on the predicted obstacle state information, the speed obstacle method is used to plan the avoidance speed. When the speed obstacle method is used to plan the speed, the residual The position residual in is used to dilate the obstacles, specifically:

[0075] Assume that the size of the anchor box containing the obstacle at time k is (l x ,l y ,l z ), the residual is Then the size of the expanded obstacle anchor frame is

[0076] Furthermore, a flight trajectory from the current position to the destination is replanned based on the avoidance speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 This is a flowchart of the implementation of the obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction of the present invention;

[0078] Figure 2 This is a point cloud clustering and obstacle state prediction effect diagram of the present invention;

[0079] Figure 3 This is a diagram showing the effect of implementing the speed barrier method of the present invention;

[0080] Figure 4 This is a diagram showing the effect of path replanning based on avoidance speed of the present invention. DETAILED DESCRIPTION

[0081] The technical solution of the present invention is described in detail below in conjunction with specific drawings. The embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0082] like Figure 1 As shown, the obstacle avoidance method for a UAV based on obstacle state prediction described in the present invention comprises the following steps: depth image acquisition, point cloud conversion, point cloud preprocessing, point cloud clustering, dynamic obstacle segmentation, dynamic obstacle state prediction based on an adaptive Kalman filter, speed planning based on a speed obstacle method, and flight trajectory replanning.

[0083] The depth image acquisition described in this embodiment adopts Intel D435i depth camera, with a measurement depth range of 0.3m-3m, a depth field of view of 87°×58°, a depth output resolution of 1280×720, a depth frame rate of 90fps, an RGB frame resolution of 1920×1080, and an RGB frame rate of 30fps.

[0084] The point cloud conversion described in this embodiment follows the imaging principle of a depth camera.

[0085] The point cloud preprocessing described in this embodiment includes point cloud clipping and point cloud filtering modules, wherein the point cloud filtering adopts a voxel filtering method.

[0086] The point cloud clustering method used in this embodiment is DBSCAN clustering.

[0087] like Figure 2 As shown, the cube anchor box is the clustered obstacle, and the center of the anchor box is The anchor box size of the obstacle is (l x ,l y ,l z ).

[0088] Static and dynamic obstacles are distinguished from the clustered obstacles. In this embodiment, the sliding window threshold k used to determine whether the obstacle is moving is min =20, distance threshold d min =0.1(m).

[0089] Calculate the cost of all clusters in the current frame and all clusters in the previous frame at time t one by one:

[0090] like Figure 2 As shown, there are three clusters in the previous frame, namely There are four clusters in the current frame, namely The cost calculation formula is:

[0091]

[0092] The cost matrix is:

[0093]

[0094] The Hungarian algorithm is used to calculate the optimal match between two adjacent frames. The cost matrix Cost is used as input, and the matching score s of any cluster in the current frame and all clusters in the previous frame is output. ij , 0≤s ij ≤1.

[0095] For example, suppose the cluster The matching scores with all clusters in the previous frame are:

[0096]

[0097] Take the match with the highest score as the best match, that is, the match in the current frame and the previous frame Clusters that are considered to be the same obstacle.

[0098] If the matching scores are all less than 0.5, it is considered that no existing clusters are matched and it is considered as a newly appeared cluster. In this embodiment, It is considered as a newly emerged cluster.

[0099] Create a separate container Vector for each cluster of dynamic obstacles i , used to store its position and size information in each frame.

[0100] For each dynamic obstacle, an adaptive Kalman filter is used to predict the future motion state based on its historical position information.

[0101] The state variable in the adaptive Kalman filter algorithm is selected as x = [p x ,p y ,p z ,v x ,v y ,v z ], which is expressed as position and velocity information in the x, y, and z axis directions.

[0102] The state equation and measurement equation describing the motion of dynamic obstacles are:

[0103] Equation of state:

[0104] x k+1 =Ax k +v k

[0105] Measurement equation:

[0106] z k =Hx k +w k

[0107] Among them, A is the state transfer matrix, H is the measurement matrix, k is the discrete state quantity, v k is the process noise, w k To measure the noise, v k 、w k All satisfy the Gaussian distribution:

[0108]

[0109] Q represents the process noise covariance matrix, and R represents the measurement noise covariance matrix.

[0110] Among them, A, H, Q, and R are initialized as:

[0111]

[0112]

[0113] Among them, △t is the interval time between two adjacent frames, which is determined by the computational efficiency of the algorithm on the actual computing power platform.

[0114] Assuming that the current moment is the kth moment, execute the prediction steps of the adaptive Kalman filter:

[0115]

[0116] It is the prior prediction value of the dynamic obstacle at time k.

[0117] Perform the update step of the adaptive Kalman filter to prepare for the next prediction:

[0118]

[0119] Adaptively update Q and R based on the difference between the actual measured value and the predicted value to achieve more accurate prediction:

[0120] Assuming that the state variables are independent of each other, the residual vector is taken as:

[0121]

[0122] Residual covariance matrix:

[0123]

[0124] The diagonal elements of the residual covariance matrix form the vector:

[0125]

[0126] The diagonal element vector of the process noise covariance matrix:

[0127]

[0128] The diagonal element vector of the measurement noise covariance matrix:

[0129]

[0130] The residual threshold vector in this example is taken as:

[0131] λ k =[10 10 10 10 10 10].

[0132] Perform the update:

[0133]

[0134] Among them, the value of i is p x 、p y 、p z , the value of j is v x 、v y 、v z , η is a custom coefficient, η=2 in this example.

[0135] Based on the predicted obstacle status information, the speed obstacle method is used to plan the optimal avoidance speed, such as Figure 3 As shown. Figure 3 As shown in Figure 1, (a) is a schematic diagram of the motion of the drone and obstacles in three-dimensional space, and the position coordinates of the drone are P A , speed is V A , the obstacle position coordinates are P O , speed is V O (b) The figure is a schematic diagram of the speed obstacle method on the xy plane. When executing the speed obstacle method, the position residual generated in the prediction stage

[0123] is used to expand the obstacle.

[0136] Define the relative position as:

[0137] P rel =P A -P O

[0138] The relative speed is defined as:

[0139] V rel =V A -VO

[0140] Speed ​​barriers are defined as a set of speed vectors that will cause a collision with obstacles in the future. Assuming that the drone reaches the safety boundary with the obstacle after time t, the collision condition is:

[0141] P rel +t·V rel =R

[0142] in,

[0143] The two rays starting from the center of the drone form a fan-shaped area tangent to the obstacle. This area is the speed obstacle area, denoted as VO. When the relative speed vector V rel When the end point falls in the VO area, the drone will collide with the obstacle.

[0144] The feasible velocity vector V to avoid obstacles safe Should meet:

[0145]

[0146] Among them, VO(P rel ,V O ,R) indicates that P rel is the vertex, the direction is the relative velocity direction of the obstacle, and the opening angle is all the velocity vectors in the velocity space determined by the safety distance.

[0147] The coordinates of the end point of the UAV flight are recorded as P goal , the previous position coordinate is P A , the maximum flight speed of the drone is V max In this embodiment, V max =3(m / s). Then the expected flight speed of the drone is

[0148] Planning speed V plan Should meet:

[0149] V plan = {V safe |min(||V des -V safe ||)}

[0150] That is, take all V safe Middle and V des The closest speed is taken as the planned speed V plan .

[0151] like Figure 4As shown, a flight trajectory from the current point to the target point is replanned based on the avoidance speed, and the path planning method can be a path planning algorithm such as A*, Dijkstra or RRT.

[0152] In one embodiment, a method for distinguishing static and dynamic obstacles from clustered obstacles includes: using a high-precision map as a reference, comparing obstacle data collected by a sensor (such as a laser radar) at different times with the obstacle position on the map. The position of a static obstacle will not change over time on the map, while the position of a dynamic obstacle will change over time. By analyzing the change in the obstacle position, it can be determined whether it is a dynamic obstacle.

[0153] In one embodiment, the method for distinguishing static and dynamic obstacles from clustered obstacles includes: tracking the clustered obstacles in consecutive frames and recording their motion information such as position and speed. According to the motion trajectory and speed change of the obstacle, it is determined whether it is a dynamic obstacle. For example, if the position of the obstacle changes significantly in consecutive frames and the speed is not zero, it is likely to be a dynamic obstacle.

[0154] In one embodiment, the point cloud data collected by a sensor such as a laser radar has a spatial distribution feature. For a static obstacle, its point cloud data should remain relatively stable in multiple consecutive frames; while for a dynamic obstacle, its point cloud data will change significantly between different frames.

[0155] In one embodiment, the static or dynamic properties of the obstacle are determined by analyzing the stability and variability of the point cloud data.

[0156] In one embodiment, cluster analysis is performed on point cloud data of consecutive frames to observe the stability of the clustering results. If the clustering results remain stable between different frames, the obstacle is likely to be static; if the clustering results change significantly, the obstacle is likely to be dynamic.

[0157] In one embodiment, the high-precision map provides detailed information about the road environment, including road structure, obstacle locations, etc. By matching sensor data with the high-precision map, static and dynamic obstacles can be distinguished more accurately.

[0158] In one embodiment, the dynamic model is used to predict the future motion state of the obstacle. If the prediction result is consistent with the sensor data, the obstacle is likely to be static; if the prediction result is significantly different from the sensor data, the obstacle is likely to be dynamic.

[0159] In one embodiment, data from different sensors (such as lidar, camera, etc.) are fused to improve the accuracy and reliability of obstacle detection. By integrating information from different sensors, the static or dynamic properties of obstacles can be more accurately determined.

[0160] In one embodiment, the method for recording the position and size information of the obstacle in each frame includes: when multiple sensors (such as laser radar, camera, etc.) work together, it is necessary to synchronize the input laser point cloud or image data in time to ensure the time consistency of each frame data. The sensor is calibrated with external parameters to obtain the relative position and posture information between the sensors, so that the data of different sensors can be fused into the same coordinate system. For the point cloud data collected by the laser radar, downsampling processing is performed to reduce the amount of data, and noise points are removed to improve the efficiency and accuracy of subsequent processing. The point cloud data is sliced ​​and all data outside a certain range around the obstacle are discarded to reduce the consumption of computing resources. The ground point cloud is segmented from the point cloud data to obtain road obstacle information. This can be achieved by constructing a top view based on a plane grid, model fitting, and surface element mesh region growth. The point cloud on the ground is segmented using a clustering algorithm (such as Euclidean clustering, density clustering, superbody clustering, etc.) to form multiple clusters, each cluster representing an obstacle. If category information is required, feature extraction and classifiers (such as support vector machines, neural networks, etc.) can be used to classify obstacles. Fit a bounding box for each obstacle cluster to determine a three-dimensional bounding box that can wrap the point cloud cluster for subsequent processing and tracking. Calculate the center point, center of mass point, length, width, height and other attribute information of the obstacle. This information is crucial for subsequent obstacle tracking and obstacle avoidance planning. Construct a Kalman filter for each obstacle for tracking and smoothly output the position and size information of the obstacle. This helps to maintain the consistency of obstacle information between consecutive frames. Record the position and size information of the obstacles detected in each frame, which can be stored in a database or written to a file for subsequent analysis and processing.

[0161] In one embodiment, a method for predicting the future state information of an obstacle based on the historical state information of the obstacle by an adaptive Kalman filter algorithm includes: first, it is necessary to set an initial state vector (including position, velocity, etc.) and an initial error covariance matrix for the Kalman filter. These initial values ​​are usually based on the first detection of the obstacle or prior knowledge. According to the motion model of the obstacle (such as linear motion, constant acceleration motion, etc.), the state transfer equation of the Kalman filter is used to predict the state of the obstacle in the next frame. This prediction is based on the current state estimate and process noise. When new observation data (such as laser radar point cloud, camera image, etc.) arrives, the observation equation is used to convert these data into a form corresponding to the state vector. Then, the state estimate and the error covariance matrix are updated by combining the predicted state and the observed data through the observation update equation of the Kalman filter. The parameters of the filter are adjusted according to the quality of the observation data or environmental changes. This may include adjusting the covariance matrix of the process noise and the observation noise, and updating the coefficients of the state transfer equation or the observation equation. If the deviation between the observed data and the predicted state is large, it may be due to an increase in observation noise or a decrease in process noise. In this case, the noise covariance matrix can be dynamically adjusted to reflect this change. If the obstacle's motion pattern changes (e.g., from uniform linear motion to accelerated motion), the coefficients of the state transfer equation may need to be adjusted to better match the actual motion. Once the Kalman filter has completed the state update in the current frame, the updated state estimate and the error covariance matrix can be used to predict the state of the obstacle in future frames. This is usually done by iterating the state transfer equation multiple times, with each iteration corresponding to a future time step. The uncertainty of the prediction can be evaluated using the error covariance matrix. The uncertainty of the prediction usually increases over time because the cumulative effect of process noise and observation noise causes the uncertainty of the state estimate to increase.

[0162] The above disclosure is only a preferred embodiment of the present invention, but the present invention is not limited thereto. Any non-creative changes that can be thought of by a person skilled in the art, as well as several improvements and modifications made without departing from the principle of the present invention, should be within the scope of protection of the present invention.

Claims

1. A drone obstacle avoidance method based on obstacle state prediction, characterized in that: include: Acquire depth image information in real time and perform point cloud conversion; Preprocess the original point cloud; Cluster the preprocessed point cloud; Distinguish between static and dynamic obstacles for clustered obstacles; Record the position and size information of the obstacle in each frame; The adaptive Kalman filter algorithm is used to predict the future state information of obstacles based on their historical state information.

2. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 1, characterized in that: The method for clustering the preprocessed point cloud includes a K-means clustering method and a DBSCAN clustering method.

3. The unmanned aerial vehicle obstacle avoidance method based on obstacle state prediction according to claim 1, characterized in that: The method for distinguishing static and dynamic obstacles includes: setting a distance threshold d min and sliding window threshold h min , for the same obstacle, when passing h min After the frame, if the displacement distance exceeds d min , it is marked as a dynamic object, otherwise it is regarded as a static object.

4. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 1, characterized in that: The step of recording the position and size information of the obstacle in each frame includes: The set of all obstacle clusters in each frame is represented by C = {C 1 ,C 2 ,...,C m }, each cluster represents an obstacle and contains the location information of each point in the cluster; At time t, calculate the average value of all points in each cluster in the current frame as the centroid coordinate of the cluster The centroid coordinates are the location information of the cluster in the current frame; According to the minimum coordinate value (x min ,y min ,z min ) and the maximum coordinate value (x max ,y max ,z max ), calculate a cubic anchor box that can contain all point clouds of the cluster, and the size information of the anchor box is: Length: l x =|x max -x min | Width: l y =|y max -y min | Height: l z = |z max - z min | Calculate the cost of the current cluster and all clusters in the previous frame, expressed as Euclidean distance: Among them, i represents the index of a cluster in the current frame, j represents the index of all clusters in the previous frame, t represents time, and △t represents the time increment; Construct the cost matrix Cost = (c ij ), the Hungarian algorithm is used to solve the optimal match in the cost matrix Cost, and a new index is assigned to the current cluster that does not match any existing cluster; For clusters matched to the same index in adjacent frames, the location and anchor box size information of the cluster in each frame are recorded.

5. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 4, characterized in that: The adaptive Kalman filter algorithm is used to predict the future state information of the obstacle based on the historical state information of the obstacle, wherein the adaptive Kalman filter algorithm includes two steps of prediction and update; the prediction step is responsible for calculating the current state variables and the error covariance matrix according to the state transfer matrix, and providing a priori estimates for the next time step; the update step is responsible for calculating the posterior estimates according to the prior estimates and the sensor measurements; the process noise covariance matrix Q and the measurement noise covariance matrix R are adaptively updated, and the process noise covariance matrix Q and the measurement noise covariance matrix R are dynamically adjusted by comparing the prediction error with a preset error threshold.

6. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 5, characterized in that: The state variable in the selected adaptive Kalman filter algorithm is x = [p x ,p y ,p z ,v x ,v y ,v z ], which is expressed as position and velocity information in the x, y, and z axis directions.

7. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 5, characterized in that: The state equation and measurement equation describing the motion of dynamic obstacles are: Equation of state: x k+1 =Ax k +v k Measurement equation: z k =Hx k +w k Among them, A is the state transfer matrix, H is the measurement matrix, k is the discrete state quantity, v k is the process noise, w k To measure the noise, v k 、w k All satisfy the Gaussian distribution: Q represents the process noise covariance matrix, and R represents the measurement noise covariance matrix.

8. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 7, characterized in that: The prediction and update equations of the adaptive Kalman filter can be expressed as: Prediction steps: Update steps: in, represents the predicted estimate of the state at time k, P k represents the error covariance matrix at time k, represents the prediction error covariance matrix at time k, F represents the state transfer matrix, and F T represents the transposed matrix of the state transfer matrix, P represents the error covariance matrix, E represents the residual, and E k represents the residual at time k, K represents the Kalman gain, K k represents the Kalman gain at time k.

9. The obstacle avoidance method for unmanned aerial vehicles based on obstacle state prediction according to claim 5, characterized in that: The specific method of adaptively updating the process noise covariance matrix Q and the measurement noise covariance matrix R is: Assuming that the state variables are independent of each other, the residual vector is taken Residual covariance matrix The diagonal elements of the residual covariance matrix form a vector Vector of diagonal elements of the process noise covariance matrix Vector of diagonal elements of the measurement noise covariance matrix Residual Threshold Vector Perform the update: in, represents the position residual in the x direction, represents the position residual in the y direction, represents the position residual in the z direction, represents the velocity residual in the x direction, represents the velocity residual in the y direction, Represents the velocity residual in the z direction; the residual covariance matrix defines the variance of the error between the predicted observations and the actual observations, and the diagonal elements reflect the observation uncertainty of a certain state variable; represents the variance of the position measurement error in the x direction, represents the variance of the position measurement error in the y direction, represents the variance of the position measurement error in the z direction, represents the variance of the velocity measurement error in the x direction, represents the variance of the velocity measurement error in the y direction, Represents the variance of the position measurement error in the z direction; the diagonal elements of the process noise covariance matrix Q represent the process noise variance of each state variable in the system state, that is, the uncertainty introduced by the system state variable in each time step due to the inherent random changes; represents the position process noise variance in the x direction, represents the position process noise variance in the y direction, represents the position process noise variance in the z direction, represents the velocity process noise variance in the x direction, represents the velocity process noise variance in the y direction, Represents the speed process noise variance in the z direction; the diagonal elements of the measurement noise covariance matrix R represent the noise variance of each measurement variable in the measurement process, that is, the uncertainty or noise level of the measurement device or sensor on each measurement quantity; represents the position measurement noise variance in the x direction, represents the position measurement noise variance in the y direction, represents the position measurement noise variance in the z direction, represents the velocity measurement noise variance in the x direction, represents the velocity measurement noise variance in the y direction, represents the velocity measurement noise variance in the z direction; Represents the position residual threshold in the x direction, Represents the position residual threshold in the y direction, Indicates the position residual threshold in the z direction; represents the velocity residual threshold in the x direction, represents the velocity residual threshold in the y direction, Represents the velocity residual threshold in the z direction, and the value of i is p x 、p y 、p z , the value of j is v x 、v y 、v z , η is the custom coefficient.

10. The unmanned aerial vehicle obstacle avoidance method based on obstacle state prediction according to claim 9, characterized in that: Based on the predicted obstacle state information, the speed obstacle method is used to plan the avoidance speed. When the speed obstacle method is used to plan the speed, the position residual is used to expand the obstacle, specifically: Assume that the anchor box size of the obstacle at time k is (l x ,l y ,l z ), the residual is Then the size of the expanded obstacle anchor frame is

Images