A consistent and efficient map-based filtering method for visual-inertial localization

Abstract

The present invention discloses a consistent and efficient filtering algorithm based on a map for visual inertial positioning, which is implemented by the following system. The system includes three modules: a local odometer module, a map feature matching module and a map-based positioning module. The local odometer is used to receive data from a camera and an IMU; the map feature matching is used to detect the similarity between the scene observed by the camera at the current moment and the pre-built map scene, and obtain the feature matching pair of the image feature at the current moment and the map feature; the map-based positioning module is used to receive the output of the local odometer and the feature matching pair. The present invention represents the maintained odometer-related variables and the relative transformation between the local odometer reference system and the pre-built map reference system on the Lie group, thereby obtaining a new invariant Kalman filtering algorithm, which ensures that the algorithm can maintain the correct observability of the system without considering the map uncertainty.

Description

Technical Field

[0001] The invention relates to a robot positioning technology, in particular to a map-based consistent and efficient filtering method for visual inertial positioning. Background Art

[0002] Positioning technology is an essential basic module for intelligent robots. In recent years, cameras and inertial measurement units (IMUs) have been widely used in positioning technology, giving rise to a large number of excellent visual inertial odometry (VIO) methods. However, the positioning accuracy of VIO will inevitably drift as the method runs over time. This requires the introduction of pre-built maps to eliminate the accumulated errors. When the positioning method integrates the pre-built map information, the system needs to ensure consistent and effective integration of the map information in order to effectively use the map information to eliminate the accumulated errors of the odometer.

[0003] At present, the framework of positioning methods can be divided into two categories. One is to initialize the relative transformation between the local odometer reference system and the pre-built map reference system, and then perform real-time positioning in the map reference system; the other is to simultaneously maintain the local odometer and the relative transformation between the local odometer reference system and the pre-built map reference system, thereby indirectly obtaining the position in the map reference system. The limitation of the first type of method is that for visual inertial positioning, the pre-built map must be in the inertial system, which limits the application of the positioning method. For the second type of method, although there is no limitation of the first type of method, there is a potential problem with this type of framework - it may destroy the observability of the system, making originally unobservable information observable, thereby introducing false observation information, reducing the positioning accuracy and even causing the method to diverge.

[0004] In addition, most algorithms do not consider the uncertainty of maps due to computational efficiency or algorithm framework limitations, and believe that maps are absolutely accurate. This will cause the positioning algorithm to over-rely on inaccurate maps, greatly reducing the positioning effect. Even if some algorithms consider the uncertainty of maps in the above framework, they destroy the observability of the system, making originally unobservable information observable, thereby introducing false observation information, reducing positioning accuracy, and even causing the algorithm to diverge.

[0005] How to ensure the correct observability of the system while taking into account the uncertainty of the map, thereby improving the accuracy and reliability of the map-based positioning algorithm, is a problem that needs to be solved. At the same time, it is also necessary to ensure that the system's computational complexity remains at a low level. Summary of the invention

[0006] In order to overcome the shortcomings of the prior art, the present invention proposes a filtering-based positioning algorithm, a visual inertial positioning technology based on a pre-built visual map. The proposed visual inertial positioning technology is implemented based on a consistent and efficient filtering algorithm. The present invention is implemented through the following technical solutions:

[0007] The invention discloses a consistent and efficient filtering algorithm based on a map for visual inertial positioning, which is characterized in that the algorithm is implemented by the following system, which includes three modules: a local odometer module, a map feature matching module and a map-based positioning module. The local odometer is used to receive data from a camera and an IMU, obtain the state of the system in a local reference system in real time, and obtain the value of the corresponding state variable and its covariance; the map feature matching is used to detect the similarity between the scene observed by the camera at the current moment and the map scene built in advance, and obtain the feature matching pair of the image feature at the current moment and the map feature; the map-based positioning module is used to receive the output of the local odometer and the feature matching pair, obtain the updated local odometer state and the relative transformation (augmented variable) between the local reference system and the map reference system, and then calculate the state of the robot in the map reference system.

[0008] As a further improvement, the local odometer module described in the present invention includes an IMU, a state propagation module connected to the IMU signal, a camera, a feature tracking module connected to the camera signal, and an MSOC-S-IKF state update module based on local feature observation connected to the state propagation module and the feature tracking module signal.

[0009] As a further improvement, the IMU described in the present invention is used to provide the system with real-time rotational angular velocity and linear acceleration; the state propagation module is used to receive the rotational angular velocity and linear acceleration provided by the IMU, and use these quantities to propagate the state of the system from the previous moment to the current moment, and obtain the state variables and covariance predicted at the current moment; the obtained state variables and covariance signals are transmitted to the MSOC-S-IKF state update module based on local feature observation. The feature tracking module is used to track the position of the feature points in the image at the previous moment in the image at the current moment, so as to obtain the feature points tracked on the image at the current moment, and transmit the obtained feature point signals to the MSOC-S-IKF state update module based on local feature observation; the MSOC-S-IKF state update module based on local feature observation is used to calculate the observation error by reprojection error through the input feature point information, combined with the input predicted state variables and covariance, and update the state variables and covariance by using the invariant Kalman filter (IKF) proposed by the invention combined with multi-state constraints (MS).

