A positioning method combining deep network and filtering

Abstract

The present invention discloses a positioning method combining a deep network with filtering, step one: recommending a visual inertial odometer kinematic model; step two: inputting a temporally adjacent k+1 frame image video sequence into a stacking network, combining the images of adjacent frames to obtain k groups of image pairs; step three: after the k groups of image pairs are input into a CNN-LSTM-ATT-VO network, 6-dof pose estimates Δx, Δy, Δz, #imgabs0#Δλ, Δψ are output, wherein Δx, Δy, Δz are respectively the translation amounts of the x-axis, y-axis and z-axis, and #imgabs1#Δλ, Δψ are respectively the rotation amounts around the x-axis, y-axis and z-axis; the network sequentially includes a convolutional neural network, a CBAM module, an LSTM network and a fully connected layer; step four: performing fusion positioning of the visual inertial odometer by a robot-centric Kalman filtering algorithm based on Lie groups. The present invention improves the robustness of the system and improves the accuracy of collaborative positioning, avoiding the inconsistency of the traditional EKF algorithm.

Description

Technical Field

[0001] The present invention belongs to the field of visual inertial sensor (VIO) navigation, and relates to a positioning method combining a deep network with filtering, and in particular to a positioning method combining a deep network with filtering based on a visual inertial odometer. Background Art

[0002] The VO pose estimation part in the traditional visual-inertial fusion framework is basically manually mathematically modeled and parameter adjustment is performed according to the time situation. In many complex situations, especially fast motion and when the external image information cannot find feature points, it is easy to fail in the feature matching link. The VO trained by deep learning neural network has the ability of parameter learning and nonlinear model fitting. In essence, it is modeled according to the relationship between the input scene image and the navigation parameters, and the parameters of the entire network model are optimized through data training. At the same time, since the complexity of IMU information is low-dimensional compared with the complexity of image information, the deep learning VO is combined with the classic framework of IMU for combined navigation by using a loose coupling method based on filtering, which can not only reduce the dimension of the state vector, but also enhance the robustness of the system. Through the study of deep learning-based VO, pose estimation is completed between adjacent frame images through an end-to-end learning method. In terms of visual-inertial fusion, the traditional filtering algorithm has more practical physical significance. Therefore, the pose information estimated by the deep learning VO is fused and improved with the physically modeled IMU model through the classic traditional filtering algorithm, so that the overall framework of the system has the potential to combine the advantages of both worlds. Summary of the invention

[0003] In view of the above-mentioned prior art, the technical problem to be solved by the present invention is to provide a positioning method based on a deep network combined with filtering based on visual inertial odometer, which adopts a robot-centric filtering algorithm (R-EKF) based on Lie group to avoid the inconsistency of the traditional EKF algorithm. The measurement update is completed by establishing a state model of the entire system and accepting measurement information from the deep network CNN-LSTM-ATT-VO network.

[0004] In order to solve the above technical problems, a positioning method combining a deep network and filtering is provided in the present invention, comprising:

[0005] Step 1: Propose a kinematic model for visual inertial odometry;

[0006] Step 2: Input the temporally adjacent k+1 frame image video sequence into the stacking network, and combine the images of adjacent frames to obtain k groups of image pairs;

[0007] Step 3: k sets of image pairs are input into the CNN-LSTM-ATT-VO network and the 6-dof pose estimates Δx, Δy, Δz are output. Δλ, Δψ, where Δx, Δy, Δz are the x-axis, y-axis, and z-axis translations, respectively. Δλ, Δψ are the rotation amounts around the x-axis, y-axis and z-axis respectively; the network includes a convolutional neural network, a CBAM module, an LSTM network and a fully connected layer in sequence;

[0008] Step 4: Perform visual-inertial odometer fusion positioning through a robot-centric Kalman filtering algorithm based on Lie groups.

[0009] Furthermore, step one is specifically as follows:

[0010] The state equation of the IMU system is:

[0011]

[0012]

[0013] Among them, a m is the measured accelerometer value, ω m is the gyroscope measurement value, which is defined based on the carrier B system at time τ and is affected by the noise n a and n ω 、Random walk b a and b ω The random noise on the accelerometer and gyroscope and the derivative of the random walk on the deviation are zero-mean Gaussian points, i is the inertial coordinate system, and the local reference coordinate system r is defined k , r k is the reference system at time k, where time k, k+1, ... corresponds to the time point of receiving the image, and the carrier coordinate system is recorded as B system, B τ is the load system at time τ, where τ, τ+1, ... correspond to the time of receiving IMU measurements between time k and k+1; is the rotation matrix of the local reference coordinate system at time k relative to the carrier system at time τ, is the gravity vector, It is the acceleration of the carrier system relative to the inertial system at time τ expressed on the carrier system;

[0014] When the inertial state remains constant, the standard state of the carrier evolves in continuous time as follows:

[0015]

[0016] Furthermore, the convolutional neural network extracts geometric relationship features from image pairs and outputs a sequence of tensors representing the geometric relationship between adjacent frames.

[0017] Furthermore, the CBAM module operates on the feature map as follows:

[0018] F′=σ(MLP(AP(F))+MLP(MP(F)))⊙F

[0019] F″=σ(f 7×7 [AP(F′), MP(F′)])⊙F′

[0020] represents the input feature map, represents the one-dimensional feature map after channel attention, It represents the final feature map after the spatial attention mechanism, σ represents the sigmoid function, MLP represents the FC layer, MP represents the maximum pooling, and AP represents the average pooling.

[0021] Furthermore, the LSTM network contains two layers of LSTM, each LSTM contains 1024 hidden units, and the default activation functions of the three gates in the LSTM are Relu functions.

[0022] Furthermore, the fully connected layer contains 128 hidden units and is followed by a Relu activation function to reduce the dimensionality of the received features.

[0023] Furthermore, step four is specifically as follows:

[0024] Define the status as: Inertial state and carrier status Specifically:

[0025]

[0026]

[0027] For the inertial state, It includes the attitude of system I at time k in reference system r, where is the rotation matrix of system I relative to the reference system r at time k, It is the expression of the translation of system I relative to system r at time k in the reference system. is the gravity vector expressed by the r system at time k; for the carrier state, Including the reference of the B system at time k is the posture in the r system, are the carrier velocity, gyroscope bias and accelerometer bias in the carrier system B system respectively;

[0028] The error state vector of the standard state is: The perturbation of the rotational state is defined as: where C∈SO(3), (·)^ is the antisymmetric operator;

[0029] Among them, the two error state quantities are expressed as follows:

[0030]

[0031]

[0032] The continuous-time error state equation is obtained:

[0033]

[0034] Where F and G are the linearized Jacobian matrices of the system state and error, respectively, n is the noise and

[0035] The system standard state and error state of the continuous model are discretized to obtain:

[0036]

[0037]

[0038]

[0039] in, is the gyroscope bias, is the acceleration bias, is the rotation matrix of system B relative to the reference system r at time k, is the velocity of system B relative to system I at time k expressed in reference system r, Δt = t k+1 -t k , the subscript s indicates a fixed value;

[0040] The state transition matrix Φ from time τ to time τ+1 τ+1，τ To make a first-order expression:

[0041]

[0042] In the formula, δt = t τ+1 -t τ ;

[0043] The covariance matrix of the intermediate state is obtained as:

[0044]

[0045] In the formula, the noise is assumed to be the random noise on the accelerometer and gyroscope, and the random walk derivative on the bias is assumed to be zero-mean Gaussian white noise Q, specifically:

[0046]

[0047] Observable The relative pose measurement output by the deep learning CNN-LSTM-ATT-VO network, the corresponding covariance matrix R is simplified to a diagonal matrix, and the error of the measurement information is It is expressed as:

[0048]

[0049] in, They represent the estimated rotation and translation of the carrier system at time K+1 relative to the reference system at time k;

[0050] The first-order Baker-Campbell-Hausdorff (BCH) formula is applied to approximate the logarithmic mapping as the subtraction of two rotation vectors:

[0051]

[0052] The Jacobian matrix of the measurement is found by the differentiation process of the above formula Measurement matrix H k+1 The specific form is:

[0053]

[0054] Perform a regular EKF update and get the error Estimated value:

[0055]

[0056]

[0057]

[0058] Then, Injected into the predicted standard state to obtain

[0059] Finally, the reference frames of all states are changed from frame r k Move forward to frame r k+1 , get the state quantity of the reference frame k+1

[0060]

[0061]

[0062]

[0063]

[0064]

[0065] In the above formula, is the rotation of the carrier system at time k+1 relative to the reference system at time k, as well as is the rotation and translation of the carrier system relative to the reference system at time k+1; after the error compensation, the attitude at the current moment is obtained, and the next EKF iteration is performed.

[0066] Beneficial effects of the invention: The present invention relates to a method for completing visual-inertial fusion positioning by combining an end-to-end deep network with a robocentric Kalman filter algorithm (R-EKF) based on Lie groups, thereby improving the robustness of the system and the accuracy of collaborative positioning. The present invention proposes a robot-centric filtering algorithm based on Lie groups for fusing the pose estimation of VO estimated by a deep network with that of an IMU. The algorithm uses the known kinematic model state information of the IMU and uses the estimation of the deep VO network as a measurement update.

[0067] 1. Compared with the traditional visual odometer calculation method, the deep learning algorithm has stronger robustness and applicability in textureless environments.

[0068] 2. Add the attention mechanism to the deep learning network and turn it into a CNN-LSTM-ATT-VO network. The attention mechanism is used to optimize the geometric relationship feature map extracted by the convolution layer to improve the positioning accuracy.

[0069] 3. Through deep learning combined with a fusion algorithm based on improved traditional filtering, the posture information estimated by deep learning VO and the physically modeled IMU model are fused and improved through the classic traditional filtering algorithm so that the overall framework of the system combines the advantages of both algorithms. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is the robot-centered vio reference frame;

[0071] Figure 2 This is the CNN-LSTM-VO network structure diagram;

[0072] Figure 3 This is the CNN-LSTM-ATT-VO network structure diagram;

[0073] Figure 4 The CBAM process

[0074] Figure 5 is the system overview for a given time step from k to k+1;

[0075] Figure 6 This is the trajectory diagram of KITTI sequence 10;

[0076] Figure 7 It is the displacement diagram in the three directions of XYZ;

[0077] Figure 8 is the absolute trajectory error map;

[0078] Fig. 9 It is the comparison of the root mean square error, median error, mean error, and standard deviation of the trajectory. DETAILED DESCRIPTION

[0079] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0080] Step 1: Define the visual inertial odometry coordinate system and establish the kinematic model;

[0081] The goal of the visual inertial odometer pose estimator designed in the present invention is to estimate the pose information of the carrier relative to the inertial reference system. The inertial coordinate system is defined as lowercase i for convenience and is fixed on the earth. The local reference coordinate system r is defined here according to the robot-centric filtering algorithm k is the reference frame at time k, see Figure 1 , where times k; k+1; ... correspond to the time points at which the image is received. The B frame is the vehicle coordinate system, which is the vehicle frame at time τ, where τ, τ+1, ... correspond to the times at which IMU measurements are received between time k and k+1. Superscripts and subscripts are used to track the coordinate frame of physical quantities. For example, is the velocity of system B relative to system C, expressed in terms of system A, C ab is the rotation matrix of system B relative to system A. Superscripts are also used to distinguish different types of quantities. For example: Used to indicate the amount of noise, It is used to represent the propagation amount in the whole EKF prediction process. The correction amount after EKF update is represented by stem.

