Multi-sensor fusion positioning method based on four-rotor unmanned aerial vehicle

Abstract

A multi-sensor fusion positioning method based on a quad-rotor unmanned aerial vehicle comprises the following steps: firstly, preliminarily screening position data by setting a series of judgment conditions, identifying and removing abnormal values, and inputting the screened position information into a deep learning model for training; then, output data obtained after deep learning model processing serves as input and is transmitted to a Holt linear prediction method; and finally, further optimizing and fusing the position data by using a Kalman filtering model. According to the method, the deep learning model, the Holt linear prediction technology and the Kalman filtering technology are combined, the precision and reliability of position information can be greatly improved, especially in the dynamic and complex environment, abnormal data and uncertainty can be better processed, and a more accurate position information prediction and fusion result is provided.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor information fusion, and specifically relates to a positioning method based on T265 binocular camera and 2D laser radar posture information fusion, which improves the positioning accuracy and robustness of drones during operation. Background Art

[0002] In recent years, with the rapid development and widespread application of technologies such as drones, robots, and autonomous driving, unmanned systems have played an increasingly important role in daily life, industrial automation, and intelligent transportation. The autonomous navigation and task execution capabilities of unmanned systems are inseparable from accurate and reliable environmental perception and positioning technologies. Among them, the environmental perception and modeling, positioning, and map construction modules provide the unmanned system with external environment and its own state information, which is the prerequisite for realizing complex autonomous control functions and is also the core and prerequisite of unmanned system technology.

[0003] In the past, traditional single sensor perception technology could only provide limited information and was also affected by its own quality and performance, which could not meet the needs of high-precision positioning and map construction in complex scenarios. Therefore, positioning based on multi-sensor information fusion has become a hot topic and challenge in current research.

[0004] Positioning is very important for UAVs. It directly affects the navigation, flight control, mission execution and other aspects of UAVs. The changing environmental factors during the operation of UAVs have a great impact on binocular cameras and lidars. Positioning with a single sensor not only cannot guarantee the positioning accuracy, but also when the sensor is damaged, the UAV will lack posture information and robustness cannot be guaranteed.

[0005] During operation, the T265 binocular camera estimates the movement of the camera by analyzing the pixel displacement between consecutive frames based on the visual odometer (VO), and then uses the IMU to judge its own posture. After fusing the binocular camera and IMU, the drone's posture information is output. However, the camera is greatly affected when the light changes, so there may be large errors in actual operation. 2D laser radar determines its own position by identifying and tracking specific points or structural objects in the map, but it will produce large errors in the case of vibration and rapid movement. At the same time, 2D laser radar only has two-dimensional information and lacks height information. 3D laser radar is not only expensive, but also heavy, which is not so friendly to drones. Summary of the invention

[0006] In order to overcome the shortcomings of the prior art, the present invention provides a multi-sensor fusion positioning method based on a quad-rotor drone, which predicts abnormal data based on time series prediction combined with a deep learning model LSTM, and then uses the Kalman filter method to fuse posture information. Specifically, first, the abnormal position is screened through certain judgment conditions, and then historical data is added to the deep learning model for prediction. Then the output data is used as the input of the Holt linear prediction to predict the position data. Finally, the output position information that meets the requirements is input into the Kalman filter model, and the position information is fused by updating the covariance matrix, gain and state of the Kalman filter.

[0007] The present invention provides the following solutions to solve the above technical problems:

[0008] A multi-sensor fusion positioning method based on a quad-rotor drone comprises the following steps:

[0009] 1) Establish a function to filter location information. The process is as follows:

[0010] 1.1) Take out the T265 binocular camera and 2D laser radar position data pos_t265[i] and pos_laser[i] respectively;

[0011] 1.2) Set the judgment threshold sum and weight k;

[0012] 1.3) Use the weighted average of the two sensor measurement values ​​((k*pos_t265[i]+(1-k)*pos_laser[i])-sum, (k*pos_t265[i]+(1-k)*pos_laser[i])-sum) as the judgment condition;

[0013] 1.4) Determine whether pos_t265[i] and pos_laser[i] are within the determination area;

[0014] 1.5) If the condition in 1.4) is met, the sensor data is used as the filtered data;

[0015] 1.6) If the condition in 1.4) is not met, take out the weighted average data of the measured values ​​at the current moment, and use the weighted average data of the measured values ​​as a constraint to predict the data that does not meet the requirements at the current moment;

