CN117421384A - Visual inertia SLAM system sliding window optimization method based on common view projection matching - Google Patents

Abstract

A sliding window optimization method of a visual inertia SLAM system based on common view projection matching relates to the technical field of SLAM. The invention aims to solve the problem of low precision of a sliding window optimization method at the rear end of the existing visual inertia SLAM system. According to the invention, on the premise of not changing the rapidity of sliding window optimization, the sliding window optimization method is improved through the co-view projection matching relationship, so that a shadow map point is eliminated, and the map point precision is improved; the number of map points successfully matched with the key frames is increased, the observation field of the key frames is enlarged, and the visual observation constraint of the SLAM system is newly increased, so that the precision of the sliding window for optimizing and estimating the pose is improved, and the positioning precision of the mobile robot is further improved.

Description

Sliding window optimization method for visual-inertial SLAM system based on common-view projection matching

Technical field

The invention belongs to the technical field of SLAM.

Background technique

SLAM (Simultaneous Localization and Mapping) technology enables mobile robots to achieve autonomous navigation, positioning and planning. Through one or more sensor devices carried by the mobile robot itself, it can sense environmental information, measure its own posture, and realize mobile control. The robot tracks its own pose, locates its position relative to the surrounding environment, and constructs an environment map. The SLAM system can be divided into two parts: front-end and back-end. The front-end implements functions such as environment perception, pose tracking, and loop detection, while the back-end implements functions such as pose optimization, trajectory correction, and map construction. The front-end mainly completes the work related to data processing so that SLAM can display and update interactive information in real time. The back-end mainly completes the calculation-intensive and time-consuming optimization related work such as eliminating accumulated errors. The back-end optimization method of SLAM is divided into filtering method and nonlinear optimization method. The filtering method estimates the posterior probability of the mobile robot's pose through prediction and measurement information based on Bayesian theory, thereby updating its pose, but the coordination The variance matrix and the state quantity have a square growth relationship, which results in an excessive amount of data and is not suitable for use in large-scale scene environments; the nonlinear optimization method incorporates the mobile robot's pose information, measurement constraints, and even the location information of map points. Into the factor graph model, the overall update of its state estimate is achieved through nonlinear optimization.

Currently, the nonlinear optimization method is the mainstream solution for SLAM back-end processing. In order to quickly correct and update the pose state of the mobile robot, the SLAM back-end usually adopts local optimization, that is, selecting a part of key frames for local pose optimization. , thereby speeding up the response speed of back-end optimization. The sliding window method is a classic local area screening and optimization method. By setting a sliding window with a fixed number of key frames, a constraint relationship model is established between visual and inertial measurements in the local area, and then the model is optimized. The sliding window method is commonly used for local area screening in SLAM back-end optimization of visual-inertial fusion. A typical example is "VINS-Mono: A Robust and VersatileMonocular Visual-Inertial State Estimator" written by Shen Shaojie's team at the Hong Kong University of Science and Technology. "Strong and versatile monocular visual inertial state estimator", and "Keyframe-based Visual-inertial SLAM using Nonlinear Optimization" by Leutenegger et al. The sliding window method updates the keyframes in the sliding window through Shure compensation operation, and marginalizes the keyframes sliding in or out while retaining the constraint relationship between keyframes, ensuring that the number of keyframes in the sliding window remains unchanged, thus It enables the SLAM system to complete optimization calculations in a faster time, but the accuracy of the optimization cannot be guaranteed. The accuracy of the optimization is completely determined by the quality of the key frames and the error constraint relationship. Therefore, the traditional visual-inertial SLAM system needs to further improve its back-end sliding window optimization method to improve its optimization accuracy.

Contents of the invention

The present invention is to solve the problem of low accuracy of the sliding window optimization method at the back end of the existing visual inertial SLAM system, and now provides a sliding window optimization method for the visual inertial SLAM system based on common view projection matching.

The sliding window optimization method of the visual inertial SLAM system based on common view projection matching according to the present invention includes the following steps:

Step 1: When the back-end module of the visual-inertial SLAM system subscribes to the latest frame image of the robot published by its front-end module, it determines whether the sliding window is full. If so, proceed to step 2, otherwise proceed to step 4;

Step 2: Determine whether the next-newest frame image is a key frame. If so, marginalize the oldest frame image in the sliding window and then perform step three. Otherwise, marginalize the next-newest frame image and then perform step three;

Step 3: Reconstruct the constraint relationship of the sliding window according to Schur's complement theory, and then perform step 4;

Step 4: Determine whether the latest frame image is a key frame based on the disparity between the latest frame image and the current latest key frame image in the sliding window. If so, project the map points in the sliding window onto the latest frame image. The map The point is a map point observed by a key frame within the sliding window that can be co-viewed with the latest frame image, and then perform step five, otherwise update the IMU constraint relationship corresponding to the latest frame image, and then return to step one;

Step 5: Match the projection points on the latest frame image with the feature points, delete the projection points that do not match the feature points, and at the same time determine whether the feature points with matching projection points have original map points. The original map points are There are map points corresponding to the feature points in the sliding window before matching. If so, proceed to step 6; otherwise, proceed to step 7;

