Unmanned aerial vehicle flight intention estimation method and system based on novel interactive multiple models

Abstract

The invention provides a novel interactive multi-model-based unmanned aerial vehicle flight intention estimation method and system, and the method comprises the steps: S1, collecting unmanned aerial vehicle sensor data, carrying out the complementary filtering operation of the unmanned aerial vehicle sensor data, and obtaining pitch angle estimation, position estimation and speed estimation; s2, selecting a motion model, constructing a model initial state equation and an observation equation, establishing a motion equation, and generating the initial state equation and the observation equation; performing model parameter adjustment operation on the motion model to obtain an interactive multi-model framework; and S3, in combination with a complementary filter and an interactive multi-model framework, obtaining an interactive multi-model IMM, inputting pitch angle estimation, position estimation and speed estimation into the interactive multi-model IMM, selecting an applicable model, and estimating to obtain a target state. According to the method, the technical problems of influence of sensor noise and errors on data and relatively low robustness, accuracy and flexibility of flight intention estimation of the unmanned aerial vehicle are solved.

Description

Technical Field

[0001] The present invention relates to the fields of unmanned aerial vehicles and moving bodies, control systems and filtering technologies, and in particular to a method and system for estimating the flight intention of an unmanned aerial vehicle based on a novel interactive multi-model. Background Art

[0002] With the rapid development of modern information technology, understanding the intention of targets (such as humans or robots) has become a key skill for intelligent systems to coexist with humans or other systems. Related research plays an important role in the fields of motion prediction, human-computer interaction, monitoring, and autonomous driving. Intent estimation methods have evolved from simple observation to inference based on observed data and have attracted increasing attention in the past few decades.

[0003] Traditional intention estimation methods are mainly divided into two categories: physics-based models and pattern-based models. Physics-based models use Newton's laws of motion to clearly describe the motion behavior of the target, associate intentions with dynamic models, and interpret different intentions through different parameters (such as speed, turning direction, and target). However, this model needs to rely on clear parameter settings. Some parameters are often difficult to obtain or not accurate enough in real applications. Therefore, it is less flexible to use and difficult to adapt to changing environments. Pattern-based models learn the motion behavior of the target from data without explicitly defining parameterized functions. They use technologies such as hidden Markov models and neural networks to predict human motion trajectories and intentions. However, this method is heavily dependent on the quality and quantity of training data. If the training data is insufficient or unrepresentative, the prediction performance of the model will be limited. Traditional intention estimation methods have difficulty capturing the changing motion states of drones, such as acceleration, hovering, and deceleration.

[0004] The existing invention patent application document "A collaborative path planning method for multiple unmanned aerial vehicles tracking multiple maneuvering targets" with publication number CN106873628A uses a traditional interactive multiple model (IMM) filter to predict the motion state (position, speed) of the target. First, it initializes the drone parameters and predicts directly through IMM filtering. Since IMM filtering depends on sensor input, the initial sensor data usually contains noise and errors. However, the aforementioned prior art does not fully filter out noise and errors, and the data passed to the interactive multiple model filter is not accurate enough, which restricts the estimation accuracy.

[0005] The existing invention patent application document "A Lane Information Perception Method Based on Unmanned Intelligent Vehicle" with publication number CN113903010A, the existing method can only display the effect of IMM processing on the vehicle position information. The result of the existing solution is relatively simple and cannot effectively display the position, speed and status information. At the same time, the traditional IMM filter used in the above existing solution performs poorly when the dynamic data changes, and has poor adaptability to dynamic data, which restricts the prediction accuracy.

[0006] The existing invention patent application document "A method for long-term trajectory prediction of targets based on interactive multi-models" with publication number CN118520374A uses UKF for state updates, which can effectively handle nonlinear system tracking problems and accurately predict the state of flying targets. However, the existing solution does not mention whether it has similar multi-model fusion capabilities, so it has limitations when dealing with targets switching between multiple motion modes. At the same time, the existing solution uses traditional IMM filters and lacks a special mechanism for handling sensor noise and drift. Directly filtering sensor data may affect estimation accuracy.

[0007] In summary, the existing technologies have technical problems such as the impact of sensor noise and errors on data, as well as low robustness, accuracy and flexibility of UAV flight intention estimation. Summary of the invention

[0008] The technical problem to be solved by the present invention is: how to solve the influence of sensor noise and error on data in the prior art, and the technical problem of low robustness, accuracy and flexibility of UAV flight intention estimation.

[0009] The present invention adopts the following technical solutions to solve the above technical problems: A method for estimating the flight intention of a UAV based on a novel interactive multi-model comprises:

[0010] S1, collecting drone sensor data, performing complementary filtering operations on the drone sensor data, and obtaining pitch angle estimation, position estimation and speed estimation through complementary filtering operations;

[0011] S2. Selecting a motion model, which includes an acceleration model, a deceleration model, and a hovering model; constructing the initial state equation and observation equation of the model, wherein the motion equations are established for the acceleration model, the deceleration model, and the hovering model, respectively, to generate the initial state equation and observation equation; describing the state transfer matrix in a parameterized manner to adjust the model parameters of the motion model and obtain an interactive multi-model framework;

