Dynamic obstacle avoidance method for UAV based on multi-sensor data fusion

Abstract

The present invention belongs to the field of unmanned aerial vehicle control systems and discloses a dynamic obstacle avoidance method for unmanned aerial vehicles based on multi-sensor data fusion, which solves the problem that when a unmanned aerial vehicle encounters an obstacle during flight, it is unable to circumvent the obstacle to efficiently perform flight actions. In the process of obstacle perception and data fusion, this method first proposes a Kalman filtering method based on the combination of circular scanning radar and visual ranging results, which achieves accurate target detection and estimation by regionalizing obstacles; further adopts a dynamic adjustment strategy, and the system dynamically adjusts the measurement noise covariance matrix parameters according to real-time feedback to estimate a more accurate obstacle distance; finally, the total obstacle avoidance cost of each obstacle sector is calculated, and the direction with the lowest cost is selected to update the movement direction of the unmanned aerial vehicle. Through the method of the present invention, more accurate and efficient dynamic obstacle avoidance can be achieved in different flight scenarios, thereby improving the overall flight performance and mission execution efficiency of the unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention relates to a dynamic obstacle avoidance method for an unmanned aerial vehicle (UAV), and in particular to a dynamic obstacle avoidance method for an UAV based on multi-sensor data fusion, belonging to the technical field of UAV control systems. Background Art

[0002] With the continuous development of drone technology, its application in military, agriculture, logistics, environmental monitoring, and other fields is gradually expanding. A drone's autonomous flight capability is key to its efficient mission execution in various complex environments. However, drones often face complex and changing flight environments, especially the presence of obstacles. This makes autonomous obstacle avoidance a core challenge in achieving intelligent drone flight. To ensure the safety and efficiency of drones in complex environments, improving their dynamic obstacle avoidance capabilities, particularly their ability to accurately identify and rapidly respond to obstacles during perception and decision-making, is a current research hotspot.

[0003] Traditional drone obstacle avoidance technology primarily relies on single sensors for obstacle detection and avoidance, such as lidar, ultrasonic sensors, and visual sensors. However, due to the unique strengths and weaknesses of each sensor type, the performance of a single sensor is often insufficient to handle complex flight scenarios. For example, lidar may experience blind spots when encountering transparent or weakly reflective objects, while visual sensors can also perform unstably in harsh environments such as low light and haze.

[0004] Therefore, to improve drones' environmental perception capabilities, researchers have begun exploring dynamic obstacle avoidance methods based on multi-sensor data fusion. By combining the strengths of different sensors, multi-sensor data fusion effectively offsets the shortcomings of a single sensor, enabling more accurate environmental perception and obstacle detection. The core goal of multi-sensor data fusion is to ensure data accuracy while improving system robustness, particularly for timely obstacle perception and rapid response in dynamic environments.

[0005] Common multi-sensor data fusion methods include Kalman filtering, particle filtering, and extended Kalman filtering. These methods, through comprehensive processing of sensor data, can effectively eliminate noise while maintaining computational efficiency, thereby improving the accuracy and reliability of target detection. Kalman filtering, as a classic estimation method, is widely used in state estimation and data fusion in fields such as aerospace and unmanned driving. It predicts and estimates dynamically changing targets using linear system models and makes corrections based on sensor observations to achieve an optimal estimate of the target state. However, traditional Kalman filtering assumes that the system model and sensor errors follow a Gaussian distribution and cannot handle the complexity of nonlinear systems.

[0006] Based on the above reasons, the existing technology of UAV’s perception accuracy of flight scenes still needs to be improved. It is urgent to develop a dynamic obstacle avoidance method that can achieve more accurate and efficient flight scenarios in different flight scenarios, thereby improving the overall flight performance and mission execution efficiency of UAVs. Summary of the Invention

[0007] To address the shortcomings of the existing technology, the purpose of the present invention is to provide a dynamic obstacle avoidance method for unmanned aerial vehicles (UAVs) based on multi-sensor data fusion. When a UAV encounters an obstacle while performing a flight action, the method can use circular scanning radar and visual ranging to perceive the presence of the obstacle, and dynamically adjust the obstacle avoidance strategy to ensure the flight safety of the UAV.