Step 6: Compare the observed times of the map points corresponding to the projected points and the original map points corresponding to the feature points, and only retain the map points that have been observed more times, so that the visual observation constraints of each key frame in the sliding window are updated, and then execute the steps eight;

Step 7: Add new map points to establish observation constraints between the latest frame image and the new map points, and then perform step 8;

Step 8: Determine whether the number of keyframes in the sliding window is full, if so, proceed to step 9, otherwise return to step 1;

Step 9: Construct the visual constraint factor, IMU pre-integration constraint factor and marginalization prior constraint factor respectively, and add the visual constraint factor, IMU pre-integration constraint factor and marginalization prior constraint factor to the sliding window factor graph optimization model Construct the objective function;

Step 10: Use the nonlinear optimization algorithm to optimize the objective function, update the state quantity of the robot in the sliding window, and complete the sliding window optimization.

Further, as described in step 2 above, the oldest frame image in the sliding window is marginalized, including:

Delete the oldest frame image and its corresponding IMU constraint relationship in the sliding window, and retain the visual constraint relationship of the oldest frame image;

Marginalize the next latest frame image as described in step 2, including:

Delete the next-new frame image and its visual constraint relationship, and retain the IMU constraint relationship corresponding to the next-new frame image.

Further, as described in step three above, the constraint relationship of the sliding window is reconstructed according to Schur's complement theory, including:

The state variable χ of all image frames in the sliding window is expressed as an incremental equation as follows:

Among them, δχ _d is the state vector to be marginalized, δχ _s is the retained state vector, H _a is the covariance matrix of δχ _d , H _c is the covariance matrix of δχ _s , Represents the transpose of H _b , H _b is the covariance matrix between δχ _d and δχ _s , b _d and b _s are the δχ _s constant vectors of δχ _d and δχ s respectively;

According to Schur's complement theory, Gaussian elimination is performed on the incremental equation of state variable χ and deduced to obtain:

Then, the retained state vector δχ _s is obtained according to the above formula to realize the reconstruction of the sliding window constraint relationship.

Further, in the above step 5, if the projection point on the latest frame image matches the descriptor of the feature point, then the projection point matches the feature point.

Further, in the above step nine, the state quantity χ _c of the visual constraint factor to be optimized is:

in, and/> are the positions of the camera in the i-th frame and j-th frame respectively,/> and/> are the postures of the camera in the i-th frame and j-th frame respectively, and the i-th frame and j-th frame are two adjacent key frames,/> and/> are the position and rotation amount of the camera relative to the IMU respectively, and λ is the inverse depth of the map point.

Further, in the above step nine, the variable to be optimized χ _imu of the IMU pre-integration constraint factor is:

in, and/> are the speeds of the IMU at time t _i and time t _j respectively,/> and/> are the accelerometer bias of the IMU at time t _i and time t _j respectively,/> and/> are the gyroscope offsets of the IMU at time t _i and time t _j respectively, and time t _i and time t _j are the times when the camera captures the i-th frame image and the j-th frame image, respectively.

Further, in the above step nine, the objective function expression is as follows:

Among them, χ is the state variable of all image frames in the sliding window and has k∈[0,N], λ _m is the inverse depth of the m-th map point, m is the total number of map points observed in the key frame image, N is the total number of all frame images in the sliding window, and the state vector of the k-th frame and/> are the position, speed and attitude of the camera in the k-th frame respectively, b _a and b _ω are the accelerometer offset and gyroscope offset of the IMU respectively, /> and/> are the position and rotation of the camera relative to the IMU respectively, e _C (x _i ,x _j ), e _B (x _i ,x _j ) and e _M respectively represent the visual constraint residual, IMU pre-integration constrained residual and marginalization first Experimentally constrained residual, x _i and x _j are the state vectors of the i-th frame image and j-th frame image respectively.

Further, the expression of the above visual constraint residual e _C (x _i ,x _j ) is as follows:

in, is the projection coordinate of map point P in the space on the normalized plane of the j-th frame image,/> is the observation coordinate of map point P in the space when it is first observed in the j-th frame image.

Further, the expression of the above IMU pre-integration constrained residual e _B (x _i ,x _j ) is:

Among them, r _p , r _q , r _v , are the position residual, attitude residual, speed residual, accelerometer residual and gyroscope residual of the robot from time t _i to time t j respectively. The time t _i and time _{t j} are respectively the i-th frame image taken by the camera. and the moment of the jth frame image.

Furthermore, the expression of the above marginalized prior constraint residual e _M is:

e _M =b _p -H _p δχ _1:N ,

Among them, δχ _1:N is the state increment of all key frames in the sliding window,

H _a is the covariance matrix of the state vector δχ _d to be marginalized, H _c is the covariance matrix of the retained state vector δχ _s , Represents the transpose of H _b , H _b is the covariance matrix between δχ _d and δχ _s , b _d and b _s are the constant vectors of δχ _d and δχ _s respectively.