[0012] S3. Combine the complementary filter and the interactive multi-model framework to obtain the interactive multi-model IMM, input the pitch angle estimation, position estimation and velocity estimation into the interactive multi-model IMM, and perform noise and error filtering operations through the complementary filter; select the applicable model from the motion models such as the acceleration model, deceleration model and hovering model to estimate the target state.

[0013] The present invention can effectively display position, speed and state information after complementary filtering combined with IMM processing. The present invention provides high-quality, low-noise data for IMM by preprocessing the data first, thereby enhancing its adaptability to dynamic data and improving the accuracy of prediction.

[0014] In a more specific technical solution, S1 includes:

[0015] S11, the accelerometer data a and the gyroscope data ω are fused through complementary filtering to obtain the pitch angle estimation θ:

[0016] θ t =α(θ t-1 +ω yt Δt)+(1-α)θ at

[0017] In the formula, θ represents the estimated value of the pitch angle, α is the complementary filter coefficient, which ranges from 0 to 1, and ω y represents the angular velocity of the gyroscope on the y-axis, θ a Represents the pitch angle calculated by the accelerometer:

[0018]

[0019] S12, compare the GPS position p and speed v in the drone sensor data with the position p estimated by the IMU IMU and speed v IMU Perform weighted averaging to obtain the position estimate p e , velocity estimation v e ;

[0020] S13, the pitch angle estimate θ, the position estimate p e and the velocity estimate v e , which is input into the IMM multi-model framework for intention estimation.

[0021] The present invention adopts a novel interactive multi-model architecture, which first uses complementary filtering to process the sensor data, filters out noise and errors, and then passes the clean and reliable data to the interactive multi-model filter, thereby improving the estimation accuracy.

[0022] In a more specific technical solution, in S12, the IMU estimated speed v is calculated using the following logic: IMU:

[0023] v IMU =v et-1 +a t Δt

[0024] The following logic is used to fuse the GPS speed v and the IMU estimated speed v IMU , get the velocity estimate v e :

[0025] v et =αv IMUt +(1-α)v t

[0026] Estimate v from the velocity e , use the following logic to perform IMU estimation and obtain the IMU estimated position p IMU :

[0027] p IMUt =p et-1 +v e Δt

[0028] Fusion of GPS position p and IMU estimated position p IMU , and obtain the position estimate p e :

[0029] p et =αp IMUt +(1-α)p t .

[0030] In a more specific technical solution, in S2, the state vector of the motion model is expressed using the following logic:

[0031]

[0032] Using the following logic, the initial state equation of the motion model is expressed:

[0033]

[0034]

[0035] Using the following logic, the observation equation is expressed as:

[0036] z k =H k x k +v k

[0037]

[0038] Among them, x k represents the state vector of the target at time k, is the motion model m i The system matrix, is the control matrix, is the control input vector, w k is the process noise, z k is the observation vector of the system, H k is the observation matrix. Since the model state vectors can be directly observed, H k is a four-row and four-column identity matrix, v k is the observation noise;

[0039] The motion model is determined based on the pitch angle θ and angular velocity ω.

[0040] The present invention adopts an interactive multi-model (IMM) which allows switching between multiple models that obey the Markov characteristic to better adapt to the flight conditions of the UAV. In the IMM interactive multi-model, the IMM filter relies on multiple sensor data inputs during the estimation process. If the sensor data quality is poor, such as the drift error of the IMU data or the signal loss and error of the GPS data, the estimation accuracy of the IMM filter will be significantly affected. Complementary filtering can be introduced to pre-process the sensor noise and error and improve the data quality. Therefore, the IMM interactive multi-model can be improved in this direction.

[0041] In a more specific technical solution, S3 includes:

[0042] S31, pre-processing pitch angle estimation, position estimation and velocity estimation, obtaining and outputting fusion data;

[0043] The following logic is used to express the prior probability sequences corresponding to the acceleration model, deceleration model, and hovering model:

[0044] u(k)=(u1(k),u2(k),u3(k));

[0045] Using the following logic, the state transition probability matrix is ​​set according to the Markov process that switches between the acceleration model, deceleration model, and hovering model:

[0046]

[0047] Determine the initial state vector of motion model j at time k Initial error covariance P 0j (k) and the interaction probability u ij (k):

[0048]

[0049] Determine the transition probability of motion model i to j, and use the following logic to determine the prior probability u of model i at time k i (k):

[0050]

[0051] S32, according to different flight states of the UAV, for the acceleration model, the deceleration model and the hovering model, respectively establish motion model filters, and initialize the state estimation of the filter based on the fusion data; perform Kalman filtering in the interactive multi-model IMM;

[0052] S33, using the interactive multi-model IMM, according to the estimated probability, the estimated state vector and its corresponding covariance moment, using each filter, performing prediction operation and update operation to complete the combined state estimation and obtain the filtering estimation result;

[0053] S34, obtaining and using the model likelihood function and the state transition probability to perform a model probability update operation on the motion model to obtain an updated motion model;

[0054] S35, estimate the probability of updating the motion model, perform weighted fusion on the estimated values ​​of each filter in the updated motion model, perform fusion state estimation through fusion output operation, and obtain applicable state estimation results through processing.