[0082] The modeling of the IMU system state equation is expressed as follows according to the requirements: τ is the load system at time τ, is the rotation matrix of the local reference coordinate system at time k relative to the carrier system at time τ, It is the acceleration of the load system relative to the inertial system at time τ expressed on the load system. The measured accelerometer value a m and the gyro measurement value ω m is defined based on the carrier B system at time τ, and they are respectively affected by the noise n a and n ω 、Random walk b a and b ω The accelerometer measurements also incorporate a fixed gravity vector And rotated to the B system through the rotation matrix, assuming that the random noise on the accelerometer and gyroscope and the derivative of the random walk on the deviation are zero-mean Gaussian distribution.

[0083]

[0084]

[0085] When the inertial state remains constant, the standard state of the carrier evolves in continuous time as shown in formula (2):

[0086]

[0087] Step 2: Use deep learning to build a VO network based on visual odometry for pose estimation;

[0088] Based on the existing convolutional network, the present invention first designs a CNN-LSTM-VO network for extracting the geometric feature relationship between images. The pose estimation is performed based on the information of a series of video sequences input by VO, that is, it is no longer just to estimate the relative pose between two pictures, but to continuously output the relative pose estimation between the position and pose of multiple frames of images with time sequence. The network structure is as follows Figure 2 As shown in the figure, the input is the image of adjacent frames in time sequence, which enters the CNN-LSTM-VO network through the stacking network. In order to adapt to the network structure, the input image is preprocessed in advance. Due to the inconsistency of image sizes in different scenes, the image size is uniformly adjusted to 1280×384 to better adjust the network structure. The preprocessed image is extracted by the convolution part of the convolution network to extract the motion relationship features between adjacent image frames. Assuming that the length of the input video sequence is k+1, after stacking, the images of adjacent frames are combined to obtain k groups of image pairs, which are passed through the convolution network for feature extraction to obtain a tensor representing the geometric relationship between adjacent frames, and the tensor sequence is input into a two-layer LSTM network through an FC layer to stabilize the features and achieve the purpose of dimensionality reduction. Finally, a 6-DOF relative pose estimation is output.

[0089] (1) Convolutional Network

[0090] In order to design a more effective CNN architecture to learn the geometric relationship features between adjacent frame images, the present invention draws on the Flownet optical flow network structure proposed by Dosoviskiy, and also refers to DeepVO. In the CNN architecture part of the network structure, the present invention presets ten layers of Conv layers in the convolution layer when designing CNN-VO. There is a Relu function behind each layer of convolution. At the same time, as the network goes deeper, the feature map becomes more abstract. Finally, the features extracted by the convolution layer are input into the fully connected layer after a maximum pooling to obtain the pose estimation.

[0091] Table 1 Convolutional layer related parameter settings

[0092]

[0093]

[0094] (2) LSTM Network

[0095] The main purpose of considering adding two layers of LSTM in the present invention is to enable the entire deep network to autonomously learn the intrinsic connection between the postures of multiple consecutive adjacent frames through data. The LSTM layer can not only learn the state in the historical data (i.e., the posture relationship between the previous multiple frame images), but also achieve the geometric constraint effect between multiple frame images in the traditional algorithm when estimating the posture at the current moment; and the LSTM network will not produce the problem of gradient disappearance in the process of long sequence data learning due to the particularity of its gate structure, and can continuously maintain the learning effect of the network. The first layer of LSTM is used to receive the output feature data of CNN, and the second layer is used to receive the output of the first layer and pass the information to the fully connected layer to output the result of posture estimation. Each LSTM contains 1024 hidden units. According to the actual needs of the present invention, the default activation function of the three gates in LSTM is changed from Sigmoid function to Relu function for more advanced posture information representation.

[0096] (3) Fully connected layer