[0016] 1.7) Take out the estimated data that meets the requirements and add it to the Kalman filter;

[0017] 2) Build an LSTM prediction model, and effectively retain and forget data by introducing a gate control mechanism. Its main features are the gate control mechanism and unit state update. The gate control mechanism includes input gate, forget gate, and output gate.

[0018] 3) Establish the Holt-Winters model:

[0019] 4) Establish a Kalman filter and use the Kalman filter model to further optimize and fuse the position data.

[0020] Further, the process of 2) is as follows:

[0021] 2.1) The input gate is responsible for updating the current state. It is a sigmoid layer that determines which values ​​need to be updated, and a tanh layer that creates a new candidate value vector, which will then be added to the state.

[0022] i t =σ(W i ·[h t-1 ,x t ]+b i ) (1)

[0023] Among them, i t is the output of the input gate, σ is the sigmoid function, W i The weight of the input gate, b i is the bias of the input gate, h t-1 is the previous hidden state, x t is the current input;

[0024]

[0025] in, is the candidate value vector, W c is the weight of the candidate value vector, b c is the bias of the candidate value vector.

[0026] 2.2) The forget gate is used to decide what information to discard from the cell state.

[0027] f t =ω(W f ·[h t-1 ,x t ]+b f ) (3)

[0028] Among them, f t represents the output of the forget gate at time t, σ is the sigmoid function, W f is the weight of the forget gate, b f is the bias of the forget gate,

[0029] 2.3) Output gate, which is responsible for determining the next hidden state. The hidden state contains information about the previous input and is used for prediction.

[0030] o t =σ(W o ·[h t-1 ,x t ]+b o ) (4)

[0031] Among them, t is the output of the output gate, W o is the weight of the output gate, b o is the bias of the output gate.

[0032] 2.4) Calculate candidate values ​​for the new cell state,

[0033] g t =tanh(W c ·[h t-1 ,x t ]+b c ) (5)

[0034] Among them, g t Candidate values ​​for the cell state, W c is the weight matrix of the unit state, b c is the bias of the cell state;

[0035] 2.5) Update the cell state according to the forget gate and input gate,

[0036] c t =f t ·c t-1 +i t ·g t (6)

[0037] Among them, c t is the cell state at the previous time step,

[0038] 2.6) Output,

[0039] h t =o t ·tanh c t (7)

[0040] Among them, h t is the output of the current time step.

[0041] Furthermore, in 3), the Holt-Winters model is established:

[0042] S t =αx t +(1-α)(S t-1 +B t-1 ) (8)

[0043] Bt =β(S t -S t-1 )+(1-β)B t-1 (9)

[0044] Where α is the data smoothing factor, 0<α<1; β is the number trend smoothing factor, 0<β<1; S t To update the level value, B t For updates on trends;

[0045] The prediction formula is,

[0046] F t+m =S t +mB t (10)

[0047] Among them, F t+m is the prediction for the next moment, and m is the length of time required for prediction.

[0048] Furthermore, the process of 4) is as follows:

[0049] 4.1) Establish the state equation and observation equation of the sensor:

[0050] x k =f(x k-1 ,u k ,ω k )=ω k +F k (x k-1 )+B k u k (11)

[0051] Among them, x k represents the true value at time k, ω k represents the process noise, u k is the input value, F represents the state transfer matrix, and B represents the control matrix;

[0052] z k =h(x k ,v k )=H k (x k )+v k (12)

[0053] Among them, z k represents the observed value, v k is the observation noise, H k represents the observation matrix;

[0054] 4.2) Establish state estimation equation and observation estimation equation:

[0055]

[0056] in, It means that the estimated value of x at the k-1th moment is k estimates;

[0057]

[0058] in, It means that the estimated value of x at the k-1th moment is k Observation of

[0059] 4.3) Establish state estimation error and observation estimation error:

[0060]

[0061] Among them, represents the estimation error of time k to time k-1,

[0062]

[0063] in, represents the observation error at time k to time k-1;

[0064] 4.4) Establish the covariance matrix:

[0065]

[0066] Among them, P k|k-1 represents the state error covariance matrix,

[0067]

[0068] Among them, S K represents the observation error covariance matrix;

[0069] 4.5) Establish Kalman filter gain:

[0070]

[0071] Where K k Expressed as Kalman filter gain;

[0072] 4.6) Update the estimate

[0073] P k|k =P k|k-1 -K k H k P k|k-1 (20)

[0074] Among them, P k|k Updated state error covariance matrix;

[0075]

[0076] in, Updated estimates.

[0077] The technical concept of the present invention is as follows: first, after obtaining the position of the sensor, use the position screening function to screen out some abnormal position data, then use the long short-term memory network to predict the current position data and add it to the data prediction queue, and then use the Holt-Winters model_time series recursive prediction method of quadratic exponential smoothing to predict abnormal position data. Finally, the position information that meets the requirements is fused using the Kalman filter algorithm to output a more accurate position and posture information.

[0078] The beneficial effects of the present invention are: combining deep learning models, Holt linear prediction and Kalman filtering technology, can greatly improve the accuracy and reliability of position information, especially in dynamic and complex environments, can better handle abnormal data and uncertainty, and provide more accurate position information prediction and fusion results. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is the trajectory diagram of the abnormal output posture of the binocular camera data when the drone is running;

[0080] Figure 2 It is the trajectory diagram of the abnormal output posture of the lidar data when the drone is running;

[0081] Figure 3 It is the trajectory diagram of the drone's output posture when there is no data anomaly during operation; DETAILED DESCRIPTION

[0082] The present invention will be further described below in conjunction with the accompanying drawings.

[0083] Reference Figure 1 to Figure 3 , a multi-sensor fusion positioning method based on a quadrotor drone, comprising the following steps:

[0084] 1) Establish a function to filter location information. The process is as follows:

[0085] 1.1) Take out the T265 binocular camera and 2D laser radar position data pos_t265[i] and pos_laser[i] respectively;

[0086] 1.2) Set the judgment threshold sum and weight k;

[0087] 1.3) Use the weighted average of the two sensor measurement values ​​((k*pos_t265[i]+(1-k)*pos_laser[i])-sum, (k*pos_t265[i]+(1-k)*pos_laser[i])-sum) as the judgment condition;

[0088] 1.4) Determine whether pos_t265[i] and pos_laser[i] are within the determination area;

[0089] 1.5) If the condition in 1.4) is met, the sensor data is used as the filtered data;

[0090] 1.6) If the condition in 1.4) is not met, take out the weighted average data of the measured values ​​at the current moment, and use the weighted average data of the measured values ​​as a constraint to predict the data that does not meet the requirements at the current moment;

[0091] 1.7) Take out the estimated data that meets the requirements and add it to the Kalman filter;

[0092] Where sum is the set screening threshold, pos_t265[i] and pos_la represent the storage of binocular camera position information and lidar position information respectively, i=0, 1, 2;

[0093] 2) Build an LSTM prediction model. The long short-term memory network (LSTM) is a special recursive neural network structure. It mainly introduces a gate control mechanism to effectively retain and forget data. Its main features are the gate control mechanism and unit state update. The gate control mechanism includes input gate, forget gate, and output gate. The process is as follows:

[0094] 2.1) The input gate is responsible for updating the current state. It is a sigmoid layer that determines which values ​​need to be updated, and a tanh layer that creates a new candidate value vector, which will then be added to the state.

[0095] i t =σ(W i ·[h t-1 ,x t ]+b i ) (1)

[0096] Among them, i t is the output of the input gate, σ is the sigmoid function, W i The weight of the input gate, b i is the bias of the input gate, h t-1 is the previous hidden state, x t is the current input;

[0097]

[0098] in, is the candidate value vector, W c is the weight of the candidate value vector, b c is the bias of the candidate value vector.

[0099] 2.2) The forget gate is used to decide what information to discard from the cell state.

[0100] f t =σ(W f ·[h t-1 ,x t ]+b f ) (3)

[0101] Among them, f t represents the output of the forget gate at time t, σ is the sigmoid function, W f is the weight of the forget gate, b f is the bias of the forget gate,

[0102] 2.3) Output gate, which is responsible for determining the next hidden state. The hidden state contains information about the previous input and is used for prediction.

[0103] o t =σ(W o ·[h t-1 ,x t ]+b o ) (4)

[0104] Among them, t is the output of the output gate, W o is the weight of the output gate, b o is the bias of the output gate.