The beneficial effects of the invention are as follows:

The present invention improves the sliding window optimization method of the SLAM backend and proposes a visual inertial SLAM system sliding window optimization method based on common view projection matching.

This method solves the problem that the same map point may be created multiple times in different running stages or different threads of SLAM, resulting in ghosting, by matching the projection of common view map points in the sliding window, and fuses these "shadow" map points into A map point improves the observation accuracy of the map point, thereby improving the accuracy of the SLAM system in estimating the pose of the mobile robot.

Furthermore, by screening common-view matching projection points, the present invention increases the number of map points successfully matched by the latest key frame, expands the observation field of view of the key frame, and adds visual observation constraints of the SLAM system, thereby improving the sliding The window optimizes the accuracy of the estimated pose, thereby improving the accuracy of the SLAM system in locating the position of the mobile robot.

Furthermore, the present invention does not change the size of the sliding window optimization area. The sliding window can accommodate a fixed number of key frames to be optimized, which can ensure the speed of the optimization operation, thereby ensuring the speed of visual inertial SLAM operation, while taking into account the improvement of The accuracy of SLAM in positioning and pose estimation of mobile robots is improved.

Description of the drawings

Figure 1 is the flow chart of the visual-inertial SLAM system;

Figure 2 is a flow chart of the sliding window optimization method of the visual-inertial SLAM system based on common-view projection matching according to the specific embodiment;

Figure 3 is a schematic diagram of the sliding window update process when the new frame image is a key frame;

Figure 4 is a schematic diagram of the sliding window update process when the new frame image is not a key frame;

Figure 5 is a schematic diagram of the common view relationship between the current key frame and its common view key frame;

Figure 6 is a schematic diagram of the projection matching relationship between the latest key frame in the sliding window and its common view key frame;

Figure 7 is a schematic diagram of the sliding window factor graph optimization model of visual inertia fusion;

Figure 8 shows the UAV flight trajectory estimated by our-VINS-Mono on the Machine Hall 01 data set;

Figure 9 is the relationship curve between the displacement change and time of the flight trajectory in Figure 8 on the three coordinate axes of x, y, and z;

Figure 10 is a graph showing the relationship between the APE curve and time using the absolute pose error index (APE) to evaluate the flight trajectory in Figure 8, and gives the root mean square error (rmse) and error median (median) of the global trajectory. , error mean (mean) and error standard deviation (std);

Figure 11 is a comparison of the APE error box plots of the flight trajectories estimated by VINS-Mono and our-VINS-Mono on the Machine Hall 01 data set;

Figure 12 is a comparison chart of the flight trajectories estimated by VINS-Mono and our-VINS-Mono on the Machine Hall 01 data set, and the APE error amount under different error evaluation indicators;

Figure 13 is a comparison chart of the flight trajectories estimated by VINS-Mono and our-VINS-Mono on the Machine Hall 01 data set, and the relationship between the density distribution of trajectory errors and the APE error.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without any creative work fall within the scope of protection of the present invention. It should be noted that, as long as there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

This embodiment will be described in detail with reference to Figures 1 to 13. The visual-inertial SLAM system sliding window optimization method based on common view projection matching described in this embodiment includes the following steps:

Step 1: Set the size of the sliding window so that it can accommodate the number of keyframes to be optimized to N.

Step 2: Use the back-end module of the visual-inertial SLAM system to subscribe to the pose status information of the mobile robot published by the front-end module. When the back-end module collects a new image frame, it determines whether the sliding window is full. If it is full, then If the sliding window needs to be updated, go to step 3; if it is not full, go to step 4.

Step 3: Determine whether the disparity between the latest frame image and the current latest key frame image in the sliding window reaches the threshold.

If the threshold is reached, the next-newest frame image is a key frame. At this time, the oldest frame in the sliding window needs to be marginalized, that is, the oldest frame image in the sliding window and its corresponding IMU constraint relationship are deleted, and the oldest frame is retained. The visual constraint relationship of the frame image is to store the constraint relationship of the map points observed in the oldest frame on the key frame in the sliding window that has a common view relationship with the oldest frame. Reconstruct the constraint relationship of the sliding window according to Schur's complement theory, and then perform step 5.

If the threshold is not reached, the next-new frame image is not a key frame. At this time, the next-new frame image will be marginalized, that is, the next-new frame image and its visual constraint relationship in the sliding window will be deleted, and the corresponding image of the next-new frame image will be retained. IMU constraint relationship. Reconstruct the constraint relationship of the sliding window according to Schur's complement theory, and then perform step 5.

The content of step 3 implements the update function of key frames in the sliding window. Depending on whether the next new frame is a key frame, it is decided to slide in or slide out to update the sliding window. If the next new frame is a key frame, the sliding window is updated. The oldest frame within the frame is marginalized, that is, it slides out of the update. Figure 3 shows a schematic diagram of the sliding window update when the next new frame is a key frame. If the next new frame is not a key frame, the next new frame will be marginalized, that is, it will be slid into the update. Figure 4 shows a schematic diagram of the sliding window update when the next new frame is not a key frame.