[0097] In the present invention, the purpose of the fully connected layer is mainly to strengthen the geometric relationship feature information and reduce the dimension of the feature. The output from the LSTM layer is a high-dimensional feature representation, so the feature information needs to be output to the fully connected layer for dimensionality reduction. In the present invention, only one FC layer is designed at the end of the entire network, which contains 128 hidden units and a Relu activation function is added thereto. Finally, the feature information output after dimensionality reduction is mapped to a 6-dof pose estimation representation.

[0098] Δx, Δy, Δz, Δλ,Δψ

[0099] Among them, Δx, Δy, Δz are the translation amounts, Δλ and Δψ are the rotation amounts. Finally, the loss function is designed and optimized for the output 6-DOF pose information.

[0100] (4) Loss function design and optimization

[0101] According to the input time sequence video information, in the present invention, assuming that there are n+1 sets of pictures input, the network input data is X=(X 1 , X 2, ..., X n+1 ), and the output pose estimation result is Y = (Y 1 , Y 2 , ..., Y n ), so when the input data is a video sequence, the probability of obtaining the pose estimation result is as follows (3):

[0102] P(Y|X)=P(Y 1 , Y 2 , ..., Y n |X 1 , X 2 , ..., X n+1 ) (3)

[0103] Therefore, the problem that needs to be solved for the entire network is to find the optimal parameter w * To maximize the probability of the above formula, it is as shown in the following formula (4):

[0104]

[0105] Where w is the parameter to be learned. In order to find the optimal parameter w for the VO pose estimation problem * , the loss function is designed to minimize the mean square error MSE between the network estimated pose and the true trajectory as shown in formula (5):

[0106]

[0107] in represents the position and attitude of the true trajectory, β is a weighted balance parameter used to balance the relationship between position and attitude, ||·|| 2 is the 2-norm.

[0108] In order to better adapt to the deep network pose estimation problem of VO, we select the spatial self-attention that focuses on the image area features, the SENet that focuses on learning feature channels to improve the ability to express features, and the CBMA that focuses on both spatial and channel feature learning from the existing methods. After verification by some papers, the resnet network is simply used in combination with the two to perform attention annotation. The experimental results of early paper research show that the recognition rate of ResNet is significantly improved after adding attention, and the recognition rate of CBAM has better performance in image feature recognition than SENet. Therefore, based on better extraction of the geometric relationship between the features of adjacent frame images, the present invention decides to add the CBAM attention mechanism on the basis of the original network to obtain a more accurate feature map and thus obtain a more accurate pose estimation result. The network structure is as follows Figure 4 As shown:

[0109] The main principle of CBAM is to perform operations on the feature map as shown in equations (6) and (7):

[0110] F=σ(MLP(AP(F))+MLP(MP(F)))⊙F (6)

[0111] F″=σ(f 7×7 [AP(F′),MP(F′)])⊙F′ (7)

[0112] CBAM’s processing of image features. The channel attention mechanism is the processing of formula (6), and the spatial attention mechanism is the processing of formula (7). represents the input feature map, represents the one-dimensional feature map after channel attention, The final feature map after the spatial attention mechanism. σ represents the sigmoid function, MLP is the FC layer, MP represents the maximum pooling, and AP represents the average pooling, both of which are designed to reduce the number of parameters.

[0113] The CBAM mechanism is added to CNN-LSTM-VO. After adding the attention mechanism, the network itself does not have a major change in the overall structure. It mainly optimizes the features of the output of each conv layer so that the feature map predicted by the entire CNN network is more consistent with the actual accuracy of subsequent pose estimation.

[0114] Step 3: Use the robot-centric Kalman filter algorithm (R-EKF algorithm) based on Lie groups to perform fusion positioning of visual inertial odometry and avoid the traditional Kalman inconsistency problem.