[0105] 2.4) Calculate candidate values ​​for the new cell state,

[0106] g t =tanh(W c ·[h t-1 ,x t ]+b c ) (5)

[0107] Among them, g t Candidate values ​​for the cell state, W c is the weight matrix of the unit state, b c is the bias of the cell state;

[0108] 2.5) Update the cell state according to the forget gate and input gate,

[0109] c t =f t ·ct-1 +i t ·g t (6)

[0110] Among them, c t is the cell state at the previous time step,

[0111] 2.6) Output,

[0112] h t =o t ·tanh c t (7)

[0113] Among them, h t is the output of the current time step.

[0114] In 2), the current input x t and the output h of the previous time step t-1 Concatenate into a vector.

[0115] Step 3) Establish the Holt-Winters model:

[0116] S t =αx t +(1-α)(S t-1 +B t-1 ) (8)

[0117] B t =β(S t -S t-1 )+(1-β)B t-1 (9)

[0118] Where α is the data smoothing factor, 0<α<1; β is the number trend smoothing factor, 0<β<1; S t To update the level value, B t For updates on trends;

[0119] The prediction formula is,

[0120] F t+m =S t +mB t (10)

[0121] Among them, F t+m is the prediction of the next moment, and m is the length of time required for prediction;

[0122] 4) Establish Kalman filtering, the process is as follows:

[0123] 4.1) Establish the state equation and observation equation of the sensor:

[0124] x k =f(xk-1 ,u k ,ω k )=ω k +F k (x k-1 )+B k u k (11)

[0125] Among them, x k represents the true value at time k, ω k represents the process noise, u k is the input value, F represents the state transfer matrix, and B represents the control matrix;

[0126] z k =h(x k ,v k )=H k (x k )+v k (12)

[0127] Among them, z k represents the observed value, v k is the observation noise, H k represents the observation matrix;

[0128] 4.2) Establish state estimation equation and observation estimation equation:

[0129]

[0130] in, It means that the estimated value of x at the k-1th moment is k estimates;

[0131]

[0132] in, It means that the estimated value of x at the k-1th moment is k Observation of

[0133] 4.3) Establish state estimation error and observation estimation error:

[0134]

[0135] Among them, x~ k|k-1 represents the estimation error of time k to time k-1,

[0136]

[0137] in, represents the observation error at time k to time k-1;

[0138] 4.4) Establish the covariance matrix:

[0139]

[0140] Among them, P k|k-1 represents the state error covariance matrix,

[0141]

[0142] Among them, S K represents the observation error covariance matrix;

[0143] 4.5) Establish Kalman filter gain:

[0144]

[0145] Where K k Expressed as Kalman filter gain;

[0146] 4.6) Update the estimate

[0147] P k|k =P k|k-1 -K k H k P k|k-1 (20)

[0148] Among them, P k|k Updated state error covariance matrix;

[0149]

[0150] in, Updated estimates;

[0151] In the above 4), since the position and attitude conform to Gaussian distribution, the fusion of position and attitude information can be completed by Kalman filtering to establish Kalman filtering;

[0152] In the update estimation of 4.6), after optimizing the obtained position and attitude information, the position and attitude information of the binocular camera is added to the state equation, Kalman filtering is performed, and the updated position and attitude information values ​​are output. Then, the position and attitude information of the lidar and the updated position and attitude information values ​​obtained in the previous step are added to the state equation together, Kalman filtering is performed, and the updated position and attitude information values ​​are output. Finally, the position and attitude information of the motion capture system and the position and attitude information values ​​output after executing the lidar are added to the state equation together, Kalman filtering is performed, and the updated position and attitude information values ​​are output to obtain the final fused position and attitude information.