[0055] The present invention introduces complementary filtering to pre-process sensor data, thereby reducing noise and error and improving the estimation accuracy of the IMM filter.

[0056] In a more specific technical solution, in the prediction operation of S33, the following logic is used to perform state prediction:

[0057]

[0058] Use the following logic to make covariance predictions:

[0059]

[0060] In the formula, represents the prediction of the state of model j at time k, is the covariance matrix of the predicted state, A(k) represents the observation matrix, B j (k) represents the control matrix, u j (k) represents the control input, A(k), B j (k),u j (k) is determined by the motion model, and Q(k) is the covariance of the motion noise.

[0061] In a more specific technical solution, in the update operation of S33, the Kalman gain is calculated using the following logic:

[0062]

[0063] Use the following logic to update the state of the motion model:

[0064]

[0065] Use the following logic to update the covariance:

[0066]

[0067] In the formula, represents the updated state estimate, P j (k) represents the covariance matrix of the updated state estimate, K(k) is the Kalman gain, z(k) is the observation vector at time k, H(k) is the observation matrix, and R(k) is the covariance of the observation noise.

[0068] In a more specific technical solution, in S34, the model probability update operation is performed using the following logic:

[0069]

[0070] Among them, the model likelihood function Λ is obtained by using the following logic j (k):

[0071]

[0072] In the formula, S j (k) is the covariance matrix of the jth motion model update at time k, V j (k) is the residual of the jth filter at time step k:

[0073]

[0074] In a more specific technical solution, in the fusion output operation of S35, represents the fusion state estimation, P k represents the covariance matrix of the fusion state estimate;

[0075] The following logic is used to estimate the fusion state:

[0076]

[0077] Using the following logic, the covariance matrix is ​​obtained:

[0078]

[0079] The present invention designs and utilizes an interactive multiple model architecture, combines complementary filtering algorithms and advanced methods of interactive multiple models (IMM), and introduces complementary filtering algorithms into traditional interactive multiple models (IMM) to preprocess sensor data, aiming to improve the accuracy and flexibility of UAV flight intention estimation. The complementary filtering algorithm is a filtering method for multi-sensor data fusion, which is commonly used in scenarios such as attitude estimation and position estimation. The present invention improves estimation accuracy and robustness by utilizing the complementary characteristics of different sensors, such as high-frequency and low-frequency characteristics.

[0080] Specifically, during the flight of the drone, the sensor data contains noise and errors. If these data are directly used for intent estimation using an IMM filter, the estimation accuracy will be significantly affected. In this regard, the present invention uses a complementary filtering algorithm to preprocess the sensor data to reduce the impact of sensor noise and errors on the data, and uses the processed data as the input of the IMM filter to perform multi-model intent estimation to obtain the final estimation result.

[0081] In a more specific technical solution, the UAV flight intention estimation system based on the new interactive multi-model includes:

[0082] The complementary filtering module is used to collect the drone sensor data, perform complementary filtering operations on the drone sensor data, and obtain the pitch angle estimation, position estimation and speed estimation through the complementary filtering operations;

[0083] An interactive multi-model framework construction module is used to select a motion model, which includes an acceleration model, a deceleration model, and a hovering model; a process of constructing the initial state equation and the observation equation of the model, wherein the motion equations are established for the acceleration model, the deceleration model, and the hovering model, respectively, to generate the initial state equation and the observation equation; a state transfer matrix is ​​described in a parameterized manner to adjust the model parameters of the motion model, and an interactive multi-model framework is obtained. The interactive multi-model framework construction module is connected to the complementary filtering module;

[0084] The target state estimation module is used to combine the complementary filter and the interactive multi-model framework to obtain the interactive multi-model IMM, input the pitch angle estimation, position estimation and speed estimation into the interactive multi-model IMM, and perform noise and error filtering operations through the complementary filter; select the appropriate model from the motion models such as the acceleration model, deceleration model and hovering model to estimate the target state, and the target state estimation module is connected with the interactive multi-model framework construction module and the complementary filtering module.

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

[0086] The present invention can effectively display position, speed and state information after complementary filtering combined with IMM processing. The present invention provides high-quality, low-noise data for IMM by preprocessing the data first, thereby enhancing its adaptability to dynamic data and improving the accuracy of prediction.

[0087] The present invention adopts a novel interactive multi-model architecture, which first uses complementary filtering to process the sensor data, filters out noise and errors, and then passes the clean and reliable data to the interactive multi-model filter, thereby improving the estimation accuracy.

[0088] The present invention adopts an interactive multi-model (IMM) which allows switching between multiple models that obey the Markov characteristic to better adapt to the flight conditions of the UAV. In the IMM interactive multi-model, the IMM filter relies on multiple sensor data inputs during the estimation process. If the sensor data quality is poor, such as the drift error of the IMU data or the signal loss and error of the GPS data, the estimation accuracy of the IMM filter will be significantly affected. Complementary filtering can be introduced to pre-process the sensor noise and error and improve the data quality. Therefore, the IMM interactive multi-model can be improved in this direction.

[0089] The present invention introduces complementary filtering to pre-process sensor data, thereby reducing noise and error and improving the estimation accuracy of the IMM filter.