The purpose of updating the sliding window is to add and delete keyframes in the sliding window area while retaining the keyframe constraint relationships. The essence of choosing slide-in update or slide-out update is to make the keyframes in the sliding window have the largest field of view and avoid the narrow and overlapping field of view of these keyframes. The process of deleting a keyframe from the sliding window but still retaining the constraint relationship of the keyframe is called marginalizing the keyframe from the sliding window, and is usually implemented using the Shure complement method.

The state variable χ of all image frames in the sliding window is constructed as a least squares expression in the sliding window and simplified to the form Hδχ = b. H is the system covariance matrix, δχ is the increment of the state quantity, and b is a constant vector. . The state variable χ is divided into two categories, namely the state quantity δχ _d to be marginalized and the state quantity δχ _s to be retained. Split the system covariance matrix H into four block matrices Ha, H _{c ,} H _b , H _a is the covariance matrix of δχ _d , that is, the marginalized variables; H _c is the covariance matrix of δχ _s , that is, the covariance matrix of the retained state variables;/> Represents the transpose of H _b , _which is the covariance matrix between δχ _d and δχ _s .

The state variable χ is expressed as an incremental equation as follows:

Among them, b _d and b _s are the constant vectors of δχ _d and δχ _s respectively.

According to Schur's complement theory, without calculating the state quantity δχ _d to be marginalized, solving the state quantity δχ _s that needs to be retained and performing Gaussian elimination, we can get:

in, Represents the Schur complement of H _a in H _b . According to the above formula, δχ _s can be derived as:

The state quantity in the above formula is only δχ _s , so that the constraint information of the marginalized key frames can be retained, and the state quantity of the marginalized key frames can be eliminated without losing accuracy. In the process of marginalization, although the state variables will be continuously updated, the linearization point is fixed using the optimization consistency strategy, that is, when calculating the marginalization Jacobian matrix, the value of the derivation variable is fixed instead of using each iteration. The Jacobian of the updated state variables is calculated to ensure the non-observable dimensions of the system and avoid changes in the null space of the covariance matrix.

Step 4: Determine whether the latest frame image is a key frame.

If it is a keyframe, it is called the latest keyframe. Project the map points in the sliding window onto the latest frame image, and the map points are map points observed in the key frames in the sliding window that can be co-viewed with the latest frame image, and then perform step 5.

If it is not a key frame, only update the IMU constraint relationship corresponding to the latest frame image, and then return to step 2.

Step 5: Determine whether the projection points on the latest frame image can successfully match the feature points on the latest frame image (two-dimensional feature points in the image plane, extracted using the ORB or SIFT method), that is, the projection on the latest frame image The point matches the descriptor of the feature point. If a projection point cannot find a matching point among the feature points of the latest key frame, then the projection point is discarded; if a projection point can be found among the feature points on the latest key frame Matching points, then determine whether there are original map points that match the feature points of the projection points. The original map points are feature points. Before matching, there are map points that can correspond to them in the sliding window. If so, perform step 6. Otherwise, go to step 7.

Step 6: Compare the observed times of the map points corresponding to the projected points and the original map points corresponding to the feature points, and retain the map points with the most observed times and update the visual observation constraints of each key frame in the sliding window, and delete the observed ones. The map point with fewer times is the map point replacement. Then proceed to step 8.

Step 7: Add a new map point so that an observation constraint is established between the latest frame image and the newly added map point, that is, a new map point is added. Then proceed to step 8.

For steps 5, 6, and 7, all map points that can be observed in all key frames that are co-viewed with the latest key frame are projected onto the latest key frame. Through the relationship between the projected points and the feature points and whether the feature points are original The corresponding map point determines the further processing measures for the map point. As shown in Figure 5, a schematic diagram of the common view relationship between the current key frame and its common view key frame is given. The same map point may be observed at different stages of the mobile robot's movement, or may be created in different threads of the SLAM system, resulting in the phenomenon of "shadow" map points. Projection matching of map points, through map point replacement processing, can filter "shadow" map points, thereby improving the accuracy of map points; through map point addition processing, the field of view of key frame observation map points can be increased, and key points can be increased. The number of frames that successfully match map points, thus increasing the visual observation constraints.

Projection matching of map points is performed based on the camera visual imaging perspective relationship. Figure 6 shows a schematic diagram of the projection matching relationship between the latest key frame in the sliding window and its common view key frame. In the projection calculation, the pose of the key frame is estimated by the SLAM tracking module. Even if the pose accuracy is not high, it does not matter. Candidate matching feature points can be found within the circular neighborhood of the projection point, and then continued through the SLAM backend. Optimize and correct poses. The following formula gives the expression for the perspective projection coordinate transformation of the pinhole camera model from a three-dimensional map point to a two-dimensional image:

In the above formula, [uv 1] ^T is the homogeneous coordinates of the projection point in the pixel coordinate system on the imaging plane, u and v are the horizontal and vertical coordinates of the projection point in the pixel coordinate system respectively. f _x and f _y are the focal lengths of the camera in the horizontal and vertical directions respectively, and c _x and _{cy are} respectively the number of horizontal and vertical pixels that differ from the center pixel coordinates of the image to the image origin pixel coordinates. Z is the depth value in the optical axis direction when transforming from the camera coordinate system to the normalized coordinate system, R is the rotation matrix of the mobile robot, t is the translation vector of the mobile robot, [x _w y _w z _w 1] ^T is the map The homogeneous coordinates of the point in the world coordinate system, x _w , y _w , and z _w are the coordinate values of the three axes in the world coordinate system respectively, K is the camera internal parameter matrix, and T represents the transformation matrix of the camera pose.

Step 8: Determine whether the number of key frames in the sliding window is full. If it is not full, return to step 2; if it is full, go to step 9.

Step 9: Based on the visual reprojection perspective relationship, construct the visual constraint factors between the mobile robot's pose and the map points. According to the IMU (Inertial Measurement Unit) pre-integration theory, the IMU pre-integration constraint factor is constructed. Based on the marginalization processing results of the updated sliding window, a priori constraint factors are constructed. The visual constraint factor, IMU pre-integration constraint factor and marginalization prior constraint factor are added to the sliding window factor graph optimization model to construct the objective function. Figure 7 shows a schematic diagram of the sliding window factor graph optimization model constructed from three constraint factors.

Based on the sliding window factor graph optimization model, a tightly coupled beam adjustment optimization objective function is established, and the maximum posterior probability of the state is obtained by minimizing the sum of visual constraint residuals, IMU pre-integration constraint residuals, and marginalized prior constraint residuals. Estimate, in the form of:

State variables of all frame images in the sliding window k∈[0,N], N is the total number of all image frames in the sliding window, λ _m is the inverse depth of the m-th map point, m is the total number of map points observed in the key frame image, and the objective function expression is as follows:

Among them, the state vector of the kth frame and/> are the position, speed and attitude of the camera in the k-th frame respectively, b _a and b _ω are the accelerometer offset and gyroscope offset of the IMU respectively, and/> are the position and rotation of the camera relative to the IMU respectively, e _C (x _i ,x _j ), e _B (x _i ,x _j ) and e _M respectively represent the visual constraint residual, IMU pre-integration constrained residual and marginalization first Experimentally constrained residual, x _i and x _j are the state vectors of the i-th frame image and j-th frame image respectively.

The visual constraint factor is constructed based on the reprojection error. A map point in space is projected onto each key frame where the map point can be observed. The coordinates of the projected point and the observed coordinates of the map point should coincide. The two types of coordinates The reprojection error is the residual error of the visual constraint factor. In the visual-inertial SLAM system, the state quantity χ _c of the visual factor to be optimized is expressed as:

in, and/> are the positions of the camera in the i-th frame and j-th frame respectively,/> and/> are the postures of the camera in the i-th frame and j-th frame respectively, and the i-th frame and j-th frame are two adjacent key frames,/> and/> are the position and rotation amount of the camera relative to the IMU respectively, and λ is the inverse depth of the map point.

Suppose there is a map point P in the space, its index number is l, and its projected coordinates on the normalized plane of the jth frame are The observation coordinates at the jth frame are/> Then the expression of the visually constrained residual e _C (x _i ,x _j ) is as follows:

in, It can be obtained from the reprojected estimation model, and the expression is:

In the above formula, Represents the observation value on the normalized plane of the i-th frame when the map point P is first observed in the i-th frame,/> is the rotation amount from the IMU coordinate system to the camera coordinate system,/> is the amount of rotation from the world coordinate system to the j-th frame IMU coordinate system,/> is the rotation amount from the IMU coordinate system to the world coordinate system in the i-th frame,/> is the amount of rotation from the camera coordinate system to the IMU coordinate system.

The IMU pre-integration constraint factor is used to describe the relative pose change relationship of the IMU measurement information between the time differences of two key frames. Align the i time and j time sampled by the IMU with the k-th frame and k+1-th frame of the image, then the variable to be optimized of the IMU pre-integration factor is expressed as:

in, and/> are the speeds of the IMU at time t _i and time t _j respectively,/> and/> are the accelerometer bias of the IMU at time t _i and time t _j respectively,/> and/> are the gyroscope offsets of the IMU at time t _i and time t _j respectively, and time t _i and time t _j are the times when the camera captures the i-th frame and the j-th frame respectively.

The position of the IMU at time t _j Speed/> and posture/> Obtained by the following formula:

Among them, Δt is the time interval between the i-th frame and the j-th frame, t _i ≤ t ≤ t _j , is the rotation matrix from the IMU coordinate system to the world coordinate system, g _w is the gravity vector in the world coordinate system, δt is the time interval between two adjacent IMU measurement data, /> is the rotation amount of the carrier relative to time t i in the IMU coordinate system, taking the position of the i-th frame time as a reference at time _t .

and/> are the accelerometer bias and gyroscope bias of the IMU at time t, respectively.