[0010] As a further improvement, the state variables and their corresponding covariances described in the present invention need to include the following variables:

[0011] 1) The state of the system at the current time t: including the rotation matrix and translation vectors Composition pose, linear speed

[0012] 2) Feature points in the local reference system

[0013] 3) Deviation of angular velocity and linear acceleration of IMU sensor (bias), constitutes the variable

[0014] 4) The system’s body position at the past s moments s is the size of the sliding window, and the position The rotation matrix representing the orientation and the vector representing the position composition. A state update module for MSOC-S-IKF based on local feature observations;

[0015] 5) Pose transformation between local odometer reference frame and map reference frame L T G (called augmented variables), represented by the rotation matrix L R G and the vector representing the position L p G This variable needs to be added to the state variable in the PnP initialization augmented variable module in the map-based positioning module;

[0016] 6) Map keyframe pose posture The rotation matrix representing the orientation and the vector representing the position This variable adds the map keyframe pose to the system state module and adds it to the state variable in the map-based positioning module.

[0017] As a further improvement, the state variables 1), 2) and 5) described in the present invention are represented together in a new Lie group space In the above, state variables 4) and 6) are represented in the Lie group space SE(3), and state variable 3) is represented in the Euclidean vector space. The specific definition of is:

[0018]

[0019] Where diag(.) represents the diagonal block matrix and SO(3) is defined as: det(.) represents the determinant of the matrix. SE 2+K (3) is defined as:

[0020]

[0021] With Lie Group The corresponding Lie algebra Defined as:

[0022]

[0023]

[0024] based on and The definition of , the state representation composed of states 1), 2), and 5) is:

[0025]

[0026] The corresponding state error is defined as:

[0027]

[0028]

[0029]

[0030]

[0031]

[0032]

[0033] The error in state 3) is defined as:

[0034]

[0035] State 4) Corresponding posture The error is defined as:

[0036]

[0037]

[0038] State 6) Corresponding posture The error is defined as:

[0039]

[0040]

[0041] In the above equations, θ represents the error of the corresponding rotation matrix R, which is defined as: θ = log(RR -1 ), log maps the Lie group SO(3) to the corresponding Lie algebra so(3). Indicates an estimated value.

[0042] As a further improvement, the state propagation module and the MSOC-S-IKF state update module based on local feature observation described in the present invention use the definition of the state error as above when solving the Jacobian of the motion equation or observation equation with respect to the state variable error.

[0043] As a further improvement, the MSOC-S-IKF state update module based on local feature observation of the present invention is completed by the following formula when performing state update:

[0044]

[0045]

[0046]

[0047] P t|t =P t|t-1 -K t H t P t|t-1

[0048] Among them, H t is the Jacobian of the observation equation relative to the state error, W t is the variance of the observation noise, r t is the “innovation term” derived from the observed residual, and exp() is the exponential mapping corresponding to the corresponding Lie group. t|t-1 is the variance of the state propagation module output, and P t|t They are the state variables and corresponding covariance after the MSOC-S-IKF state update module is updated based on local feature observations.

[0049] As a further improvement, the map feature matching module described in the present invention is used to obtain map feature points matched with the current camera and map key frames that can observe these map feature points through the information of the current camera and the map, that is, matched feature pairs and map key frames, and the matched feature pairs and map key frame signals are transmitted to the map-based positioning module.

[0050] As a further improvement, the map-based positioning module of the present invention includes the following modules:

[0051] Determine whether there is a feature matching pair module, determine whether the augmented variable is initialized, use PnP to initialize the augmented variable module, add the augmented variable to the system state module, add the map key frame pose to the system state module, and update the MSOC-S-IKF state module based on map feature observation, output the odometer state and augmented variable module; determine whether there is a feature matching pair module for receiving information from the map feature matching module, determine whether there is a map key frame matching the current frame and the corresponding feature matching pair; determine whether the augmented variable is initialized module for determining whether the relative pose (i.e., augmented variable) between the map reference system and the local odometer reference system has been initialized; use PnP to initialize the augmented variable module for calculating the relative pose between the map reference system and the local odometer reference system, and obtain the value of the augmented variable and the corresponding covariance; the augmented variables are added to the system state module to add the augmented variables to the variables maintained by the system, so that the augmented variables can be updated in real time through the subsequent MSOC-S-IKF state update module based on map feature observation; the map keyframe pose is added to the system state module to add the pose and covariance of the map keyframe to the system state variables and the system covariance, thereby naturally considering the uncertainty of the map and improving the consistency of the system; the MSOC-S-IKF state update module based on map feature observation uses map information to update the system state variables while satisfying 1) considering map uncertainty, 2) maintaining the correct observability of the system, and 3) the low amount of computation required by the system, so as to obtain higher-precision odometer states and augmented variables, and thus obtain higher-precision robot poses in the map reference system.

