A multi-sensor fusion positioning method based on a quad-rotor unmanned aerial vehicle

Abstract

A kind of multi-sensor fusion positioning method based on quadrotor unmanned aerial vehicle, first, a series of judgment conditions are set to carry out preliminary screening to position data, identify and eliminate outliers, input the position information screened into a deep learning model for training;Then, the output data obtained after being processed by the deep learning model is passed to the Holt linear prediction method as input;Finally, the position data is further optimized and fused using Kalman filter model.The present application combines deep learning model, Holt linear prediction and Kalman filter technology, which 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.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of multi-sensor information fusion, and particularly relates to a positioning method based on T265 binocular camera and 2D laser radar pose information fusion. BACKGROUND

[0002] In recent years, with the rapid development and wide application of unmanned aerial vehicles, robots and automatic driving technologies, unmanned systems play an increasingly important role in daily life, industrial automation and intelligent transportation. The autonomous navigation and task execution capabilities of unmanned systems cannot be separated from accurate and reliable environment perception and positioning technologies. Among them, environment perception and modeling, positioning and map construction modules provide the outside environment and the state information of the unmanned system, which are the premise of realizing complex autonomous control functions and the core and premise of unmanned system technology.

[0003] The traditional single sensor perception technology in the past can only provide limited information and is affected by its own quality and performance, which cannot meet the high-precision positioning and map construction requirements in complex scenarios. Therefore, multi-sensor information fusion-based positioning has become a hot topic and challenge in current research.

[0004] Positioning is very important for unmanned aerial vehicles, as it directly affects navigation, flight control, task execution and other aspects. The changing environment factors during the operation of the unmanned aerial vehicle have a great influence on binocular cameras and laser radars. Single sensor positioning cannot guarantee positioning accuracy, and when the sensor is damaged, the unmanned aerial vehicle will lack pose information, and robustness cannot be guaranteed.

[0005] The T265 binocular camera estimates the camera motion by analyzing the pixel displacement between consecutive frames according to the visual odometry (VO) during operation, and then uses the IMU to judge its own attitude. The binocular camera and the IMU are fused to output the pose information of the unmanned aerial vehicle. However, the light changes have a greater impact on the camera, so there may be a large error in actual operation. The 2D laser radar determines its position by recognizing and tracking specific points or structural objects in the map, but it will produce a large error in the case of vibration and rapid movement. At the same time, the 2D laser radar only has two-dimensional information, lacking height information. The 3D laser radar is not only expensive, but also heavy, which is not very friendly to unmanned aerial vehicles. SUMMARY

[0006] In order to overcome the prior art, the present application provides a multi-sensor fusion positioning method based on a quadrotor unmanned aerial vehicle, which is based on time series prediction combined with a deep learning model LSTM for abnormal data prediction, and then uses a Kalman filtering method for pose information fusion. Specifically, first, the abnormal position is screened through certain judgment conditions, and then the historical data is added to the deep learning model for prediction. Then the output data is used as the input of Holt linear prediction for position data prediction. Finally, the output position information meeting the requirements is input into the Kalman filtering model, and the position information is fused by updating the covariance matrix, gain and state of Kalman filtering.

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

[0008] A multi-sensor fusion positioning method based on a quadrotor unmanned aerial vehicle, comprising the following steps:

[0009] 1) Establish a screening position information function, the process is as follows:

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

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

[0012] 1.3) Use the weighted average of the two sensor measurements ((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) Judge whether pos_t265[i] and pos_laser[i] are in the judgment area;

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

[0015] 1.6) If the condition of 1.4) is not met, the weighted average data of the measurement value at the current time is taken out, and the weighted average data of the measurement value is used as a constraint to predict the data that does not meet the requirements at the current time;

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

[0017] 2) Construct an LSTM prediction model, which effectively retains and forgets data by introducing a gate control mechanism, the main features of which are gate control mechanism and unit state update, wherein the gate control mechanism includes input gate, forget gate, and output gate;

[0018] 3) Establish Holt-Winters model:

[0019] 4) Establish Kalman filter, using Kalman filter model for further optimization of position data fusion.

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

[0021] 2.1) input gate, responsible for updating the current state, is a sigmoid layer to determine which values need to be updated, and a tanh layer to create 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] Where i t is the output of the input gate, σ is the sigmoid function, W i is 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] Where, 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) forget gate, used to determine what information to discard from the cell state,

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

[0028] Where f t represents the forget gate output 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, responsible for determining the next hidden state, which 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] where o 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) calculating a candidate value of the new cell state,

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

[0034] where g t is the candidate value of the cell state, W c is the weight matrix of the cell state, b c is the bias of the cell state;

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

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

[0037] where c t is the cell state of the previous time step,

[0038] 2.6) outputting,

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

[0040] where h t is the output of the current time step.

[0041] Further, in the 3), a 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 a data smoothing factor, 0 < α < 1; β is a trend smoothing factor, 0 < β < 1; S t is an update to the level value, B t is an update to the trend;

[0045] The prediction formula is,

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

[0047] where F t+m is a prediction for the next time, m is the length of time to be predicted.

[0048] Further, 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] where x k represents the true value at time k, ω k represents process noise, u k is the input value, F represents the state transition matrix, and B represents the control matrix;

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

[0053] where z k represents the observation value, v k is the observation noise, and H k represents the observation matrix;

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

[0055]

[0056] where, represents the estimate of x k at time k-1;

[0057]

[0058] where, represents the observation of x k at time k-1;

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

[0060]

[0061] where, represents the estimation error of k-1 at time k,

[0062]

[0063] where, represents the observation error of k-1 at time k;

[0064] 4.4) Establish the covariance matrix:

[0065]

[0066] where, k|k-1 represents the state error covariance matrix,

[0067]

[0068] where, K represents the observation error covariance matrix;

[0069] 4.5) Establish the Kalman filter gain:

[0070]

[0071] where K k represents the 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] where, P k|k is the updated state error covariance matrix;

[0075]

[0076] wherein, updated estimate.

[0077] The technical concept of the present application is as follows: first, the position of the sensor is acquired, then the position data of some abnormal positions is screened out using a position screening function, then the current time position data is predicted using a long short-term memory network and added to the data prediction queue, then the abnormal position data is predicted using the Holt-Winters model time series recursive prediction method of quadratic exponential smoothing. Finally, the position information meeting the requirements is fused using a Kalman filter algorithm, and a more accurate position and attitude information is output.

[0078] The present application has the beneficial effects that: combining deep learning model, Holt linear prediction and Kalman filter technology can greatly improve the accuracy and reliability of position information, especially in dynamic and complex environment, it can better handle abnormal data and uncertainty, and provide more accurate position information prediction and fusion results. BRIEF DESCRIPTION OF DRAWINGS

[0079] Fig. 1 is a trajectory graph of the abnormal output position and pose of the binocular camera data of the unmanned aerial vehicle when running;

[0080] Fig. 2 is a trajectory graph of the abnormal output position and pose of the laser radar data of the unmanned aerial vehicle when running;

[0081] Fig. 3 is a trajectory graph of the abnormal output position and pose of the unmanned aerial vehicle when running; DETAILED DESCRIPTION

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

[0083] Referring to Figs. 1-3 A multi-sensor fusion positioning method based on a quad-rotor unmanned aerial vehicle, comprising the following steps:

[0084] 1) Establish a screening position information function, the process is as follows:

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

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

[0087] 1.3) The weighted average of the two sensor measurements ((k*pos_t265[i]+(1-k)*pos_laser[i])-sum, (k*pos_t265[i]+(1-k)*pos_laser[i])-sum) is used as a judgment condition;

[0088] 1.4) Determine whether pos_t265[i], pos_laser[i] is in the judgment area;

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

[0090] 1.6) If the condition of 1.4) is not met, the weighted average data of the measurement value at the current time is taken out, and the weighted average data of the measurement value is used as a constraint to predict the data that does not meet the requirements at the current time;