and/> Represent respectively the acceleration and angular velocity output by the IMU at time t,/> and/> represent the acceleration noise and angular velocity noise of the IMU measurement information at time t respectively, and assume that they are both white noise obeying Gaussian distribution. Transform the above formula and transform the reference coordinate system of position, velocity and rotation from the world coordinate system to the body coordinate system of the i-th frame, we can get:

In the above formula That is, the IMU pre-integrated variables of position, velocity and attitude, the expression is as follows:

Represents the IMU pre-integrated variable of the robot attitude at time t relative to time t _i .

It can be seen that the state quantity at the j-th frame is only related to the measured value of the IMU and will not be affected by the state quantity at the i-th frame, thus ensuring that the measured value has nothing to do with the state quantity and avoiding repeated integration. Then the expression of IMU pre-integration constrained residual e _B (x _i ,x _j ) is as follows:

Among them, r _p , r _q , r _v , are the position residual, attitude residual, speed residual, accelerometer residual and gyroscope residual of the robot from time t _i to time t j respectively. The time t _i and time _{t j} are respectively the i-th frame image taken by the camera. and the moment of the jth frame image,/> is the quaternion multiplication symbol, [·] _xyz means taking the imaginary part of the quaternion to form a three-dimensional vector, /> is the amount of rotation from the world coordinate system to the IMU coordinate system at time t _i (quaternion representation),/> is the rotation amount (quaternion representation) from the IMU coordinate system at time t _i to the IMU coordinate system at time t _j .

The a priori constraint factor is the constraint relationship retained after eliminating marginalized key frames when updating the sliding window. A least squares expression is constructed using a factor graph optimization model. The state vector χ is set as a node. The pose information in the marginalized state vector χ _d and the constraint relationship between the map points are added to the retained information in the form of a priori information. In the optimization relationship of the state vector χ _s , the loss of the constraint information corresponding to the marginalized key frames after updating the sliding window is avoided. After the marginalization process, the obtained prior matrix H _p and the corresponding prior residual amount b _p are consistent with the marginalization expression in step 3. The marginalization prior constraint residual e _M is expressed The formula is:

e _M =b _p -H _p δχ _1:N ,

Among them, δχ _1:N is the state increment of all key frames in the sliding window,

H _a is the covariance matrix of the state vector δχ _d to be marginalized, H _c is the covariance matrix of the retained state vector δχ _s , Represents the transpose of H _b , H _b is the covariance matrix between δχ _d and δχ _s , b _d and b _s are the constant vectors of δχ _d and δχ _s respectively.

Step 10: Optimize the objective function using a nonlinear optimization algorithm, such as Gauss-Newton method, Levenberg-Marquadt method, etc. Taking the Levenberg-Marquadt method as an example, by setting a confidence area for the state increment, the approximation failure caused by large changes in the state increment is avoided, and the non-singular and ill-conditioned coefficient matrix of the linear equation system is avoided. Therefore, it can Provides more stable and accurate status increments. Construct the objective function into the form of a state increment equation:

(H _h +μI)Δχ=g,

In the above formula, H _h is the Hessian matrix (Hessian matrix), μ is the Lagrange multiplier (Lagrange multiplier), and the size of the parameter μ is adjusted to reflect whether the nonlinear quadratic approximation model is accurate, and then iteratively Method optimization to obtain the optimal solution.

Update the poses of all key frames in the sliding window, complete a sliding window optimization, and determine whether the optimization thread of the SLAM backend has been suspended. If not, return to step 2; if it is suspended, end.

This implementation method improves the sliding window optimization method through the common view projection matching relationship without changing the speed of the sliding window optimization, eliminates "shadow" map points, improves the accuracy of the map points, and increases the probability that key frames successfully match map points. The number expands the observation field of view of key frames and adds visual observation constraints of the SLAM system, thereby improving the accuracy of sliding window optimization estimation poses and thus improving the positioning accuracy of mobile robots.

Implementation and verification of the invention:

The present invention proposes a visual-inertial SLAM method based on common-view projection matching to improve sliding window optimization, which is implemented using C++ language programming. The beneficial effects of the present invention are verified by evaluating the accuracy of the mobile robot's motion trajectory estimated by the SLAM system. Based on the visual-inertial SLAM system-VINS-Mono framework, the VINS-Mono using the improved sliding window optimization method proposed in this invention is named our-VINS-Mono. The EuRoc flight data set published by ETH Zurich is used as a verification data set. This data set uses a micro-rotor aircraft as a carrier and is equipped with a visible light camera sensor and an inertial measurement unit to sense surrounding environment information and measure the aircraft's own motion attitude.