[0052] As a further improvement, the module for judging whether there is a matching feature pair described in the present invention is used to judge whether the map feature matching module has an output. If there is an output, it means that there is a feature matching pair between the current frame and the map frame, and the subsequent map-based positioning related process is performed. If there is no output, it means that there is no feature matching pair between the current frame and the map frame, and the odometer state and the augmented variables (if the augmented variables have been initialized) are directly output without performing the MSOC-S-IKF state update based on map feature observation; the module for judging whether the augmented variables are initialized is used to determine whether the system needs to initialize the augmented variables (i.e., the relative position between the local odometer reference system and the map reference system). If the map feature matching module has output matching feature pairs and map key frames, and the augmented variables have been initialized, the map key frames are directly added to the system state, and the matching feature pairs and map key frames will also be used in the subsequent MSOC-S-IKF state update module based on map feature observation; if the map feature matching module has output matching feature pairs and map key frames, but the augmented variables have not been initialized, the matching feature pairs and map key frames are The frame is used to initialize the augmented variable module using PnP; the augmented variable module is initialized using PnP, and the map feature matching module outputs matching feature pairs and map key frames, and the PnP algorithm is used to solve the relative pose between the map key frame and the current frame of the camera, and the map key frame pose and the current moment The robot's pose in the local odometer reference system is used to solve the relative pose (augmented variable) between the local odometer reference system and the map reference system to complete the initialization; the augmented variable is added to the system state module to add the initialized augmented variables and their covariance to the system state state, thereby maintaining and updating the augmented variables in real time, so that its estimation accuracy gradually improves and tends to be stable; the map key frame pose is added to the system state module specifically as follows: when the map key frame has a feature match with the current frame and the augmented variables have been initialized, the map key frame is directly added to the system state; when the map key frame has a feature match with the current frame and the augmented variables have not been initialized, the initialization is completed by using the PnP initialization augmented variable module, and then the map key frame pose signal used for initialization is passed to the map key frame pose added to the system state module.

[0053] As a further improvement, the MSOC-S-IKF state update module based on map feature observation described in the present invention includes three parts:

[0054] a) Observation equation based on map feature points: project the matched map feature points to the current frame and the map key frame respectively; when calculating the reprojection error, it is necessary to solve the Jacobian matrix of the observation equation with respect to the map point. By calculating the left null space of the Jacobian matrix and multiplying the left null space by the observation equation, the error term corresponding to the map point feature is eliminated, and the uncertainty of the map feature point is implicitly considered;

[0055] b) IKF state update based on Schmidt filter: The purpose of integrating Schmidt filter with IKF state update is to maintain the computational efficiency of the filtering algorithm while considering the uncertainty of map information; for the obtained observation equation

[0056]

[0057] Follow the steps below to update the status:

[0058]

[0059]

[0060]

[0061]

[0062]

[0063] Among them, the subscript t represents the state quantity and covariance of the system at the current moment, and t|t-1 represents the state quantity and covariance of the system before the state update. The “innovation term” derived from the observed residuals, is the Jacobian of the observation equation based on the map feature points about the system state, which is divided into two parts: They are respectively a ,X n The relevant Jacobian, where X a Indicates the status part that needs to be updated in real time, X n Represents the keyframe part of the map. The observation noise The covariance matrix, P t|t-1 is the covariance of the system state before the state update, according to It can be divided into blocks:

[0064]

[0065] The final updated state of the robot. The state of the keyframe portion of the map, which remains unchanged.

[0066] c) Observability Constraint: In order to ensure the correct observability of the system and thus the consistency of the system, the present invention proposes an "Observability Constraint" (OC) algorithm for map-based positioning. Specifically, assuming that the right null space of the observability matrix of the system under ideal conditions is (in The values ​​corresponding to all time-invariant quantities are the corresponding initialized values). For the observation equation that projects the matched map feature points to the current frame, when calculating the Jacobian matrix of the observation equation with respect to the state variables, assuming that the Jacobian matrix originally calculated at the estimated value is H, after considering the observability constraint, the corresponding Jacobian should be calculated by the following formula:

[0067]

[0068] By replacing H with H * , which can ensure that the system always has correct observability.

[0069] The beneficial effects of the present invention are as follows:

[0070] 1) A new Lie group is proposed, on which the maintained odometer-related variables and the relative transformation between the local odometer reference frame and the pre-built map reference frame (i.e., augmented variables) are represented. This leads to a new "Invariant Kalman Filter" (IKF) algorithm, which ensures that the algorithm can maintain the correct observability of the system without considering map uncertainties.

[0071] 2) The algorithm combines the proposed IKF with the Schmidt filter, which ensures the computational efficiency of the algorithm while taking into account the map information, so that the computational complexity of the system only increases linearly with the increase of map information.

[0072] 3) In order to ensure the correct observability of the system while taking into account the uncertainty of map information, an “observability constraint” (OC) algorithm is proposed.

[0073] 4) Expand the IKF into a multi-state constrained (MS) IKF to improve the estimation accuracy of the filter and obtain the final multi-state (MS) observability constrained (OC) Schmidt (S) invariant Kalman filter (IKF): MSOC-S-IKF. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 is a system flow chart of the system used by the algorithm of the present invention;

[0075] Figure 2 It is the coordinate system definition diagram; DETAILED DESCRIPTION