[0008] In order to achieve the above objectives, the present invention adopts the following technical solutions:

[0009] The present invention discloses a method for dynamic obstacle avoidance of an unmanned aerial vehicle based on multi-sensor data fusion, comprising the following steps:

[0010] S1. Divide the horizontal plane of the drone into N obstacle sectors;

[0011] S2. Obtain the aircraft speed output by the combined navigation ;

[0012] S3, real-time collection of the original obstacle data of the drone's circular scanning radar and depth camera, respectively recorded as and ;

[0013] S4. When the obstacle distances measured by the two sensors are both greater than the minimum obstacle perception threshold MIN_OBS_DIS for a long time, the Kalman filter will be initialized, and the output POS(i) will be MIN_OBS_DIS, and VEL(i) will be the observed ,At this time, the UAV flies according to the direction and size of the speed target generated by the controller flight instruction;

[0014] S5. When the obstacle distance measured by the two sensors is less than MIN_OBS_DIS, the Kalman filter state prediction and correction update are used in combination with the accuracy data provided by the circular scanning radar to dynamically adjust the covariance matrix of the measurement noise of the circular scanning radar and the depth camera to estimate a more accurate obstacle distance POS(i).

[0015] S6. In the obstacle avoidance decision, the environment is numbered into several sectors i (i=1, 2, 3, ..., N), and each sector has a corresponding obstacle distance and echo intensity , traverse the valid obstacle sectors adjacent to the movement direction, calculate the total obstacle avoidance cost of each obstacle sector, find the sector with the lowest cost and determine the sector direction, and update the UAV's movement target direction and speed.

[0016] Preferably, in the aforementioned step S1, the number N of obstacle sectors is an integer, which is determined by the resolution n of the circular scanning radar, N=360 / n.

[0017] Preferably, in the aforementioned step S5, when performing state prediction, it is necessary to establish a state model and perform position and speed estimation: the transfer function between the speed target value and the drone position is designed: ,

[0018] in, and are the Laplace transforms of the UAV position and velocity target values ​​in the complex frequency domain; the parameters 、 、 Determined by fitting actual flight data, represents the complex frequency variable in the Laplace transform;

[0019] This yields a continuous-time model: ,

[0020] 、 and They are the UAV body speed, position and speed target value in the time domain; for The derivative of

[0021] In MATLAB, the state matrix of the discrete-time state space model is calculated using the c2d function and the ssdata function. , input matrix ;State matrix , input matrix , output matrix They are:

[0022] ;

[0023] The state space expression of the state model is:

[0024] ,

[0025] in, Represents the derivative of the state variables. The state variables of the model include the position and velocity of the UAV, which can be expressed as:

[0026] ,

[0027] represents the output vector; Indicates the input speed target value; is the position state, Speed ​​status.

[0028] Preferably, according to the actual flight speed target value of the UAV and the UAV position data, the Ident toolbox in Matlab is used to perform parameter fitting to obtain the parameters 、 、 The value of .

[0029] Kalman filter state prediction specifically includes:

[0030] Status prediction: ,

[0031] Covariance prediction: ,

[0032] in, 、 and Corresponding to the aforementioned matrices 、 and ,Right now ; Indicates the current moment, Indicates the previous moment; Represents state variables The predicted status of Represents state variables Estimated state of , including POS and VEL); Represents the state variable at the current moment The predicted state, Represents the state variable at the previous moment The estimated state of is the target value of the attitude angle, is the target value of the attitude angle at the previous moment; is the prediction covariance, is the forecast covariance at the current moment, is the forecast covariance of the previous moment, is the covariance matrix of the process noise, , is the vector of process noise, corresponding to internal noise; represents the expected value operator, Indicates transpose.

[0033] The present invention improves the Kalman filter by introducing multiple sensors (circular scanning radar and depth camera) for simultaneous observation and dynamically adjusting the measurement noise covariance matrix. The specific process of correction and update is as follows: ,