[0115] The core of the estimator is the proposed robot-centric EKF formulation, which defines all its states relative to a local reference frame by fusing visual and inertial information. This includes inertial states that are tracked as “features” relative to the latest reference state. During the EKF prediction, the vehicle state is passed in the local reference frame. After performing the EKF update with the arrival of each new image, the robot-centric EKF moves the reference frame from one time step to the next in the composition step. The process is then repeated in the new reference frame. The three-step process of prediction, update, and composition is shown in Figure 5 Shown on the right.

[0116] The robot-centric Lie group-based Error-State Extended Kalman Filter (R-EKF) formulation employs a robot-centric Visual Inertial Odometry (VIO). Unlike other common formulations that have states relative to an Earth-referenced inertial frame, the states of the R-EKF are relative to a reference frame that moves with the robot. This formulation allows us to fuse measurements that are relative in nature, rather than using absolute measurements referenced in an inertial frame. The R-EKF is also an Error-State EKF. The Error-State EKF separates the states into standard states and error states. The standard state tracks the dynamics of the global data. The error state tracks the errors caused by noise, which are formulated as perturbations to the standard state. This means that the error state always runs around 0, which reduces the errors far from any singularities and reduces errors associated with linearization and discretization.

[0117] The definition state is shown in (8), which is divided into inertia state and carrier status For inertial state It includes the attitude of system I at time k in reference system r, where is the rotation matrix of system I relative to the reference system r at time k, It is the expression of the translation of system I relative to system r at time k in the reference system. is the gravity vector expressed by the r system at time k. For the carrier state, Including the reference of the B system at time k is the posture in the r system, are assumed to be the carrier velocity, gyroscope bias and accelerometer bias in the carrier system B, respectively.

[0118]

[0119] The two state quantities are expressed as shown in formula (9):

[0120]

[0121] The error state vector corresponding to the standard state is shown in equation (10). The error state is defined as the perturbation to the nominal state. Except for rotation, all others are defined in Euclidean space. The perturbation of the rotation state is defined as equation (11), where C∈SO(3), And (·)^ is an antisymmetric operator.

[0122]

[0123]

[0124] The two error state quantities are expressed as follows:

[0125]

[0126] Substituting the defined error disturbance into (11) and (12) to solve the error state, after first-order Taylor expansion and removing all second-order terms, the continuous-time error state equation can be obtained as shown in equation (13):

[0127]

[0128] Where F and G are the linearized Jacobian matrices of the system state and error, respectively, n is the noise and

[0129]

[0130] in is the gyroscope bias, is the acceleration bias, is the rotation matrix of system B relative to the reference system r at time k, It is the expression of the velocity of system B relative to system I at time k in the reference system r at time k.

[0131] In order to make the R-EKF prediction, the above formula (13) is displayed to obtain the system standard state quantity and error state of the continuous model. The discretization can be obtained as shown in formula (14), where Δt = t k+1 -t k , the subscript s indicates a constant value.

[0132] According to the general steps of EKF, the state transfer matrix is ​​obtained. R-EKF converts the state transfer matrix Φ from τ to τ+1 τ+1，τ The first-order expression is as follows (15), where δt = t τ+1 -t τ .

[0133]

[0134] Then we calculate the covariance matrix of the intermediate state based on the state transfer matrix in the above formula as shown in formula (16), where we assume the noise is random noise on the accelerometer and gyroscope, and the random walk derivative on the bias is assumed to be zero-mean Gaussian white noise Q as shown in formula (17).

[0135]

[0136]

[0137] Measurement model:

[0138] Observable The relative pose measurement value output from the deep learning CNN-LSTM-ATT-VO network, and its corresponding covariance matrix R, assumes that the measurement noise between the 6-DOF outputs of deep learning is uncorrelated and can be simplified to a diagonal matrix, which converts the error of the measurement information into It is expressed as shown in the following formula (18):

[0139]

[0140] in They respectively represent the estimated rotation and translation of the carrier system at time K+1 relative to the reference system at time k.