[0076] The technical solution of the present invention is further described below by means of specific embodiments in conjunction with the accompanying drawings:

[0077] Figure 1 It is a system flow chart of the system used in the algorithm of the present invention; the system is mainly composed of three parts: local odometer part, map feature matching part, and map-based positioning part. The so-called MSOC-S-IKF is a general term for the four major algorithms that combine multi-state constraints (MS), observability constraints (OC), Schmidt filtering (S) and "invariant Kalman filtering" (IKF). These four major parts are also the four major innovations of the present invention. Among them, IKF is the architecture of the entire filtering algorithm, which is applied to the two major parts of local odometer and map-based positioning. Multi-state constraints (MS) only act on the local odometer part. Observability constraints (OC) and Schmidt filtering (S) act on the map-based positioning part.

[0078] The whole system involves five types of coordinate systems, such as Figure 2 Shown:

[0079] 1) Local odometer coordinate system L. The robot poses obtained by the local odometer are all expressed in this system.

[0080] 2) Inertial Measurement Unit (IMU) / Robot Coordinate System I. The angular velocity and linear velocity provided by the IMU are expressed in this system. The subscript t indicates the time t.

[0081] 3) Camera coordinate system C fixed to the IMU.

[0082] 4) Map coordinate system G. Map information mainly consists of map keyframes and map feature points. The poses of keyframes and the positions of feature points are expressed in the map coordinate system.

[0083] 5) Map key frame coordinate system KF.

[0084] 1. Based on the new Lie group System status

[0085] Before introducing the various modules of the system, we first introduce the representation of the state variables maintained by the system. Unlike conventional systems that represent state variables in Euclidean vector space, one of the innovations of this invention is to represent the state variables of the system in a new Lie group space Above. Lie Group By Lie Group SE 2+K (3) and the Lie group SO(3), which is defined as:

[0086]

[0087] Where diag(.) represents the diagonal block matrix and SO(3) is defined as: det(.) represents the determinant of the matrix. SE 2+K (3) is defined as:

[0088]

[0089] With Lie Group The corresponding Lie algebra Defined as:

[0090]

[0091]

[0092] Based on Lie Group and the corresponding Lie algebra The present invention represents the state of the system in Specifically, it means (Here we assume that only one local feature point is considered). We define the state of the system at time t as

[0093]

[0094]

[0095] in L R I Represents the rotation of the robot body coordinate system I in the local odometer coordinate system L. This variable can also rotate the three-dimensional vector in the I system to the L system. L v I and L p I represent the velocity and position of I under L respectively. L p f is the position of the local feature point f under L. L p G and L R G It constitutes the relative transformation between the odometer reference system and the map reference system, namely the “augmented variable”. g ,b a Indicates the bias of the gyroscope and accelerometer in the IMU.

[0096] With the state definition (1) of the system, we also need to define the error corresponding to the state. Different from the traditional definition in Euclidean vector space, the state error of (1) is defined as:

[0097]

[0098] in Represents an estimate. Converting it to Euclidean vector space, we have the following definition:

[0099]

[0100]

[0101]

[0102]

[0103]

[0104]

[0105]

[0106]

[0107]

[0108] in is the error vector of the rotation matrix R. log maps the Lie group SO(3) to the corresponding Lie algebra so(3). We define ∈ t The error in the system state is called the "right invariant error".

[0109] In particular, when we need to consider the uncertainty of the map, we need to add the relevant information of the map to the state of the system, so the state of the system can be further expanded from formula (1) to:

[0110]

[0111] in Contains the key frame poses of m maps And the locations of n map feature points Each map keyframe pose The rotation matrix representing the orientation and the vector representing the position Correspondingly, the error of the map keyframe pose Defined as:

[0112]

[0113]

[0114] The error of the map feature point is defined as:

[0115]

[0116] The state of the entire system taking into account map uncertainty The error of is expanded from formula (3) to:

[0117]

[0118] Since the state of the system is defined in Lie groups And there is a special error definition (4). The propagation and update of state and variance in this system state is called "Invariant Kalman Filter" (IKF). The specific state propagation and update method will be introduced in the next section.

[0119] 2. Local Odometry

[0120] like Figure 1 As shown, the input of the local odometer at time t is the angular velocity ω of the body obtained by the IMU t and linear acceleration a t As mentioned above, multi-state constraints (MS) and the "invariant Kalman filter" (IKF) are partially related to the local odometer. In order to improve the positioning accuracy of the local odometer through MS, it is necessary to introduce additional IMU clone variables Ignore map-related variables Then there is the following state definition:

[0121]

[0122] in Contains the poses of s cloned IMUs IMU pose of each clone The rotation matrix representing the orientation and the vector representing the position Correspondingly, the error of the cloned IMU pose is Defined as

[0123]

[0124]

[0125] Furthermore, The error is:

[0126]

[0127] 1) State Propagation

[0128] The IMU data at time t is used for state propagation (prediction). Through the IMU kinematic model,

[0129] The state prediction of the system at time t+1 can be obtained:

[0130]