[0034] Kalman gain calculation: ,

[0035] The covariance update process is: ,

[0036] represents the estimated output of the state variable; Estimate the output for the state variables at the current moment; is the identity matrix, is the covariance updated at the current moment;

[0037] represents the optimal Kalman gain, a gain matrix used to minimize the estimation error, based on the process noise covariance that follows a Gaussian distribution and the measurement noise covariance , ; Indicates the measurement value at the current moment, including the obstacle distance measured by the circular scanning radar at the current moment Distance to obstacles measured by the depth camera ; is the vector of measurement noise, corresponding to the measurement noise; is the optimal Kalman gain at the current moment;

[0038] Mapping the data accuracy measured by circular scanning radar to numerical space , the highest accuracy corresponds to 0.1 corresponds to the lowest precision is 10;

[0039] The measurement noise covariance matrix is ​​optimized as:

[0040] ,

[0041] is a constant representing the observation covariance matrix parameters proposed by the simulation. The specific values ​​need to be adjusted according to the actual system. The adjustment basis is: based on the actual measured speed, circular scanning radar distance, and visual ranging distance results, the parameters are first adjusted in the simulation to make the output result POS closer to the actual one. At this time, the GNSS and radar will jointly participate in the decision-making, and the actual distance of the obstacle will be empirically analyzed to determine the parameters of the system and measurement noise covariance matrix in the Kalman filter.

[0042] Further preferably, in the obstacle avoidance decision, a total obstacle avoidance cost function including obstacle distance, deviation direction and confidence penalty is designed, with the weight dynamically adjusted according to the echo intensity of the circular scanning radar. The process is as follows:

[0043] (1) Calculate the smoothing confidence: Normalized intensity is the distance confidence, denoted as , echo intensity The larger it is, the higher the confidence level:

[0044] ,

[0045] Neighborhood smoothing weight , which controls how much confidence is taken into account for neighboring sectors, It is not smooth, only uses its own strength; When highly smoothed, the confidence after smoothing for:

[0046] ,

[0047] The lower (the echo is weak), the confidence penalty The bigger:

[0048] ,

[0049] (2) Design normalized distance cost and deviation cost;

[0050] Original distance cost for:

[0051] ,

[0052] Normalized distance cost for:

[0053] ,

[0054] in is the minimum value of obstacle distance measurement, The maximum value of obstacle distance measurement;

[0055] sector The angle between the center heading and the desired heading (or current heading) is the heading deviation angle , design tolerance threshold , deviations less than or equal to this angle are considered acceptable, regardless of cost;

[0056] Original course deviation cost for:

[0057] .

[0058] The normalized heading deviation cost is:

[0059] ,

[0060] in, Indicates that the heading deviates from the maximum value of the strategy by 90°.

[0061] (3) Design dynamic weight allocation:

[0062] ,

[0063] in, The weight corresponding to the confidence penalty; The weight corresponding to the distance cost; The weight corresponding to the cost of heading deviation; is the minimum baseline weight of the confidence penalty, ensuring that even if (very high confidence), but also retain a certain ;

[0064] , which is used to control the distribution ratio of the remaining distance and heading. The specific value needs to be adjusted through simulation to minimize the final output obstacle avoidance cost in the desired direction.

[0065] (4) Calculate the total cost of obstacle avoidance :

[0066] ,

[0067] Finally, based on the total obstacle avoidance cost of each sector, the drone chooses the direction with the lowest cost.

[0068] The present invention is beneficial in that:

[0069] (1) In the prior art, traditional multi-sensor fusion methods usually use fixed weight coefficients to balance the data contributions of different sensors. However, during actual flight, radar accuracy, the working status of visual sensors, and environmental factors will change, and these changes will affect the performance of each sensor. Based on this, the present invention proposes an optimization method for dynamic adjustment strategy. Through a real-time feedback mechanism, the weights of each area are automatically adjusted according to the changes in the flight environment and the echo intensity of the scanning radar, so as to optimize the performance of the perception system and improve the perception accuracy of the UAV in different flight scenarios.