[0091] 1.7) The estimated data that meets the requirements is taken out and added to the Kalman filter;

[0092] Where sum is the set screening threshold, pos_t265[i], pos_laser[i] respectively represent the storage binocular camera position information, laser radar position information, i = 0, 1, 2;

[0093] 2) Construct an LSTM prediction model, long short-term memory network (LSTM) is a special recursive neural network structure, which mainly preserves and forgets data through the introduction of gate control mechanism, its main features gate control mechanism and unit state update, where the gate control mechanism includes input gate, forget gate, and output gate, the process is as follows:

[0094] 2.1) Input gate, which is responsible for updating the current state, is a sigmoid layer to determine which values need to be updated, and a tanh layer to create a new candidate value vector, then this vector will be added to the state.

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

[0096] Where, i t is the output of the input gate, σ is the sigmoid function, W i is 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] where, 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) a forget gate for deciding what information to discard from the cell state,

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

[0101] where f t represents the forget gate output 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) an output gate responsible for determining the next hidden state, which contains information about the previous inputs, and is used for prediction,

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

[0104] where o 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) computing the candidate value for the new cell state,

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

[0107] where g t is the candidate value for the cell state, W c is the weight matrix of the cell state, b c is the bias of the cell state;

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

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

[0110] where c t is the cell state of the previous time step,

[0111] 2.6) output,

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

[0113] where h t is the output of the current time step.

[0114] In the 2), the input x t at the current time and the output h t-1 at the last time step are concatenated into a vector.

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

[0116] S t = ax t + (1 - a) (S t-1 + B t-1 ) (8)

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

[0118] where a is the data smoothing factor, 0 < a < 1; b is the trend smoothing factor, 0 < b < 1; S t is the update of the level value, B t is the update of the trend;

[0119] The prediction formula is,

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

[0121] where F t+m is the prediction for the next time, m is the length of time to be predicted;

[0122] 4) Establish Kalman filter, 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] Where, x k ω represents the true value at time k. k Indicates process noise, u k For input values, F represents the state transition 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 It is observation noise, H k This represents the observation matrix;

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

[0129]

[0130] in, This indicates the estimation of x at time k-1. k The estimate;

[0131]

[0132] in, This indicates the estimation of x at time k-1. k Observations;

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

[0134]

[0135] Where, x~ k|k-1 This represents the estimation error from time k to time k-1.

[0136]

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

[0138] 4.4) Establishing the covariance matrix:

[0139]

[0140] wherein, in the formula, P k|k-1 represents the state error covariance matrix,

[0141]

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

[0143] 4.5) Establishing the Kalman filter gain:

[0144]

[0145] wherein, K k represents the Kalman filter gain;

[0146] 4.6) Updating the estimate

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

[0148] wherein, P k|k is the updated state error covariance matrix;

[0149]

[0150] wherein, is the updated estimate;

[0151] In the 4), since the position and the attitude conform to the Gaussian distribution, the fusion of the position and the attitude information can be completed in the manner of the Kalman filter, and the Kalman filter is established.

[0152] In the updating of the estimate of the 4.6), the position and the attitude information obtained by optimization are added to the state equation, the Kalman filter is executed, the updated position and attitude information value is output, the position and the attitude information of the laser radar and the updated position and attitude information value obtained in the previous step are jointly added to the state equation, the Kalman filter is executed, the updated position and attitude information value is output, the position and the attitude information of the motion capture system and the position and the attitude information value output after the execution of the laser radar are jointly added to the state equation, the Kalman filter is executed, the updated position and attitude information value is output, and the final fused position and attitude information is obtained.

[0153] The embodiments of the present specification are merely illustrative of the implementation forms of the inventive concept, and are only used for the purpose of description. The protection scope of the present application should not be regarded as being limited to the specific forms shown in the embodiments, and the protection scope of the present application also includes the equivalent technical means that can be thought of by those skilled in the art according to the inventive concept.

Claims