[0090] The present invention designs and utilizes an interactive multiple model architecture, combines complementary filtering algorithms and advanced methods of interactive multiple models (IMM), and introduces complementary filtering algorithms into traditional interactive multiple models (IMM) to preprocess sensor data, aiming to improve the accuracy and flexibility of UAV flight intention estimation. The complementary filtering algorithm is a filtering method for multi-sensor data fusion, which is commonly used in scenarios such as attitude estimation and position estimation. The present invention improves estimation accuracy and robustness by utilizing the complementary characteristics of different sensors, such as high-frequency and low-frequency characteristics.

[0091] The present invention solves the technical problems existing in the prior art of the influence of sensor noise and error on data, and the low robustness, accuracy and flexibility of UAV flight intention estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] Figure 1 Schematic diagram of a method for estimating UAV flight intention based on a novel interactive multi-model according to Embodiment 1 of the present invention;

[0093] Figure 2 This is a schematic diagram of a novel interactive multi-model architecture according to Embodiment 1 of the present invention;

[0094] Figure 3A schematic diagram of specific steps for designing an interactive multi-model IMM according to Embodiment 1 of the present invention;

[0095] Figure 4 This is a schematic diagram of drone intention estimation data stream processing according to Embodiment 1 of the present invention;

[0096] Figure 5 This is a schematic diagram of the structural advantages of Embodiment 1 of the present invention;

[0097] Figure 6 This is a functional schematic diagram of Example 1 of the present invention. DETAILED DESCRIPTION

[0098] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0099] Example 1

[0100] like Figure 1 As shown, the method for estimating the flight intention of a UAV based on a novel interactive multi-model provided by the present invention includes the following basic steps:

[0101] S1, perform complementary filtering operation on drone data;

[0102] In this embodiment, the flight data of the drone is mainly obtained by the onboard sensors. The linear acceleration a of the drone is obtained by the accelerometer (IMU), the angular velocity ω of the drone is obtained by the gyroscope, and the position p and velocity v of the drone are obtained by the GPS, where: They represent the acceleration, angular velocity, position and speed of the x, y and z axes respectively.

[0103] In this embodiment, after determining the drone sensor data, a complementary filtering operation needs to be performed on it. The complementary filtering operation can be divided into two parts. The first part is the estimation of the pitch angle. The accelerometer data a and the gyroscope data ω are fused through complementary filtering to obtain the pitch angle estimation θ. The fusion formula is as follows:

[0104] θ t =α(θ t-1 +ω yt Δt)+(1-α)θ at

[0105] Among them, θ represents the estimated value of the pitch angle, α is the complementary filter coefficient, and its value ranges from 0 to 1. The closer it is to 1, the more trust is placed on the gyroscope data, and vice versa, the more trust is placed on the accelerometer data. y represents the angular velocity of the gyroscope on the y-axis, θ a It represents the pitch angle calculated by the accelerometer. The calculation formula is as follows:

[0106]

[0107] In this embodiment, the second part is the position and velocity estimation of complementary filtering, which combines the GPS position p and velocity v with the position p estimated by the IMU IMU and speed v IMU Perform weighted averaging to obtain a smoother and more accurate position p e and speed v e Estimate. First, we need to calculate the velocity v estimated by the IMU IMU :

[0108] v IMU =v et-1 +a t Δt

[0109] In this embodiment, the GPS speed v is fused with the IMU estimated speed v IMU Get the velocity estimate v e :

[0110] v et =αv IMUt +(1-α)v t

[0111] Get the velocity estimate v e After that, the position p estimated by IMU can be IMU Calculation:

[0112] p IMUt =p et-1 +v e Δt

[0113] Finally, the GPS position p is fused with the IMU estimated position p IMU Get the position estimate p e :

[0114] p et =αp IMUt +(1-α)p t

[0115] At this point, the complementary filtering process is completed, and the output data is the pitch angle estimate θ and the position estimate p e and velocity estimate v e , and then these data are fed as input into the IMM multi-model framework for intention estimation.

[0116] S2, select the motion model, construct the model's initial state equation and observation equation, and build and apply the interactive multi-model framework;

[0117] In this embodiment, during the operation of motion model selection, the establishment of the motion model is the basis of the IMM interactive multi-model. Without the model, IMM cannot be carried out. Determining the motion model of the drone is a very important step. Assuming that the drone is moving in a straight line in a plane, the motion model can be determined as acceleration, deceleration and hovering. First, the acceleration model means that the drone leans forward, accelerates forward, and the pitch angle increases. Secondly, the deceleration model indicates that the speed of the drone decreases, which is manifested as a backward tilt and a decrease in the pitch angle. Finally, the hovering model means that the drone is stationary in the air, with a speed of 0, an angular velocity of 0, and a pitch angle of 0. The combination of these models can accurately analyze and estimate the motion state of the drone in the plane.

[0118] In the process of constructing the initial state equation and observation equation of the model in this embodiment, the motion model m is determined. i After that, start to establish motion equations for the acceleration model, deceleration model, and hovering model, and generate initial state equations and observation equations. The advantages of the construction process of the three motion models in the present invention are reflected in: taking into account different motion models, covering a certain range of target motion behaviors; using a parameterized method to describe the state transfer matrix during the construction process, which makes it easy to adjust parameters in different situations to adapt to different needs; the formula logic is clear, easy to understand and modify, and the differences and characteristics of different models are well reflected. It can effectively reduce the complexity of the system and make it easier to debug and maintain; optimize the mathematical expression and calculation method of the motion equation, improve the calculation efficiency of the algorithm, and enable the model system to respond to the motion state of the drone more quickly, which is convenient for subsequent experimental needs.