[0131] The system status as well as It will be updated according to the input IMU data, and the rest of the state variables remain unchanged. The subscript t+1|t represents the predicted state at time t+1 obtained through the IMU data, that is, the state variable output by the state propagation module. f represents the kinematic equation of the IMU. and is the observation noise of IMU linear acceleration and angular velocity, which is constructed as zero-mean Gaussian white noise. Through the above formula, we can obtain The covariance of the error state, that is, the covariance of the output of the state propagation module:

[0132]

[0133] Where P t|t and Q t|t Respectively represent the state covariance and noise covariance at time t. Φ t and G t They represent (6) respectively about the system state error and noise The Jacobian matrix of .

[0134] 2) MSOC-S-IKF state update based on local feature observation

[0135] The camera image is used in the observation equation part. By tracking the feature points of the image stream, that is, Figure 1 In the feature tracking module, we can obtain a series of feature points {f} and a series of historical poses of clones that observe these features For each For the observed feature f, we have the following observation equation:

[0136]

[0137] in is the two-dimensional pixel coordinate of feature f in the image, L p f is the three-dimensional coordinate of f in the L system, is the observation noise of the image, h is the observation equation, which L p f Through posture Projected into the image.

[0138] By performing a first-order Taylor expansion on equation (8) at the current estimated state, we can obtain

[0139]

[0140]

[0141] in represents the observed residual. Denotes the observation equation (8) with respect to the state The Jacobian matrix (excluding the local feature point f). i H f is the Jacobian of the observation equation (8) with respect to the feature point f. and ∈ f is the “right invariant error” of the relevant quantity.

[0142] Since a feature f can be The camera corresponding to the pose at multiple moments (assuming n moments) in the feature f is observed, so for each camera that observes feature f, the pose corresponding to We can get an observation equation (8), and then get (10). Stacking these n equations (10) together, we can get

[0143]

[0144] By calculating H f The left null space N, (11) can be transformed into:

[0145]

[0146] in The IKF state is updated by equation (12). The specific update method of IKF is:

[0147] The input is: observation residual r (for formula (12), r = f r' MS ), the Jacobian H of the observation equation with respect to the system state (for equation (12), ), observation noise w (for (12), w = w′ MS ) corresponding to the covariance W, the system state output by the state propagation module and the covariance P t|t-1 .

[0148] renew:

[0149]

[0150]

[0151]

[0152] P t|t =P t|t-1 -K t H t P t|t-1

[0153] Where exp is the exponential mapping corresponding to the corresponding Lie group.

[0154] The output is: the updated system state at time t And the corresponding covariance P t|t .

[0155] 3. Map feature matching

[0156] like Figure 1 As shown in the figure, to perform map-based positioning, we need to obtain the matching relationship between the current camera and the map information, that is, the map feature matching module. The pre-built map will contain feature descriptors, such as BRIEF, ORB, SIFT, etc. Through these feature descriptors, we can obtain the map feature points and map keyframes that match the current camera, that is, the matched feature pairs. This matching information will be used in the map-based positioning algorithm.

[0157] 4. Map-based positioning

[0158] This part of the state update involves two innovations. One is the combination of Schmidt filter (S) and IKF, which takes the uncertainty of the map into account; the other is the observability constraint (OC) algorithm, which is used to ensure that the original observability of the system will not be destroyed when the state is updated based on the map. Both innovations are to keep the system consistent and improve the positioning accuracy of the map-based positioning algorithm. The states involved in this module are:

[0159]

[0160] in Contains m map keyframe poses. Different from the state defined in equation (3), here we only add the pose of the map keyframe to the state, but not the state of the map feature points, thus saving storage and subsequent calculation.

[0161] Through the map feature matching module, the feature matching pairs of the map key frame and the current frame can be obtained, such as Figure 2 KF in 1 , KF 2 The black dot and C t When there is a feature match between the map frame and the current frame, the map frame is added to the state vector.

[0162] 1) Augmented variables L T G initialization

[0163] If the feature matching pair of the map key frame and the current frame is obtained, and the augmented variable L T G It has not been initialized yet. The relative pose between the map frame and the current frame is obtained through the PnP (Perspective-n-Point) algorithm, that is, Figure 2 middle Then the relative transformation between the map reference system and the local odometer reference system is obtained L T G Then we use this relative pose L T G is added to the state estimator.

[0164] 2) MSOC-S-IKF state update based on map feature observation

[0165] a) Observation equation based on map feature points: Figure 2 As shown, the matched map feature points can be Projected to the current frame C respectively t , map key frame KF 1 and KF 2 (Generalized as KF in the following formula i ):

[0166]

[0167] in Represent the map feature points F j (The location on the map is ) between the current frame C and the map key frame KF i Two-dimensional pixel observation. 1 ,h 2 are the corresponding observation projection equations respectively. is the corresponding observation noise.

[0168] Performing a first-order Taylor expansion of equation (14) at the estimated value, we can obtain the following observation equation:

[0169]

[0170] in They are respectively the observation equation (14) about The Jacobian matrix at its estimated value. They are The "right invariant error" of .