[0070] (2) In the process of obstacle perception and data fusion, this method proposes an improved Kalman filtering method based on the combination of circumferential scanning radar and visual ranging results, and with the help of GNSS data-assisted observation, it regionalizes obstacles and adaptively adjusts the measurement noise covariance to achieve accurate target detection and estimation.

[0071] (3) The total obstacle avoidance cost of the present invention consists of three parts: heading deviation cost, distance cost and confidence penalty. The deviation cost measures the degree of deviation of the obstacle from the flight trajectory. The greater the deviation, the higher the cost. The distance cost represents the actual distance between the obstacle and the UAV. The closer the distance, the higher the cost. The role of the confidence penalty is that the lower the confidence (weaker the echo), the higher the cost. Finally, based on the estimated obstacle distance of each sector, the direction of deviation from the route and the echo intensity of the scanning radar, the total obstacle avoidance cost of each obstacle sector is calculated, and the direction with the lowest cost is selected to update the UAV's movement direction. Through the above-mentioned method of dynamically adjusting parameters, the system can achieve more accurate and efficient dynamic obstacle avoidance in different flight scenarios, thereby improving the overall flight performance and mission execution efficiency of the UAV. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a flow chart of the UAV dynamic obstacle avoidance method based on multi-sensor data fusion of the present invention;

[0073] Figure 2 This is a flow chart of the state prediction and correction update of the Kalman filter in the present invention. DETAILED DESCRIPTION

[0074] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0075] This embodiment provides a method for dynamic obstacle avoidance of UAV based on multi-sensor data fusion. In normal flight state, the flight mainly relies on the instructions given by the controller. For example, if only the pitch axis is increased, the UAV will generally receive the instruction to fly forward. That is, the flight control system will provide a corresponding speed target value for the speed controller based on the pitch axis. When the UAV encounters an obstacle during flight, it uses the circular scanning radar and visual ranging to sense the existence of the obstacle and dynamically adjust the obstacle avoidance strategy to ensure the flight safety of the UAV. The specific process is as follows: Figure 1 As shown, the following steps are included:

[0076] S1. Divide the horizontal plane of the drone into N obstacle sectors, where N is an integer determined by the resolution n of the circumferential scanning radar, N = 360 / n. Specifically, the number of sectors (effective sectors) to be evaluated in the target flight direction is determined based on the drone's wheelbase. For example, a small drone may only need to evaluate obstacles in the target flight direction and ten adjacent sectors, for a total of eleven sectors. However, a medium-sized drone may need to evaluate obstacles in all thirty-one sectors in the target flight direction.

[0077] S2. Obtain the aircraft speed output by the combined navigation In this embodiment, the combined navigation system includes radar and vision sensors. The circular scanning radar can detect obstacles in the surrounding environment by transmitting and receiving electromagnetic waves. It has strong anti-interference capabilities and can operate effectively in various complex environments. The vision sensor can provide more accurate obstacle shape and location data, and is particularly suitable for identifying obstacles at close range. By fully leveraging the working principles and complementary advantages of radar and vision sensors, they can be combined to leverage their respective strengths, overcome the shortcomings of a single sensor in specific environments, and improve the flight reliability of the drone.

[0078] S3, real-time collection of the original obstacle data of the drone's circular scanning radar and depth camera, respectively recorded as and .

[0079] S4. When the obstacle distances measured by the two sensors are both greater than the minimum obstacle perception threshold MIN_OBS_DIS for a long time, the Kalman filter will be initialized, and the output POS(i) will be MIN_OBS_DIS, and VEL(i) will be the observed At this time, the UAV flies in the direction and size of the speed target generated by the controller flight instructions.

[0080] S5. During the flight, the flight control system Figure 2 The Kalman filter state prediction and correction update shown in the figure is used to update the obstacle distance. When the obstacle distance measured by the two sensors is less than MIN_OBS_DIS, the Kalman filter state prediction and correction update is used in combination with the accuracy data provided by the circular scanning radar to dynamically adjust the covariance matrix of the measurement noise of the circular scanning radar and the depth camera to estimate a more accurate obstacle distance POS(i).