[0141] Because φ=ln(C) is not differentiable when φ→π. To avoid this problem, the first-order Baker-Campbell-Hausdorff (BCH) formula shown in formula (19) is applied in the present invention, which approximates the logarithmic mapping as the subtraction of two rotation vectors. This approximation is effective because both rotations are extremely small.

[0142]

[0143] Through the differentiation process of the above formula, we can find the Jacobian matrix of the measurement The derivation process is shown in equations (20) and (21). The measurement matrix H k+1 The specific form of is shown in formula (22).

[0144]

[0145]

[0146]

[0147] Measurement Update:

[0148] According to equation (23), the conventional EKF is updated and the error is obtained The estimated value of .

[0149]

[0150] Then, according to the perturbation defined above, Injected into the predicted standard state, we get

[0151] Finally, the reference frames of all states are changed from frame r k Move forward to frame r k+1 , get the state quantity of the reference frame k+1 As shown in Equation 24. After the current attitude is obtained after error compensation, the next EKF iteration can be performed.

[0152]

[0153] In the above formula is the rotation of the carrier system at time k+1 relative to the reference system at time k, as well as is the rotation and translation of the carrier system relative to the reference system at time k+1.

[0154] At the same time, the state covariance matrix must also be propagated to the next moment to reflect the operation in the above formula.

[0155] To further illustrate the beneficial effects of the present invention, the following simulation comparison verification was completed. Sequence 10 in the KITTI dataset was used to verify the algorithm. The frequency of the IMU was 10HZ and the frequency of the camera was 100HZ. The deep network training data was trained using sequences 00-06. The algorithm obtained the pose information of these sequences. The following are the trajectories drawn by the EVO evaluation tool for these sequences, and compared with the traditional visual odometer monocular algorithm VINS. Figure 6 , Figure 7 It can be seen that the positioning algorithm proposed in the present invention can accurately estimate the position information of the carrier. Figure 8 , Fig. 9 The absolute trajectory error APE between the trajectory estimated by the R-EKF algorithm proposed by the present invention and the real estimation is obtained, and the root mean square error, median error, average error, and standard deviation of the trajectory are calculated respectively, and compared with the monocular VINS algorithm. It can be seen that the positioning accuracy of the present invention is higher, so the method provided by the present invention can improve the positioning accuracy higher than the traditional monocular visual inertial mileage calculation method.

Claims

1. A positioning method combining a deep network and filtering, characterized in that: include: Step 1: Establish the kinematic model of visual inertial odometry; Step 2: Input the temporally adjacent k+1 frame image video sequence into the stacking network, and combine the images of adjacent frames to obtain k groups of image pairs; Step 3: k sets of image pairs are input into the CNN-LSTM-ATT-VO network and the 6-dof pose estimates Δx, Δy, Δz are output. Δλ, Δψ, where Δx, Δy, Δz are the x-axis, y-axis, and z-axis translations, respectively. Δλ, Δψ are the rotation amounts around the x-axis, y-axis and z-axis respectively; the network includes a convolutional neural network, a CBAM module, an LSTM network and a fully connected layer in sequence; Step 4: Perform visual-inertial odometer fusion positioning through a robot-centric Kalman filtering algorithm based on Lie groups.

2. According to claim 1, a positioning method combining a deep network and filtering is characterized in that: Step 1 is as follows: The state equation of the IMU system is: Among them, a m is the measured accelerometer value, ω m is the gyroscope measurement value, which is defined based on the carrier B system at time τ and is affected by the noise n a and n ω 、Random walk b a and b ω The random noise on the accelerometer and gyroscope and the derivative of the random walk on the deviation are zero-mean Gaussian points, i is the inertial coordinate system, and the local reference coordinate system r is defined k , r k is the reference system at time k, where time k, k+1, ... corresponds to the time point of receiving the image, and the carrier coordinate system is recorded as B system, B τ is the load system at time τ, where τ, τ+1, ... correspond to the time of receiving IMU measurements between time k and k+1; is the rotation matrix of the local reference coordinate system at time k relative to the carrier system at time τ, is the gravity vector, It is the acceleration of the carrier system relative to the inertial system at time τ expressed on the carrier system; When the inertial state remains constant, the standard state of the carrier evolves in continuous time as follows:

3. According to claim 1, a positioning method combining a deep network and filtering is characterized in that: The convolutional neural network extracts geometric relationship features from image pairs and outputs a tensor sequence representing the geometric relationship between adjacent frames.

4. The positioning method combining a deep network and filtering according to claim 1, characterized in that: The CBAM module operates on the feature map as follows: F′=σ(MLP(AP(F))+MLP(MP(F)))⊙F F″=σ(f 7×7 [AP(F′),MP(F)])⊙F′ represents the input feature map, represents the one-dimensional feature map after channel attention, It represents the final feature map after the spatial attention mechanism, σ represents the sigmoid function, MLP represents the FC layer, MP represents the maximum pooling, and AP represents the average pooling.

5. The positioning method combining a deep network and filtering according to claim 1, characterized in that: The LSTM network includes two layers of LSTM, each LSTM contains 1024 hidden units, and the default activation functions of the three gates in the LSTM are Relu functions.

6. The positioning method combining a deep network and filtering according to claim 1, characterized in that: The fully connected layer contains 128 hidden units and is followed by a Relu activation function to reduce the dimension of the received features.

7. The positioning method combining a deep network and filtering according to claim 1, characterized in that: Step 4 is as follows: Define the status as: Inertial state and carrier status Specifically: For the inertial state, It includes the attitude of system I at time k in reference system r, where is the rotation matrix of system I relative to the reference system r at time k, It is the expression of the translation of system I relative to system r at time k in the reference system. is the gravity vector expressed in the r system at time k; For the carrier state, Including the reference of the B system at time k is the posture in the r system, are the carrier velocity, gyroscope bias and accelerometer bias in the carrier system B system respectively; The error state vector of the standard state is: The perturbation of the rotational state is defined as: where C∈SO(3), (·)^ is the antisymmetric operator; Among them, the two error state quantities are expressed as follows: The continuous-time error state equation is obtained: Where F and G are the linearized Jacobian matrices of the system state and error, respectively, n is the noise and The system standard state and error state of the continuous model are discretized to obtain: in, is the gyroscope bias, is the acceleration bias, is the rotation matrix of system B relative to the reference system r at time k, is the velocity of system B relative to system I at time k expressed in reference system r, Δt = t k+1 -t k , the subscript s indicates a fixed value; The state transition matrix Φ from time τ to time τ+1 τ+1,τ Perform a first-order expression: In the formula, δt = t τ+1 -t τ ; The covariance matrix of the intermediate state is obtained as: In the formula, the noise is assumed to be the random noise on the accelerometer and gyroscope, and the random walk derivative on the bias is assumed to be zero-mean Gaussian white noise Q, specifically: Observable The relative pose measurement output by the deep learning CNN-LSTM-ATT-VO network, the corresponding covariance matrix R is simplified to a diagonal matrix, and the error of the measurement information is It is expressed as: in, They represent the estimated rotation and translation of the carrier system at time K+1 relative to the reference system at time k; The first-order Baker-Campbell-Hausdorff (BCH) formula is applied to approximate the logarithmic mapping as the subtraction of two rotation vectors: The Jacobian matrix of the measurement is found by the differentiation process of the above formula Measurement matrix H k+1 The specific form is: Perform a regular EKF update and get the error Estimated value: Then, Injected into the predicted standard state to obtain Finally, the reference frames of all states are changed from frame r k Move forward to frame r k+1 , get the state quantity of reference frame k+1 In the above formula, is the rotation of the carrier system at time k+1 relative to the reference system at time k, as well as is the rotation and translation of the carrier system relative to the reference system at time k+1; after the attitude at the current moment is obtained after error compensation, the next EKF iteration is performed.

Images