[0171] Since we do not add the feature points of the map to the state, we need to implicitly take the uncertainty of the map into account to ensure the consistency of the system. The specific approach is: after the two observation equations in (15) are combined into one equation through matrix stacking, the similar operation as (12) can be calculated by The left null space is used to eliminate map feature points The corresponding error term finally obtains the following compact observation equation expression:

[0172]

[0173] in is the simplified observation error, is the simplified observation equation about the state variable The Jacobian matrix of is a state variable The "right invariant error" of is the simplified observation noise.

[0174] b) IKF state update based on Schmidt filter

[0175] Since map-based positioning needs to consider the position of the map keyframe in the state, and as the system's running time increases, the number of map keyframe pairs in the state gradually increases. In order to ensure the computational efficiency of the filtering algorithm, we incorporate the idea of ​​Schmidt filter state update when updating the IKF state. Specifically, for the observation equation (16), the state update is performed according to the following steps:

[0176]

[0177]

[0178]

[0179]

[0180]

[0181] in The observation noise The covariance matrix of P. t|t-1 is the covariance of the system state before the state update, according to It can be divided into blocks:

[0182]

[0183] The final updated state of the robot. The state of the keyframe portion of the map, which remains unchanged.

[0184] c) Observability Constraints

[0185] In order to ensure the correct observability of the system and thus the consistency of the system, this paper proposes an "observability constraint" (OC) algorithm for map-based positioning. Specifically, it is assumed that the null space of the observability matrix of the system under ideal conditions is (in All time-invariant quantities have values ​​corresponding to the corresponding initialized values).

[0186] In calculating (14)-(a) about the state When the Jacobian matrix of , assuming that the Jacobian matrix originally calculated at the estimated value is H, after considering the observability constraint, the corresponding Jacobian should be calculated as follows:

[0187]

[0188] By replacing H with H * , which can ensure that the system always has correct observability.

[0189] In order to verify the effectiveness of the proposed MSOC-S-IKF, we compared it with the pure odometer Open-VINS[1] and the optimized algorithm VINS-Fusion[2][3], and also conducted ablation experiments to verify the effectiveness of the innovations proposed in this invention. MSC-S-EKF does not consider observability constraints (OC) and invariant Kalman filters (IKF); MSC-EKF does not consider observability constraints (OC), Schmidt filters (S), and invariant Kalman filters (IKF); MSC-IKF does not consider observability constraints (OC) and Schmidt filters (S). The experiment was verified on four datasets: EuRoC[4], corresponding to V102-MH05 in the table below; Kaist[5], corresponding to Urban39 in the table below; YQ[6], corresponding to YQ2-YQ4 in the table below; 4Seasons[7], corresponding to Office-Loop2 in the table below. The “—” in the table means that the algorithm cannot obtain this variable. It means that the algorithm cannot obtain the correct positioning result on the corresponding data set.

[0190]

[0191] From the above table, we can see that the algorithm MSOC-S-IKF proposed by us can run successfully on all datasets and has the best positioning results in most cases. Compared with the pure odometer Open-VINS, MSOC-S-IKF has significantly improved the positioning accuracy because it can correctly integrate map information, especially for large scene datasets Kaist, YQ, 4seasons. Compared with the optimization-based positioning algorithm VINS-Fusion, and the ablation algorithm MSC-EKF, MSC-IKF, these algorithms do not correctly consider the uncertainty of the map, and the positioning accuracy (especially on large scene datasets) is far inferior to MSOC-S-IKF. On some datasets, the compared algorithms cannot even run properly. Compared with MSC-S-IKF, the positioning results (especially the positioning results of local odometers) are very poor because it fails to maintain the correct observability of the system.

[0192] Obviously, the present invention is not limited to the above embodiments, and many variations are possible. All variations that can be directly derived or associated with the contents disclosed by ordinary technicians in this field should be considered to be within the protection scope of the present invention.

[0193] [1] P.Geneva, K.Eckenhoff, W.Lee, Y.Yang and G.Huang, "OpenVINS: A ResearchPlatform for Visual-Inertial Estimation," 2020 IEEE International Conference on Robotics and Automation(ICRA), Paris, France, 2020, pp.4666-4672, doi:10.1109 / ICRA40945.2020.9196524.

[0194] [2]T.Qin, P.Li and S.Shen, "VINS-Mono: A Robust and Versatile MonocularVisual-Inertial State Estimator," in IEEE Transactions on Robotics , vol.34,no.4,pp.1004-1020,Aug.2018,doi:10.1109 / TRO.2018.2853729.

[0195] [3]T.Qin,S.Cao,J.Pan,and S.Shen,“A general optimization-basedframework for global pose estimation with multiple sensors,”arXive-prints,2019,retrieved on March 1st,2020.[Online].Available:https: / / arxiv.org / abs / 1901.03642v1

[0196] [4]B.Siciliano et al.,“EuRoC-The Challenge Initiative for EuropeanRobotics,”ISR / Robotik 2014;41st International Symposium on Robotics,Munich,Germany,2014,pp.1-7.

[0197] [5]J.Jeong,Y.Cho,Y.S.Shin , et al.“Complex urban dataset with multi-level sensors from highly diverse urban environments”,The InternationalJournal of Robotics Research,2019,38(6):027836491984399.