[0081] First, when predicting the state, it is necessary to establish a state model and estimate the position and speed: design the transfer function between the speed target value and the drone position:

[0082] ,

[0083] in, and are the Laplace transforms of the UAV position and velocity target values ​​in the complex frequency domain; the parameters 、 、 According to the actual flight data, the parameters are determined by using the Ident toolbox in Matlab. represents the complex frequency variable in the Laplace transform;

[0084] This yields a continuous-time model:

[0085] ,

[0086] 、 and They are the UAV body speed, position and speed target value in the time domain; for The derivative of

[0087] In MATLAB, the state matrix of the discrete-time state space model is calculated using the c2d function and the ssdata function. , input matrix ;State matrix , input matrix , output matrix They are:

[0088] ;

[0089] The state space expression of the state model is:

[0090] ,

[0091] in, Represents the derivative of the state variables. The state variables of the model include the position and velocity of the UAV, which can be expressed as:

[0092] ,

[0093] represents the output vector; Indicates the input speed target value; is the position state, Speed ​​status.

[0094] Next, during the correction update, the Kalman filter was improved by introducing multiple sensors (circular scanning radar and depth camera) for simultaneous observation and dynamically adjusting the measurement noise covariance matrix. The specific process is as follows:

[0095]

[0096] Kalman gain calculation: ,

[0097] The covariance update process is: ,

[0098] represents the estimated output of the state variable; Estimate the output for the state variables at the current moment; is the identity matrix, is the covariance updated at the current moment;

[0099] represents the optimal Kalman gain, a gain matrix used to minimize the estimation error, based on the process noise covariance that follows a Gaussian distribution and the measurement noise covariance , ; Indicates the measurement value at the current moment, including the obstacle distance measured by the circular scanning radar at the current moment Distance to obstacles measured by the depth camera ; is the vector of measurement noise, corresponding to the measurement noise; is the optimal Kalman gain at the current moment;

[0100] Mapping the data accuracy measured by circular scanning radar to numerical space , the highest accuracy corresponds to 0.1 corresponds to the lowest precision is 10;

[0101] The measurement noise covariance matrix is ​​optimized as:

[0102] ,

[0103] is a constant representing the observation covariance matrix parameters proposed by the simulation. The specific values ​​need to be adjusted according to the actual system. The adjustment basis is: based on the actual measured speed, circular scanning radar distance, and visual ranging distance results, the parameters are first adjusted in the simulation to make the output result POS closer to the actual one. At this time, the GNSS and radar will jointly participate in the decision-making, and the actual distance of the obstacle will be empirically analyzed to determine the parameters of the system and measurement noise covariance matrix in the Kalman filter.

[0104] S6. In the obstacle avoidance decision, a total obstacle avoidance cost function is designed that includes obstacle distance, deviation direction, and confidence penalty, with dynamic weight adjustment based on the echo intensity of the circular scanning radar. The environment is numbered into several sectors i (i=1, 2, 3, ..., N), and each sector has a corresponding obstacle distance and echo intensity , traverse the valid obstacle sectors adjacent to the movement direction, calculate the total obstacle avoidance cost of each obstacle sector, find the sector with the lowest cost and determine the sector direction, and update the UAV's movement target direction and speed.

[0105] The specific process is as follows:

[0106] (1) Calculate the smoothing confidence: Normalized intensity is the distance confidence, denoted as , echo intensity The larger it is, the higher the confidence level:

[0107] ,

[0108] Neighborhood smoothing weight , which controls how much confidence is taken into account for neighboring sectors, It is not smooth, only uses its own strength; When highly smoothed, the confidence after smoothing for:

[0109] ,

[0110] The lower (the echo is weak), the confidence penalty The bigger:

[0111] .

[0112] (2) Design normalized distance cost and deviation cost;

[0113] Original distance cost for:

[0114] ,

[0115] Normalized distance cost for:

[0116] ,

[0117] in is the minimum value of obstacle distance measurement, The maximum value of obstacle distance measurement;