[0119] In this embodiment, the state vector of the model is:

[0120]

[0121] In this embodiment, the model state equation is as follows:

[0122]

[0123] In this embodiment, the observation equation is as follows:

[0124] z k =H k x k +v k

[0125]

[0126] Among them, x k represents the state vector of the target at time k, is the motion model m i The system matrix, is the control matrix, is the control input vector, w k is the process noise, z k is the observation vector of the system, H k is the observation matrix. Since the model state vectors can be directly observed, H k is a four-row and four-column identity matrix, v k is the observation noise. The motion model is mainly determined by the pitch angle θ and the angular velocity ω. When the pitch angle θ>0 and the angular velocity ω>0 of the drone, it means that the drone is tilting forward and the angle continues to increase, which means that the drone is accelerating forward, that is, the acceleration model. When the pitch angle θ<0 and the angular velocity ω<0 of the drone, it means that the drone is tilting backward and the angle continues to decrease, which means that the drone is decelerating, that is, the deceleration model. When the pitch angle θ=0 and the angular velocity ω=0 of the drone, it means that the drone is in a horizontal state and there is no obvious tilt, which means that the drone is hovering, that is, the hovering model.

[0127] S3, design interactive multi-model IMM;

[0128] like Figure 2 As shown, in this embodiment, IMM (Interacting Multiple Model) interactive multiple model is an advanced estimation method, and the aforementioned model can effectively combine multiple motion models to improve the accuracy and robustness of state estimation. The core idea of ​​IMM includes: when the real motion mode of the target is uncertain, multiple pre-defined dynamic models are used to estimate the state of the target. Each model represents the possible motion characteristics of the target, such as motion models such as drone acceleration model, deceleration model and hovering model. IMM can adaptively select and combine the optimal model in complex motion scenes, thereby providing more accurate target state estimation. Therefore, the IMM interactive multiple model is particularly suitable for actual object motion scenes that need to deal with multiple uncertainties and changes. In the present invention, the interactive multiple model IMM has the following advantages over other inventions: combining complementary filters and interactive multiple model frameworks, the input of the model is the output result of complementary filtering, and the noise and errors in the sensor data are filtered out by complementary filters, and the influence of noise and errors on the IMM filter will be reduced, thereby improving the accuracy of estimation.

[0129] like Figure 3 and Figure 4 As shown, in this embodiment, step S3 of designing the interactive multi-model IMM further includes the following specific steps:

[0130] S31, preprocessing the sensor data using a complementary filtering algorithm, filtering out errors and noise in the process of sensor data collection, and outputting fused data;

[0131] In the model input process of this embodiment, there are a total of 3 motion models in the model set of the IMM algorithm, and the prior probability sequence corresponding to the model set is u(k)=(u1(k), u2(k), u3(k)). The switching between the models conforms to the Markov process, so the state transition probability matrix is ​​assumed to be:

[0132]

[0133] Determine the initial state vector of model j at time k Initial error covariance P 0j (k) and the interaction probability u ij (k). Where p ij is the transition probability from model i to model j. The probability will change over time. i (k) is the prior probability of model i at time k.

[0134]

[0135] In this embodiment, the core idea of ​​complementary filtering is to combine the advantages of two or more sensors or signal sources with different frequency ranges when processing signals, and learn from each other to obtain better estimation results. The core idea of ​​IMM is to model the system into multiple sub-models, and dynamically select the appropriate sub-model for state estimation and prediction according to the state change of the system. In terms of connection and logical relationship, in the IMM interactive multi-model, the IMM filter relies on multiple sensor data inputs during the estimation process. If the sensor data quality is poor, such as the drift error of IMU data or the signal loss and error of GPS data, the estimation accuracy of the IMM filter will be significantly affected. Complementary filtering can just solve this shortcoming, providing stable and low-noise attitude and motion estimation, ensuring the reliability of input data of IMM in complex environments, and then the interactive multi-model can flexibly respond to different flight mode changes of drones through parallel processing of multiple models and dynamic weight adjustment, making intention estimation more accurate and more flexible. The combination of the two can achieve efficient and accurate intention speculation at a low computational cost, and adapt to flight requirements in complex dynamic environments.

[0136] Therefore, the connection between complementary filtering and interactive multiple models combines the advantages of both, which can significantly improve the quality of sensor data, enhance the system's adaptability, real-time and robustness to complex dynamic environments, and reduce computational complexity. This combined method can more accurately and stably handle changes in different flight states and sudden behaviors in drone intention estimation.

[0137] S32, according to different flight states of the UAV, respectively establish filters based on different motion models, and initialize the state estimation of the filter based on the fusion data;

[0138] In this embodiment, Kalman filtering is used inside the IMM, and the Kalman filtering algorithm includes: a prediction operation and an update operation.

[0139] S33, using the IMM algorithm to combine each filter into a state estimate based on the estimated probability, the estimated state vector and its corresponding covariance matrix to obtain a filter estimation result;