Taking the Machine Hall 01 data subset in the EuRoc flight data set as an example, VINS-Mono and our-VINS-Mono are used to estimate the flight trajectory of the rotorcraft in the Machine Hall 01 data set, and then different evaluation indicators are used to evaluate the accuracy of the two trajectories. to evaluate. Figure 8 shows the UAV flight trajectory estimated by our-VINS-Mono on the Machine Hall 01 data set. Figure 9 shows the flight trajectory in Figure 8 on the three coordinate axes of x, y, and z. The relationship curve between the displacement change and time; Figure 10 is the relationship between the APE error curve and time using the absolute pose error index (APE) to evaluate the flight trajectory in Figure 8, and the root mean square error of the global trajectory is given ( rmse), error median (median), error mean (mean) and error standard deviation (std). Figure 11 is a comparison of the APE error box plots of the flight trajectories estimated by VINS-Mono and our-VINS-Mono on the Machine Hall 01 data set. From the figure, we can see the median APE error of our-VINS-Mono. The number is obviously lower than that of VINS-Mono, and the error partition moves downward as a whole. Figure 12 is a comparison chart of the flight trajectories estimated by VINS-Mono and our-VINS-Mono on the Machine Hall 01 data set, and the APE error amount under different error evaluation indicators. It can be seen from the figure that our-VINS-Mono The APE errors under the max (maximum error value), std, median, mean, and rmse indicators are all smaller than VINS-Mono, while the APE error under the min (minimum error) indicator is larger, indicating that the overall error tends to be in the middle Near the number, outliers have less impact on the overall error. Figure 13 is a comparison of the flight trajectories estimated by VINS-Mono and our-VINS-Mono on the Machine Hall 01 data set. The density distribution of the trajectory error and the relationship between the APE error. From the figure, we can see that our-VINS-Mono The distribution of the error amount is relatively concentrated, which is consistent with the error concentration in the median in the box plot.

In summary, the present invention proposes a visual inertial SLAM method based on common-view projection matching to improve sliding window optimization, aiming to optimize the position accuracy of map points by performing projection matching on common-view map points of key frames within the sliding window. , eliminate ghost map points, increase observation constraints between key frames and map points, and improve the pose estimation accuracy of the visual-inertial SLAM system, thereby improving the positioning accuracy of the mobile robot. In the sliding window, all map points that can be observed in all key frames that have a common view relationship with the latest key frame are projected onto the latest key frame, and the matching relationship between each projection point and the feature point is judged, and the map point is replaced by , eliminate "shadow" map points and avoid redundant storage; through the addition of map points, the number of successful matches between key frames and map points is increased, and the observation field of view of key frames is expanded. In addition, the present invention does not change the size of the sliding window, and there are still a fixed number of key frames in the sliding window, which can ensure the speed of the calculation time.

Although the present invention is described herein with reference to specific embodiments, it is to be understood that these embodiments are merely exemplary of the principles and applications of the invention. It is therefore to be understood that many modifications may be made to the exemplary embodiments and other arrangements may be devised without departing from the spirit and scope of the invention as defined by the appended claims. It is to be understood that the features described in the different dependent claims may be combined in a different manner than that described in the original claims. It will also be understood that features described in connection with individual embodiments can be used in other described embodiments.

Claims (10)

1. The visual inertia SLAM system sliding window optimization method based on common view projection matching is characterized by comprising the following steps of:

step one: when the back end module of the visual inertia SLAM system subscribes to the latest frame image of the robot published by the front end module, judging whether the sliding window is full, if yes, executing the second step, otherwise, executing the fourth step;

step two: judging whether the next new frame image is a key frame or not, if so, marginalizing the oldest frame image in the sliding window and then executing the step III, otherwise, marginalizing the next new frame image and then executing the step III;

step three: reconstructing the constraint relation of the sliding window according to the Shu's complement theory, and then executing the fourth step;

step four: judging whether the latest frame image is a key frame according to the parallax of the latest frame image and the current latest key frame image in the sliding window, if so, projecting map points in the sliding window onto the latest frame image, wherein the map points are observed map points of the key frame which can be viewed together with the latest frame image in the sliding window, executing a fifth step, otherwise, updating an IMU constraint relation corresponding to the latest frame image, and returning to the first step;

step five: matching the projection points on the latest frame image with the feature points, deleting the projection points which are not matched with the feature points, judging whether the feature points with the matched projection points have original map points or not, wherein the original map points are map points which can be corresponding to the feature points in a sliding window before the feature points are matched, if so, executing the step six, otherwise, executing the step seven;

step six: comparing the observed times of the map points corresponding to the projection points and the original map points corresponding to the feature points, only reserving the map points with more observed times, updating the visual observation constraint of each key frame in the sliding window, and then executing the step eight;

step seven: adding new map points, establishing observation constraint between the latest frame image and the newly added map points, and executing the step eight;

step eight: judging whether the number of key frames in the sliding window is full, if yes, executing a step nine, otherwise, returning to the step one;

step nine: respectively constructing a visual constraint factor, an IMU pre-integral constraint factor and an marginalized prior constraint factor, and adding the visual constraint factor, the IMU pre-integral constraint factor and the marginalized prior constraint factor into a sliding window factor graph optimization model to construct an objective function;

step ten: and optimizing the objective function by using a nonlinear optimization algorithm, updating the state quantity of the robot in the sliding window, and completing the sliding window optimization.