[0118] sector The angle between the center heading and the desired heading (or current heading) is the heading deviation angle , design tolerance threshold , deviations less than or equal to this angle are considered acceptable, regardless of cost;

[0119] Original course deviation cost for:

[0120] ,

[0121] The normalized heading deviation cost is:

[0122] ,

[0123] in, Indicates that the heading deviates from the maximum value of the strategy by 90°.

[0124] (3) Design dynamic weight allocation:

[0125] ,

[0126] in, The weight corresponding to the confidence penalty; The weight corresponding to the distance cost; The weight corresponding to the cost of heading deviation; is the minimum baseline weight of the confidence penalty, ensuring that even if (very high confidence), but also retain a certain ;

[0127] , which is used to control the distribution ratio of the remaining distance and heading. The specific value needs to be adjusted through simulation to minimize the final output obstacle avoidance cost in the desired direction.

[0128] (4) Calculate the total cost of obstacle avoidance :

[0129] ,

[0130] Finally, based on the total obstacle avoidance cost of each sector, the drone chooses the direction with the lowest cost to achieve perfect dynamic obstacle avoidance.

[0131] In summary, given that radar accuracy, the operating status of visual sensors, and environmental factors all vary during actual UAV flight, these changes can affect the performance of each sensor. To address these variations, the obstacle avoidance method proposed in this paper employs a dynamic adjustment strategy. Through a real-time feedback mechanism, the measurement noise covariance is adaptively adjusted based on real-time flight data. The total obstacle avoidance cost for each region is then calculated, optimizing the performance of the perception system to achieve precise obstacle detection and estimation. This allows for more accurate and efficient dynamic obstacle avoidance in various flight scenarios, thereby improving the UAV's overall flight performance and mission execution efficiency.

[0132] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any form, and any technical solutions obtained by equivalent replacement or equivalent transformation fall within the scope of protection of the present invention.

Claims

1. A UAV dynamic obstacle avoidance method based on multi-sensor data fusion, characterized by: The steps include: S1. Divide the horizontal plane of the drone into N obstacle sectors; S2. Obtain the aircraft speed output by the combined navigation ; S3, real-time collection of the original obstacle data of the drone's circular scanning radar and depth camera, respectively recorded as and ; S4. When the obstacle distances measured by the two sensors are both greater than the minimum obstacle perception threshold MIN_OBS_DIS for a long time, the Kalman filter will be initialized, and the output POS(i) will be MIN_OBS_DIS, and VEL(i) will be the observed ,At this time, the UAV flies according to the direction and size of the speed target generated by the controller flight instruction; S5. When the obstacle distance measured by the two sensors is less than MIN_OBS_DIS, the Kalman filter state prediction and correction update are used in combination with the accuracy data provided by the circular scanning radar to dynamically adjust the covariance matrix of the measurement noise of the circular scanning radar and the depth camera to estimate a more accurate obstacle distance POS(i). S6. In the obstacle avoidance decision, the environment is numbered into several sectors i, i = 1, 2, 3, ..., N, and each sector has a corresponding obstacle distance and echo intensity , traverse the valid obstacle sectors adjacent to the movement direction, calculate the total obstacle avoidance cost of each obstacle sector, find the sector with the lowest cost and determine the sector direction, and update the movement target direction and speed of the UAV; The obstacle avoidance decision-making process is as follows: (1) Calculate the smoothing confidence: Normalized intensity is the distance confidence, denoted as , echo intensity The larger it is, the higher the confidence level: ， Neighborhood smoothing weight , which controls how much confidence is taken into account for neighboring sectors, It is not smooth, only uses its own strength; When highly smoothed, the confidence after smoothing for: ， The lower the confidence penalty The bigger: ， (2) Design normalized distance cost and deviation cost; Original distance cost for: ， Normalized distance cost for: ， in is the minimum value of obstacle distance measurement, The maximum value of obstacle distance measurement; sector The angle between the center heading and the desired heading is the heading deviation angle , design tolerance threshold , deviations less than or equal to this angle are considered acceptable, regardless of cost; Original course deviation cost for: ， The normalized heading deviation cost is: ， in, Indicates that the heading deviates from the maximum value of the strategy by 90°; (3) Design dynamic weight allocation: ， in, The weight corresponding to the confidence penalty; The weight corresponding to the distance cost; The weight corresponding to the cost of heading deviation; is the minimum baseline weight of the confidence penalty, ensuring that even if , but also retain a certain ; , used to control the distribution ratio of the remaining distance and heading; (4) Calculate the total cost of obstacle avoidance : ， Based on the total obstacle avoidance cost of each sector, the drone chooses the direction with the lowest cost.