[0140] Prediction Steps: represents the prediction of the state of model j at time k, is the covariance matrix of the predicted state, A(k) represents the observation matrix, B j (k) represents the control matrix, u j (k) represents the control input, A(k), B j (k),u j (k) is determined by the motion model, Q(k) is the covariance of the motion noise, generally Gaussian white noise

[0141] Status prediction:

[0142]

[0143] Covariance prediction:

[0144]

[0145] Update steps: represents the updated state estimate, P j (k) represents the covariance matrix of the updated state estimate, K(k) is the Kalman gain, which is used to adjust the balance between the predicted value and the observed value, z(k) is the observation vector at time k, H(k) is the observation matrix, which is also determined by the motion model. In this patent, it is a unit matrix with four rows and four columns, and R(k) is the covariance of the observation noise, which is generally Gaussian white noise.

[0146] Calculate the Kalman gain:

[0147]

[0148] Status Update:

[0149]

[0150] Covariance update:

[0151]

[0152] S34, updating the probability of the motion model using the likelihood function and the state transition probability;

[0153] The model probability update is as follows:

[0154]

[0155] The model likelihood function Λ j The calculation formula of (k) is as follows:

[0156]

[0157] Where S j (k) is the covariance matrix of the jth motion model update at time k, V j (k) is the residual of the jth filter at time step k, calculated as follows:

[0158]

[0159] S35, estimating the updated motion model probability, and obtaining the final state estimation by weighted fusion of the estimated values ​​of each filter;

[0160] In the fusion output operation of this embodiment, represents the fusion state estimation, P k represents the covariance matrix of the fusion state estimate;

[0161] State Estimation:

[0162]

[0163] Covariance estimation:

[0164]

[0165] Example 2

[0166] In complex moving body navigation and tracking systems, the combination of complementary filtering and interactive multiple models (IMM) can significantly improve the estimation accuracy of the moving body state (such as posture, position, and speed) and adapt to complex motion behaviors in a variety of dynamic environments. This combination strategy combines the advantages of both and solves the problem that a single filter is difficult to cope with multi-sensor data fusion and multi-motion mode switching.

[0167] Attitude estimation is the basis for the control of all moving bodies (such as vehicles, ships, drones, etc.). Usually, the attitude of a moving body is provided by multiple sensors (such as accelerometers and gyroscopes), but these sensors have their own advantages and disadvantages. Accelerometers can provide stable long-term attitude estimates, especially low-frequency information, but are susceptible to short-term dynamic interference. Gyroscopes can accurately capture high-frequency angular velocity changes in the short term, but their drift accumulates over time, resulting in inaccurate long-term estimates. Complementary filtering combines the advantages of both: using accelerometers to correct the drift of gyroscopes, and using the high-frequency dynamic response of gyroscopes to compensate for the short-term errors of accelerometers. This complementary mechanism makes attitude estimation both stable and accurate in the rapid dynamic changes of moving bodies.

[0168] Position and velocity estimation is more complicated in the navigation of moving objects, especially when the moving object often changes its motion mode (such as turning, accelerating, decelerating, etc.). A single dynamic model is difficult to cope with the multi-mode switching of the moving object, and the interactive multi-model (IMM) provides a solution for this. IMM assumes that the moving object may have multiple motion modes, and each mode has its own corresponding motion model (such as uniform linear motion, uniformly accelerated motion, turning motion, etc.). For each mode, the system uses a filter (such as a Kalman filter or an extended Kalman filter) for estimation. During actual operation, IMM runs multiple filters in parallel, calculates the probability of each model in real time, and dynamically adjusts the weights of each filter based on these probabilities, and finally generates the best estimate of the moving object.

[0169] For example, when a moving object equipped with sensors is moving, it needs to estimate its posture, speed and position in real time. In order to ensure safety and efficiency, the moving object usually uses a motion capture system to perceive its own motion state and infer the future motion state estimation results.

[0170] First, the complementary filtering algorithm is used to fuse and estimate the data of two or more sensors and output the fused data. The fused data is then used as the input data of the IMM filter. The IMM filter performs model interaction, parallel filtering, model probability update and fusion output steps on the complementary filter data and then outputs the final intention estimation result.

[0171] By combining complementary filtering and IMM, the moving body system can effectively process different sensor data and multiple motion modes in uncertain and complex environments, providing accurate and continuous attitude, velocity and position estimation. This method is widely used in motion control and tracking tasks in autonomous driving, aircraft, ships, robots and other complex dynamic systems, significantly improving the robustness and accuracy of navigation and control.

[0172] like Figure 5As shown, the structural advantages of the present invention are mainly reflected in the model flexibility. The IMM interactive multi-model allows switching between different dynamic models, which can autonomously select or adaptively use the most suitable model according to the flight state of the drone; complementary filtering can effectively fuse multiple sensor data (such as accelerometers, gyroscopes, etc.), reduce noise and drift, and provide stable and accurate attitude estimation. It removes high-frequency noise and compensates low-frequency drift in the signal preprocessing stage, making the sensor data more reliable and able to balance steady-state and dynamic responses. Under steady-state conditions, complementary filtering provides accurate attitude information through a low-pass filter; under dynamic conditions, a high-pass filter can help capture high-speed motion changes. This balance provides IMM with more accurate state input, which helps it make more accurate intention estimates in steady-state and dynamic changes. A new interactive multi-model architecture, that is, complementary filtering combined with interactive multi-model (IMM), can significantly improve the quality of sensor data, enhance the system's adaptability, real-time and robustness to complex dynamic environments, and reduce computational complexity.