[0198] [6]X.Ding et al.,“Persistent Stereo Visual Localization on Cross-Modal Invariant Map,”in IEEE Transactions on Intelligent TransportationSystems , vol.21,no.11,pp.4646-4658,Nov.2020,doi:10.1109 / TITS.2019.2942760.

[0199] [7]R.Wenzel,R.Wang,N.Yang ,et al.(2020,September).“4Seasons:A cross-season dataset for multi-weather SLAM in autonomous driving”,In DAGM GermanConference on Pattern Recognition,2020,pp.404-417,Springer,Cham.

Claims

1. A consistent and efficient map-based filtering method for visual inertial positioning, characterized in that: The method is implemented by the following system, which includes three modules: a local odometer module, a map feature matching module and a map-based positioning module. The local odometer is used to receive data from the camera and the IMU, obtain the state of the system in the local reference system in real time, and obtain the value of the corresponding state variable and its covariance; the map feature matching is used to detect the similarity between the scene observed by the camera at the current moment and the map scene built in advance, and obtain the feature matching pair of the image feature at the current moment and the map feature; the map-based positioning module is used to receive the output of the local odometer and the feature matching pair, obtain the updated local odometer state and the relative transformation between the local reference system and the map reference system, and then calculate the state of the robot in the map reference system; The local odometer module includes an IMU, a state propagation module connected to the IMU signal, a camera, a feature tracking module connected to the camera signal, and an MSOC-S-IKF state update module based on local feature observation connected to the state propagation module and the feature tracking module signal; The IMU is used to provide the system with real-time rotational angular velocity and linear acceleration; the state propagation module is used to receive the rotational angular velocity and linear acceleration provided by the IMU, and use these quantities to propagate the state of the system from the previous moment to the current moment, and obtain the state variable predicted at the current moment and its corresponding covariance; the obtained state variable and covariance signal are transmitted to the MSOC-S-IKF state update module based on local feature observation; the feature tracking module is used to track the position of the feature point in the image at the previous moment in the image at the current moment, so as to obtain the tracked feature point on the image at the current moment, and transmit the obtained feature point signal to the MSOC-S-IKF state update module based on local feature observation; the MSOC-S-IKF state update module based on local feature observation is used to calculate the observation error by reprojection error through the input feature point information, combined with the input predicted state variable and covariance, and update the state variable and covariance by using the proposed invariant Kalman filter combined with multi-state constraints; The MSOC-S-IKF state update module based on map feature observation includes three parts: a) Observation equation based on map feature points: The matched map feature points are projected to the current frame and the map key frame respectively; when calculating the reprojection error, it is necessary to solve the Jacobian matrix of the observation equation with respect to the map point, and by calculating the left null space of the Jacobian matrix, the left null space is multiplied by the observation equation, thereby eliminating the error term corresponding to the map point feature, and implicitly considering the uncertainty of the map feature point; b) IKF state update based on Schmidt filter: The purpose of integrating Schmidt filter with IKF state update is to maintain the computational efficiency of the filtering method while considering the uncertainty of map information; for the obtained observation equation Follow the steps below to update the status: Among them, the subscript t represents the state quantity and covariance of the system at the current moment, t|t-1 represents the state quantity and covariance of the system before the state update, and r Gt is the "innovation term" derived from the observed residuals, is the Jacobian of the observation equation based on the map feature points about the system state, which is divided into two parts: They are respectively a ,X n The relevant Jacobian, where X a Indicates the status part that needs to be updated in real time, X n Represents the keyframe part of the map. The observation noise The covariance matrix, P t|t-1 is the covariance of the system state before the state update, according to It can be divided into blocks: The final updated state of the robot. The state of the keyframe part of the map. remain unchanged; c) Observability constraint: In order to ensure the correct observability of the system and thus the consistency of the system, the "observability constraint" OC method is used for map-based positioning. If the right null space of the observability matrix of the system under ideal conditions is in The values ​​corresponding to all time-invariant quantities are the corresponding initialized values. For the observation equation that projects the matched map feature points to the current frame, when calculating the Jacobian matrix of the observation equation with respect to the state variables, if the Jacobian matrix originally calculated at the estimated value is H, after considering the observability constraint, the corresponding Jacobian should be calculated by the following formula: By replacing H with H * , ensuring that the system always has correct observability.

2. The consistent and efficient map-based filtering method for visual inertial positioning according to claim 1, characterized in that: The state variables and their corresponding covariances include the following variables: 1) The state of the system at the current time t: including the rotation matrix and translation vectors Composition pose, linear speed 2) Feature points in the local reference system 3) Deviation of angular velocity and linear acceleration of IMU sensor Composition variables 4) The system’s body position at the past s moments s is the size of the sliding window set, and the position {X CPi }The rotation matrix representing the orientation and the vector representing the position composition, A state update module for MSOC-S-IKF based on local feature observations; 5) Pose transformation between local odometer reference frame and map reference frame L T G , represented by the rotation matrix L R G and the vector representing the position L p G This variable needs to be added to the state variable in the PnP initialization augmented variable module in the map-based positioning module; 6) Map keyframe pose posture The rotation matrix representing the orientation and the vector representing the position This variable adds the map keyframe pose to the system state module and adds it to the state variable in the map-based positioning module.