2. The method for dynamic obstacle avoidance of unmanned aerial vehicles based on multi-sensor data fusion according to claim 1, characterized in that: In step S1, the number N of obstacle sectors is an integer, which is determined by the resolution n of the circular scanning radar, N=360 / n.

3. The method for dynamic obstacle avoidance of unmanned aerial vehicle based on multi-sensor data fusion according to claim 1, characterized in that: When performing state prediction, it is necessary to establish a state model and perform position and speed estimation: The transfer function between the design speed target value and the UAV position is: ， in, and are the Laplace transforms of the UAV position and velocity target values ​​in the complex frequency domain; the parameters 、 、 Determined by fitting actual flight data, represents the complex frequency variable in the Laplace transform; This yields a continuous-time model: ， 、 and They are the UAV body speed, position and speed target value in the time domain; for The derivative of In MATLAB, the state matrix of the discrete-time state space model is calculated using the c2d function and the ssdata function. , input matrix ;State matrix , input matrix , output matrix They are: ； The state space expression of the state model is: ， in, Represents the derivative of the state variables. The state variables of the model include the position and velocity of the UAV, which can be expressed as: ， represents the output vector; Indicates the input speed target value; is the position state, Speed ​​status.

4. The method for dynamic obstacle avoidance of unmanned aerial vehicles based on multi-sensor data fusion according to claim 3, characterized in that: According to the actual flight speed target value of the UAV and the UAV position data, the parameters are fitted using the Ident toolbox in Matlab to obtain the parameters 、 、 The value of .

5. The method for dynamic obstacle avoidance of unmanned aerial vehicles based on multi-sensor data fusion according to claim 3, characterized in that: The Kalman filter state prediction includes: Status prediction: , Covariance prediction: , in, 、 and Corresponding to the aforementioned matrices 、 and ,Right now ; Indicates the current moment, Indicates the previous moment; Represents state variables The predicted status of Represents state variables Estimated state of , including POS and VEL); Represents the state variable at the current moment The predicted state, Represents the state variable at the previous moment The estimated state of is the target value of the attitude angle, is the target value of the attitude angle at the previous moment; is the prediction covariance, is the forecast covariance at the current moment, is the forecast covariance of the previous moment, is the covariance matrix of the process noise, , is the vector of process noise, corresponding to internal noise; represents the expected value operator, Indicates transpose.

6. The method for dynamic obstacle avoidance of unmanned aerial vehicles based on multi-sensor data fusion according to claim 5, characterized in that: The process of correction and update is as follows: , Kalman gain calculation: , The covariance update process is: , represents the estimated output of the state variable; Estimate the output for the state variables at the current moment; is the identity matrix, is the covariance updated at the current moment; represents the optimal Kalman gain, a gain matrix used to minimize the estimation error, based on the process noise covariance that follows a Gaussian distribution and the measurement noise covariance , ; Indicates the measurement value at the current moment, including the obstacle distance measured by the circular scanning radar at the current moment Distance to obstacles measured by the depth camera ; is the vector of measurement noise; is the optimal Kalman gain at the current moment; Mapping the data accuracy measured by circular scanning radar to numerical space , the highest accuracy corresponds to 0.1 corresponds to the lowest precision is 10; The measurement noise covariance matrix is ​​optimized as: ， is a constant, which represents the observation covariance matrix parameter proposed by the simulation. The specific value is adjusted according to the actual system.

Images