1. A multi-sensor fusion positioning method based on a quadcopter unmanned aerial vehicle, characterized in that, The method comprises the following steps: 1) Establish a screening position information function, the process is as follows: 1.1) Take out the T265 binocular camera, 2D laser radar position data pos_t265[i], pos_laser[i] respectively; 1.2) Set the threshold sum and weight k of judgment; 1.3) Use the weighted average of the measurement values of the two sensors ((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) Judge whether pos_t265[i], pos_laser[i] is in the judgment area; 1.5) If the condition of 1.4) is met, the sensor data is taken as the screened data; 1.6) If the condition of 1.4) is not met, take the weighted average data of the measurement value at the current time, and use the weighted average data of the measurement value as a constraint to predict the data that does not meet the requirements at the current time; 1.7) Take out the estimated data that meets the requirements and add it to the Kalman filter; 2) Construct an LSTM prediction model, effectively retain and forget data by introducing a gate control mechanism, the main features of which are gate control mechanism and unit state update, wherein the gate control mechanism includes input gate, forget gate, and output gate; 3) Establish a Holt-Winters model: 4) Use the Kalman filter model to further optimize and fuse the position data.

2. The multi-sensor fusion positioning method based on quadcopters of claim 1, wherein, The process of 2) is as follows: 2.1) Input gate, which is responsible for updating the current state, is a sigmoid layer to determine which values need to be updated, and a tanh layer to create a new candidate value vector, which will be added to the state, i t = σ(W i · [h t-1 , x t ]+ b i ) (1) where i t is the output of the input gate, σ is the sigmoid function, W i is 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; wherein, is a vector of candidate values, W c is a weight of the vector of candidate values, b c is a bias of the vector of candidate values; 2.2) Forget gate, which is used to determine what information to discard from the unit state, f t = σ(W f · [h t-1 , x t ]+ b f ) (3) where f t represents the forget gate output 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, which contains information about the previous input and is used for prediction, o t = σ(W o · [h t-1 , x t ]+b o ) (4) wherein o 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 the candidate value of the new unit state, g t = tanh(W c [h t-1 ,x t ]+ b c ) (5) where g t is a candidate value for the cell state, W c is a weight matrix for the cell state, b c is a bias for the cell state; 2.5) Update the unit state according to the forget gate and input gate, c t = f t · c t-1 + i t · g t (6) where c t is the cell state of the previous time step, 2.6) Output, h t = o t • tanh c t (7) where h t is the output of the current time step.

3. The multi-sensor fusion positioning method based on quadcopters of claim 2, wherein, In 3), a 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) Wherein alpha is a data smoothing factor, 0 < alpha < 1; β is a trend smoothing factor, 0 < β < 1 ; S t is an update to the level value, B t is an update to the trend; The prediction formula is, F t+m = S t + mB t (10) where F t+m is the prediction for the next time instant, and m is the length of time for which the prediction is required.

4. The multi-sensor fusion positioning method based on quadcopters of claim 3, wherein, 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) where x k represents the true value at time k, ω k represents the process noise, u k is the input value, F k represents the state transition matrix, B k represents the control matrix; z k = h(x k , v k ) = H k (x k ) + v k (12) where z k represents an observation value, v k is an observation noise, H k represents an observation matrix; 4.2) Establish the state estimation equation and observation estimation equation: wherein, represents an estimate of x k at time k - 1. wherein, represents an estimate of the observation of x k at time k-1. 4.3) Establish the state estimation error and observation estimation error: wherein in the formula denotes the estimation error of the kth instant with respect to the k-1th instant, wherein, represents the observation error at time k for time k-1. 4.4) Establish the covariance matrix: where P k|k-1 denotes the state error covariance matrix, where S K denotes the observation error covariance matrix; 4.5) Establish the Kalman filter gain: where K k denotes the Kalman filter gain; 4.6) Update the estimate P k|k = P k|k-1 - K k H k P k|k-1 (20) P = P - Kx (x - x) (5) k|k is the updated state error covariance matrix; wherein is the updated estimate.

Images