2. The method for optimizing a sliding window of a visual inertial SLAM system based on co-view projection matching according to claim 1, wherein in the second step, the step of marginalizing the oldest frame image in the sliding window comprises:

deleting the oldest frame image and the IMU constraint relation corresponding to the oldest frame image in the sliding window, and reserving the visual constraint relation of the oldest frame image;

the step two, the marginalizing of the image of the next new frame includes:

deleting the next new frame image and the visual constraint relation thereof, and reserving the IMU constraint relation corresponding to the next new frame image.

3. The method for optimizing the sliding window of the visual inertial SLAM system based on co-view projection matching according to claim 1, wherein the reconstructing the constraint relation of the sliding window according to the schulplement theory in the step three includes:

the state variable χ of all image frames within the sliding window is expressed in terms of an incremental equation as follows:

wherein δχ _d For the state vector to be marginalized δχ _s To preserve state vector, H _a Is delta chi _d Covariance matrix of H _c Is delta chi _s Is used for the co-variance matrix of (a),represents H _b Transpose of H _b Is delta chi _d And delta chi _s Covariance matrix between b _d And b _s Delta chi respectively _d And δχ _s A constant vector;

and carrying out Gaussian elimination on an increment equation of a state variable χ according to a Shuerbu theory, and deducing to obtain:

further, a reserved state vector delta chi is obtained according to the above method _s And realizing the reconstruction of the constraint relation of the sliding window.

4. The method for optimizing sliding window of visual inertial SLAM system based on co-view projection matching according to claim 1, wherein in step five, if the projection point on the latest frame image matches with the descriptor of the feature point, the projection point matches with the feature point.

5. The method for optimizing sliding window of visual inertial SLAM system based on co-view projection matching according to claim 1, wherein in step nine, the state quantity χ to be optimized of the visual constraint factor _c The method comprises the following steps:

wherein,and->The camera is at the position of the i-th frame and the j-th frame, respectively,>and->The pose of the camera in the ith frame and the jth frame are respectively, wherein the ith frame and the jth frame are two adjacent key frames, < ->And->The position and rotation of the camera relative to the IMU are respectively shown, and lambda is the inverse depth of the map point.

6. The co-view projection matching based visual inertia of claim 5The SLAM system sliding window optimization method is characterized in that in the step nine, the variable χ to be optimized of the IMU pre-integration constraint factor _imu The method comprises the following steps:

wherein,and->Respectively t _i Time sum t _j Speed of IMU at time, +.>And->Respectively t _i Time sum t _j Accelerometer bias of moment IMU, +.>And->Respectively t _i Time sum t _j Gyro offset of IMU at moment, t _i Time sum t _j The moments are the moments when the camera shoots the ith frame image and the jth frame image, respectively.

7. The method for optimizing a sliding window of a visual inertial SLAM system based on co-view projection matching according to claim 1, wherein in step nine, the objective function expression is as follows:

wherein χ is the state variable of all image frames in the sliding window and hask∈[0,N]，λ _m For the inverse depth of the mth map point, m is the total number of map points observed by the key frame images, N is the total number of all frame images in the sliding window, and the state vector of the kth frame is-> And->B is the position, speed and attitude of the camera at the kth frame, respectively _a And b _ω The accelerometer bias and the gyroscope bias of the IMU, and->The position and rotation amount of the camera relative to the IMU, e _C (x _i ,x _j )、e _B (x _i ,x _j ) And e _M Respectively representing a vision constraint residual error, an IMU pre-integral constraint residual error and an marginalized prior constraint residual error, and x _i And x _j The state vectors of the i-th frame image and the j-th frame image, respectively.

8. The method for optimizing sliding window of visual inertial SLAM system based on co-view projection matching according to claim 7, wherein the visual constraint residual e _C (x _i ,x _j ) The expression is as follows:

wherein,for the projection coordinates of the map point P in space on the normalized plane of the jth frame of image, +.>Is the observed coordinate of the map point P in the space when the map point P is observed for the first time in the j-th frame image.

9. The method for optimizing a sliding window of a visual inertial SLAM system based on co-view projection matching according to claim 7, wherein the IMU pre-integration constraint residual e _B (x _i ,x _j ) The expression is:

wherein r is _p 、r _q 、r _v 、Respectively t _i From time to t _j Position residual error, attitude residual error, speed residual error, accelerometer residual error and gyroscope residual error of the moment robot, wherein t is as follows _i Time sum t _j The moments are the moments when the camera shoots the ith frame image and the jth frame image, respectively.

10. The method for optimizing sliding window of visual inertial SLAM system based on co-view projection matching according to claim 7, wherein the marginalized a priori constraint residual e _M The expression is:

e _M ＝b _p -H _p δχ _1:N ，

wherein δχ _1:N For the state delta of all key frames within the sliding window,

H _a for the state vector δχ to be marginalized _d Covariance matrix of H _c For the reserved state vector δχ _s Is used for the co-variance matrix of (a),represents H _b Transpose of H _b Is delta chi _d And delta chi _s Covariance matrix between b _d And b _s Delta chi respectively _d And δχ _s Constant vector.