3. The consistent and efficient map-based filtering method for visual inertial positioning according to claim 2, characterized in that: The state variables 1), 2) and 5) are represented together in a new Lie group space In the above, state variables 4) and 6) are represented in the Lie group space SE(3), and state variable 3) is represented in the Euclidean vector space. The specific definition of is: Where diag(.) denotes the definition of the diagonal block matrix SO(3): det(.) represents the determinant of the matrix, SE 2+K (3) is defined as: With Lie Group The corresponding Lie algebra Defined as: based on and The definition of, the state representation composed of state 1), 2), and 5) is: The corresponding state error is defined as: The error in state 3) is defined as: State 4) Corresponding posture The error is defined as: State 6) Corresponding posture The error is defined as: In the above equations, θ represents the error of the corresponding rotation matrix R, which is defined as: θ = log(RR -1 ), log maps the Lie group SO(3) to the corresponding Lie algebra so(3), and the above formulas Indicates an estimated value; When the state propagation module and the MSOC-S-IKF state update module based on local feature observation solve the Jacobian of the motion equation or observation equation with respect to the state variable error, the state variable error is the error defined above.

4. The consistent and efficient map-based filtering method for visual inertial positioning according to claim 1, characterized in that: The MSOC-S-IKF state update module based on local feature observation is completed by the following formula when performing state update: P t|t =P t|t-1 -K t H t P t|t-1 Among them, H t is the Jacobian of the observation equation relative to the state error, W t is the variance of the observation noise, r t The "innovation term" exp() derived from the observed residual is the exponential mapping corresponding to the corresponding Lie group, P t|t-1 is the variance of the state propagation module output, X MSt and P t|t They are the state variables and corresponding covariance after the MSOC-S-IKF state update module is updated based on local feature observations.

5. The consistent and efficient map-based filtering method for visual inertial positioning according to claim 1, 2 or 3, characterized in that: The map feature matching module is used to obtain the map feature points matched with the current camera and the map key frames that can observe these map feature points through the information of the current camera and the map, that is, the matched feature pairs and map key frames. The matched feature pairs and map key frame signals are transmitted to the map-based positioning module.

6. The consistent and efficient map-based filtering method for visual inertial positioning according to claim 5, characterized in that: The map-based positioning module includes the following modules: a module for determining whether there is a feature matching pair, a module for determining whether an augmented variable is initialized, a module for initializing an augmented variable using PnP, a module for adding augmented variables to a system state module, a module for adding a map keyframe pose to a system state module, an MSOC-S-IKF state update module based on map feature observation, and a module for outputting an odometer state and an augmented variable; the module for determining whether there is a feature matching pair is used to receive information from a map feature matching module, determine whether there is a map keyframe matching the current frame and a corresponding feature matching pair, and if there is an output, it means that there is a feature matching pair between the current frame and the map frame, and then the subsequent map-based positioning related process is performed; if there is no output, it means that the current frame and the map frame have a feature matching pair. If there is no feature matching pair between map frames, the odometer state and augmented variables are directly output without updating the MSOC-S-IKF state based on map feature observation; the module for determining whether the augmented variables are initialized is used to determine whether the relative pose (i.e., augmented variables) between the map reference system and the local odometer reference system has been initialized. If the map feature matching module has output matching feature pairs and map key frames, and the augmented variables have been initialized, the map key frames are directly added to the system state, and the matching feature pairs and map key frames will also be used in the subsequent MSOC-S-IKF state update module based on map feature observation. If the map feature matching module has output matching feature pairs and map key frames, but the augmented variables have been initialized, the map key frames are directly added to the system state. If the augmented variables are not initialized, the matched feature pairs and map key frames are used to initialize the augmented variables module using PnP; the augmented variables initialized using PnP are used to calculate the relative pose between the map reference system and the local odometer reference system, and the map feature matching module receives the output matched feature pairs and the map key frames, and uses the PnP method to solve the relative pose between the map key frame and the current frame of the camera, and uses the map key frame pose and the current moment The robot's pose in the local odometer reference system is used to solve the relative pose between the local odometer reference system and the map reference system, and obtains the value of the augmented variable and the corresponding covariance to complete the initialization; the module for adding the augmented variables to the system state is used to add the augmented variables to the variables maintained by the system. The augmented variables are updated in real time through the subsequent MSOC-S-IKF state update module based on map feature observation; the module for adding the map keyframe pose to the system state is used to add the pose and covariance of the map keyframe to the state variables of the system and the covariance of the system, thereby naturally considering the uncertainty of the map, thereby improving the consistency of the system; the MSOC-S-IKF state update module based on map feature observation uses map information to update the system state variables while satisfying 1) considering the map uncertainty, 2) maintaining the correct observability of the system, and 3) the required amount of calculation of the system is low, thereby obtaining a higher precision odometer state and augmented variables, thereby obtaining a higher precision pose of the robot in the map reference system.

Images