[0153] The contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A multi-sensor fusion positioning method based on a quad-rotor drone, characterized in that: The method comprises the following steps: 1) Establish a function to filter location information. The process is as follows: 1.1) Take out the T265 binocular camera and 2D laser radar position data pos_t265[i] and pos_laser[i] respectively; 1.2) Set the judgment threshold sum and weight k; 1.3) Use the weighted average of the two sensor measurement values ​​((k*pos_t265[i]+(1-k)*pos_laser[i])-sum, (k*pos_t265[i]+(1-k)*pos_laser[i])-sum) as the judgment condition; 1.4) Determine whether pos_t265[i] and pos_laser[i] are within the determination area; 1.5) If the condition in 1.4) is met, the sensor data is used as the filtered data; 1.6) If the condition in 1.4) is not met, take out the weighted average data of the measured values ​​at the current moment, and use the weighted average data of the measured values ​​as a constraint to predict the data that does not meet the requirements at the current moment; 1.7) Take out the estimated data that meets the requirements and add it to the Kalman filter; 2) Build an LSTM prediction model, and effectively retain and forget data by introducing a gate control mechanism. Its main features are the gate control mechanism and unit state update. The gate control mechanism includes input gate, forget gate, and output gate. 3) Establish the Holt-Winters model: 4) Establish a Kalman filter and use the Kalman filter model to further optimize and fuse the position data.

2. A multi-sensor fusion positioning method based on a quad-rotor drone as claimed in claim 1, characterized in that: The process of 2) is as follows: 2.1) The input gate is responsible for updating the current state. It is a sigmoid layer that determines which values ​​need to be updated, and a tanh layer that creates a new candidate value vector, which is then added to the state. i t =σ(W i ·[h t-1 ,x t ]+b i ) (1) Among them, i t is the output of the input gate, σ is the sigmoid function, W i The weight of the input gate, b i is the bias of the input gate, h t-1 is the previous hidden state, x t is the current input; in, is the candidate value vector, W c is the weight of the candidate value vector, b c is the bias of the candidate value vector; 2.2) The forget gate is used to decide what information to discard from the cell state. f t =σ(W f ·[h t-1 ,x t ]+b f ) (3) Among them, f t represents the output of the forget gate at time t, σ is the sigmoid function, W f is the weight of the forget gate, b f is the bias of the forget gate, 2.3) Output gate, which is responsible for determining the next hidden state. The hidden state contains information about the previous input and is used for prediction. the t =σ(W o ·[h t-1 ,x t ]+b o ) (4) Among them, t is the output of the output gate, W o is the weight of the output gate, b o is the bias of the output gate; 2.4) Calculate candidate values ​​for the new cell state, g t =tanh(W c ·[h t-1 ,x t ]+b c ) (5) Among them, g t Candidate values ​​of the cell state, W c is the weight matrix of the unit state, b c is the bias of the cell state; 2.5) Update the cell state according to the forget gate and input gate, c t =f t ·c t-1 +i t ·g t (6) Among them, c t is the cell state at the previous time step, 2.6) Output, h t =o t fishy t (7) Among them, h t is the output of the current time step.

3. A multi-sensor fusion positioning method based on a quad-rotor drone as claimed in claim 2, characterized in that: In the above 3), the Holt-Winters model is established: S t =αx t +(1-α)(S t-1 +B t-1 ) (8) B t =β(S t -S t-1 )+(1-β)B t-1 (9) Where α is the data smoothing factor, 0<α<1; β is the number trend smoothing factor, 0<β<1; S t To update the level value, B t For updates on trends; The prediction formula is, F t+m =S t +mB t (10) Among them, F t+m is the prediction for the next moment, and m is the length of time required for prediction.

4. A multi-sensor fusion positioning method based on a quad-rotor drone as claimed in claim 3, characterized in that: The process of 4) is as follows: 4.1) Establish the state equation and observation equation of the sensor: x k =f(x k-1 ,u k ,ω k )=ω k +F k (x k-1 )+B k u k (11) Among them, x k represents the true value at time k, ω k represents the process noise, u k is the input value, F represents the state transfer matrix, and B represents the control matrix; z k =h(x k ,v k )=H k (x k )+v k (12) Among them, z k represents the observed value, v k is the observation noise, H k represents the observation matrix; 4.2) Establish state estimation equation and observation estimation equation: in, It means that the estimated value of x at the k-1th moment is k estimates; in, It means that the estimated value of x at the k-1th moment is k Observation of 4.3) Establish state estimation error and observation estimation error: Among them, represents the estimation error of time k to time k-1, in, represents the observation error at time k to time k-1; 4.4) Establish the covariance matrix: Among them, P k|k-1 represents the state error covariance matrix, Among them, S K represents the observation error covariance matrix; 4.5) Establish Kalman filter gain: Where K k Expressed as Kalman filter gain; 4.6) Update the estimate Among them, P k|k Updated state error covariance matrix; in, Updated estimates.

Images