[0173] like Figure 6 As shown in the figure, in terms of functionality, this new interactive multi-model architecture performs well in terms of accuracy in intent estimation. This achievement is due to the complementary filtering algorithm. In the IMM interactive multi-model, the IMM filter relies on multiple sensor data inputs during the estimation process. If the sensor data quality is poor, such as drift error of IMU data or signal loss and large error of GPS data, IMM may make errors when estimating the state and switching models, resulting in inaccurate or untimely intent estimation. IMM itself does not have a special mechanism to deal with sensor noise and drift. Therefore, the estimation accuracy of the IMM filter will be significantly affected. Complementary filtering can effectively suppress noise in different frequency bands and reduce the impact of sensor drift through the combination of high-pass and low-pass filters. The sensor data after complementary filtering is cleaner and more reliable, which improves the estimation accuracy and robustness of the IMM filter. Through the preprocessing of sensor data by complementary filtering, IMM can work with higher quality input, thereby improving the accuracy and stability of drone intent estimation.

[0174] In summary, the present invention can effectively display position, speed and state information after complementary filtering combined with IMM processing. The present invention provides high-quality, low-noise data for IMM by preprocessing the data first, thereby enhancing its adaptability to dynamic data and improving the accuracy of prediction.

[0175] The present invention adopts a novel interactive multi-model architecture, which first uses complementary filtering to process the sensor data, filters out noise and errors, and then passes the clean and reliable data to the interactive multi-model filter, thereby improving the estimation accuracy.

[0176] The present invention adopts an interactive multi-model (IMM) which allows switching between multiple models that obey the Markov characteristic to better adapt to the flight conditions of the UAV. In the IMM interactive multi-model, the IMM filter relies on multiple sensor data inputs during the estimation process. If the sensor data quality is poor, such as the drift error of the IMU data or the signal loss and error of the GPS data, the estimation accuracy of the IMM filter will be significantly affected. Complementary filtering can be introduced to pre-process the sensor noise and error and improve the data quality. Therefore, the IMM interactive multi-model can be improved in this direction.

[0177] The present invention introduces complementary filtering to pre-process sensor data, thereby reducing noise and error and improving the estimation accuracy of the IMM filter.

[0178] The present invention designs and utilizes an interactive multiple model architecture, combines complementary filtering algorithms and advanced methods of interactive multiple models (IMM), and introduces complementary filtering algorithms into traditional interactive multiple models (IMM) to preprocess sensor data, aiming to improve the accuracy and flexibility of UAV flight intention estimation. The complementary filtering algorithm is a filtering method for multi-sensor data fusion, which is commonly used in scenarios such as attitude estimation and position estimation. The present invention improves estimation accuracy and robustness by utilizing the complementary characteristics of different sensors, such as high-frequency and low-frequency characteristics.

[0179] The present invention solves the technical problems existing in the prior art of the influence of sensor noise and error on data, and the low robustness, accuracy and flexibility of UAV flight intention estimation.

[0180] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A UAV flight intention estimation method based on a novel interactive multi-model, characterized in that: The method comprises: S1, collecting drone sensor data, performing complementary filtering operation on the drone sensor data, and obtaining pitch angle estimation, position estimation and speed estimation through the complementary filtering operation; S2, selecting a motion model, the motion model including: an acceleration model, a deceleration model and a hovering model; constructing a model initial state equation and an observation equation, wherein the motion equations are respectively established for the acceleration model, the deceleration model and the hovering model to generate the initial state equation and the observation equation; describing the state transfer matrix in a parameterized manner to adjust the model parameters of the motion model to obtain an interactive multi-model framework; S3. Combine the complementary filter and the interactive multi-model framework to obtain an interactive multi-model IMM, input the pitch angle estimation, the position estimation and the speed estimation into the interactive multi-model IMM, and perform noise and error filtering operations through the complementary filter; select an applicable model from the motion models such as the acceleration model, the deceleration model and the hovering model to estimate the target state.

2. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 1 is characterized in that: The S1 includes: S11, fusing the accelerometer data a and the gyroscope data ω through complementary filtering to obtain the pitch angle estimation θ: i t =α(θ t-1 +oh yt Δt)+(1-α)θ at In the formula, θ represents the estimated value of the pitch angle, α is the complementary filter coefficient, which ranges from 0 to 1, and ω y represents the angular velocity of the gyroscope on the y-axis, θ a Represents the pitch angle calculated by the accelerometer: S12, compare the GPS position p and speed v in the drone sensor data with the position P estimated by the IMU IMU and speed v IMU Perform weighted averaging to obtain the position estimate p e , the velocity estimate v e ; S13, the pitch angle estimate θ, the position estimate p e and the velocity estimate v e , which is input into the IMM multi-model framework for intention estimation.

3. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 2 is characterized in that: In S12, the following logic is used to calculate the IMU estimated speed v IMU : v IMU =v et-1 +a t Δt The GPS velocity v is fused with the IMU estimated velocity v using the following logic: IMU , get the velocity estimate v e : v et =αv IMUt +(1-a)v t According to the velocity estimate v e , use the following logic to perform IMU estimation and obtain the IMU estimated position p IMU : p IMUt =p et-1 +v e Δt Fuse the GPS position p with the IMU estimated position p IMU , and obtain the position estimate p e : p et =αp IMUt +(1-α)p t 。 4. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 1 is characterized in that: In S2, the state vector of the motion model is expressed using the following logic: The initial state equation of the motion model is expressed using the following logic: The observation equation is expressed using the following logic: z k =H k x k +v k Among them, x k represents the state vector of the target at time k, is the motion model m i The system matrix, is the control matrix, is the control input vector, w k is the process noise, z k is the observation vector of the system, H k is the observation matrix. Since the model state vectors can be directly observed, H k is a four-row and four-column identity matrix, v k is the observation noise; The motion model is determined according to the pitch angle θ and the angular velocity ω.

5. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 1 is characterized in that: The S3 includes: S31, preprocessing the pitch angle estimation, the position estimation and the speed estimation to obtain and output fusion data; The following logic is used to express the prior probability sequences corresponding to the acceleration model, the deceleration model, and the hovering model: u(k)=(u1(k), u2(k), u3(k)); The following logic is used to set the state transition probability matrix according to the Markov process that switches between the acceleration model, the deceleration model and the hovering model: Determine the initial state vector of the motion model j at time k Initial error covariance P 0j (k) and the interaction probability u ij (k): Determine the transition probability of the motion model i to j, and use the following logic to determine the prior probability u of the model i at time k i (k): S32, according to different flight states of the UAV, respectively establish motion model filters for the acceleration model, the deceleration model and the hovering model, and initialize the state estimation of the filter based on the fusion data; perform Kalman filtering in the interactive multi-model IMM; S33, using the interactive multi-model IMM, according to the estimated probability, the estimated state vector and its corresponding covariance moment, using each filter, performing prediction operation and update operation to complete the combined state estimation and obtain the filtering estimation result; S34, obtaining and using the model likelihood function and the state transition probability to perform a model probability update operation on the motion model to obtain an updated motion model; S35, estimating the probability of the updated motion model, weightedly fusing the estimated values ​​of each filter in the updated motion model, performing fusion state estimation through a fusion output operation, and processing to obtain an applicable state estimation result.

6. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 5 is characterized in that: In the prediction operation of S33, the following logic is used to perform state prediction: Use the following logic to make covariance predictions: In the formula, represents the prediction of the state of model j at time k, is the covariance matrix of the predicted state, A(k) represents the observation matrix, B j (k) represents the control matrix, u j (k) represents the control input, A(k), B j (k) and u j (k) is determined by the motion model, and Q(k) is the covariance of the motion noise.

7. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 5 is characterized in that: In the updating operation of S33, the Kalman gain is calculated using the following logic: The state of the motion model is updated using the following logic: Use the following logic to update the covariance: In the formula, represents the updated state estimate, P j (k) represents the covariance matrix of the updated state estimate, K(k) is the Kalman gain, z(k) is the observation vector at time k, H(k) is the observation matrix, and R(k) is the covariance of the observation noise.

8. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 5 is characterized in that: In S34, the model probability update operation is performed using the following logic: Among them, the model likelihood function Λ is obtained by using the following logic: j (k): In the formula, S j (k) is the covariance matrix of the j-th motion model update at time k, V j (k) is the residual of the jth filter at time step k:

9. The method for estimating UAV flight intention based on a novel interactive multi-model according to claim 1, characterized in that: In the fusion output operation of S35, represents the fusion state estimation, P k represents the covariance matrix of the fusion state estimate; The fusion state estimation is performed using the following logic: The covariance matrix is ​​obtained by processing using the following logic:

10. A UAV flight intention estimation system based on a novel interactive multi-model, characterized in that: The system comprises: A complementary filtering module is used to collect drone sensor data, perform complementary filtering operations on the drone sensor data, and obtain pitch angle estimation, position estimation, and speed estimation through the complementary filtering operations; An interactive multi-model framework construction module is used to select a motion model, wherein the motion model includes: an acceleration model, a deceleration model, and a hovering model; a process of constructing an initial state equation and an observation equation of the model, wherein motion equations are established for the acceleration model, the deceleration model, and the hovering model, respectively, to generate the initial state equation and the observation equation; a state transfer matrix is ​​described in a parameterized manner to adjust the model parameters of the motion model to obtain an interactive multi-model framework, wherein the interactive multi-model framework construction module is connected to the complementary filtering module; A target state estimation module is used to combine the complementary filter and the interactive multi-model framework to obtain an interactive multi-model IMM, input the pitch angle estimation, the position estimation and the speed estimation into the interactive multi-model IMM, and perform noise and error filtering operations through the complementary filter; select an applicable model from the motion models such as the acceleration model, the deceleration model and the hovering model to estimate the target state, and the target state estimation module is connected to the interactive multi-model framework construction module and the complementary filtering module.

Images