SLAM method and system based on graph optimization and EKF framework integrating monocular vision and IMU

Abstract

The present invention provides a SLAM method and system that integrates monocular vision and IMU under a graph optimization and EKF framework. The method includes: collecting images and inertial data through a camera and an inertial measurement unit (IMU), respectively, and feeding the images and inertial data into a monocular REKF-VIO system with preset frequency parameters; performing state prediction, update, and state expansion operations on the monocular REKF-VIO system to estimate the state posterior mean and system covariance of the current frame; initializing the monocular REKF-VIO system to construct initial map points of an initial global map; performing screening processing based on the initial map points to obtain visual key frames; based on the visual key frames, using graph optimization and an EKF complementary framework to obtain an optimized global map and motion state information; feeding the motion estimation information into the optimized global map to perform local map updates and closed-loop optimization. The present invention solves the technical problems of low front-end image frame processing efficiency, error interference, and low precision.

Description

Technical Field

[0001] The present invention relates to the field of real-time positioning and mapping of indoor mobile robots, and in particular to a SLAM method and system based on graph optimization and the fusion of monocular vision and IMU under the EKF framework. Background Art

[0002] With the continuous advancement of computer technology, artificial intelligence (AI) is a key area of ​​future development, encompassing hot research areas such as graphics and image processing, computer vision, and pattern recognition. SLAM (Simultaneous Localization and Mapping) technology provides an effective solution for indoor autonomous mobile robot applications.

[0003] Indoor positioning suffers from GPS positioning accuracy degradation. Monocular cameras combined with IMU sensors can serve as a GPS alternative, not only due to their compactness, portability, and affordability, but also due to their complementary nature in pose estimation. Monocular vision-based SLAM (Simultaneous Local Area Mapping) provides good motion estimation and mapping information in texture-rich environments, but its robustness is limited, and monocular vision suffers from scale uncertainty. Complementarily, IMUs (Instrumental Units) output the body's own acceleration and angular velocity information at high frequency, unaffected by environmental factors, and robustly provide inter-frame motion estimation and absolute scale information. However, long-term IMU operation inevitably leads to cumulative errors. Therefore, tightly coupling IMU measurement information to monocular vision-based SLAM can improve the overall robustness and positioning accuracy of the system. In practical applications, for autonomous mobile robots indoors to successfully complete their assigned tasks, it is necessary to provide them with motion estimates with low latency and high accuracy.

[0004] VINS-mono is an open-source monocular vision-based SLAM solution from the Hong Kong University of Science and Technology based on a tightly coupled sliding window nonlinear optimization method. This solution uses Harris corners and LK (Lucas-Kanade) optical flow tracking on the front end, tightly coupled vision and IMU fusion, and a sliding window nonlinear optimization method for pose correction. This existing technology is a relatively excellent and comprehensive system framework among current VIO solutions. However, the efficiency of processing each frame on the front end could be further improved.

[0005] MSCKF, short for Multi-State Constraint Kalman Filter, is a filtering-based VIO algorithm. It integrates IMU and visual information within the EKF framework. Compared to simple VO algorithms, MSCKF can adapt to more intense motion and temporary texture loss. However, this existing technology suffers from high computational complexity and requires platforms with abundant computing resources. As a result, its positioning accuracy is generally lower than that of optimization-based solutions.

[0006] ORB_SLAM3 is a SLAM system that supports vision, visual-inertial fusion, and hybrid maps. It can run on monocular, binocular, and RGB-D cameras using pinhole or fisheye camera models. It is the first tightly coupled feature-based VIO system that relies only on maximum a posteriori estimation (even during IMU initialization). The front-end processing efficiency of this technology can be further optimized per frame.

[0007] For example, the existing invention patent document "Indoor autonomous positioning method based on tight coupling of visual point and line features and IMU" with publication number CN111595334A overcomes the constraints of relying solely on point features in structured architectural environments and achieves autonomous positioning in indoor areas such as corridors with weak textures and drastic light changes by adding rich line features in structured architectural environments as visual constraints; it also performs graph optimization by adding a point and line filtering extraction strategy and a four-parameter orthogonal representation of line features to perform feature matching; and this existing solution introduces a mechanism for tight coupling of IMU data and visual point and line features. For example, the existing invention patent document "Invariant Central Difference Filter Method for Visual SLAM of Autonomous Mobile Robots" with publication number CN110610513A designs a central difference filter calculation method for matrix Lie group vector space and invariant Kalman filter. The filter state variables are composed of robot posture, velocity, 3D landmark position vector and accelerometer and gyroscope deviation vector represented by SE(3) Lie group vector. The mean value and error of Sigma sampling point of CDKF filter are designed in matrix Lie group space. The inverse depth observation model of motion image feature points is constructed using monocular vision camera. The prediction and update iterative calculation of CDKF central difference filter are designed to carry out robot positioning and map construction calculation tasks. From the specific implementation content of the above existing solutions, it can be seen that since the existing filtering-based method has the shortest calculation time for processing each frame and can realize real-time motion estimation in the system, it can be used as the front-end input data processing for each frame image. However, such existing methods are affected by linearization errors, resulting in low estimation accuracy.

[0008] In summary, the existing technology has technical problems such as low front-end image frame processing efficiency, error interference and low accuracy. Summary of the Invention

[0009] The technical problem to be solved by the present invention is how to solve the technical problems of low efficiency, error interference and low precision in front-end image frame processing.

[0010] The present invention solves the above technical problems by adopting the following technical solutions: A SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework includes:

[0011] S1, collect images and inertial data through the camera and inertial measurement unit IMU respectively, and send the images and inertial data to the monocular REKF-VIO system with preset frequency parameters;

[0012] S2. Perform state prediction, update, and state expansion operations on the monocular REKF-VIO system to estimate the state posterior mean and system covariance of the current frame, wherein step S2 includes:

[0013] S21, using the monocular REKF-VIO system to perform monocular REKF-VIO state prediction based on the image and inertial data;

[0014] S22, performing an update operation on the monocular REKF-VIO system to estimate the posterior mean of the current frame state and the current frame covariance;

[0015] S23, when the initial global map of the monocular REKF-VIO system is not established, executing a preset state expansion logic to expand the system state posterior mean and system covariance of the monocular REKF-VIO system;

[0016] S3, initialize the monocular REKF-VIO system to construct the initial map points of the initial global map;

[0017] S4, performing screening processing based on the initial map points to obtain visual key frames;

[0018] S5. Based on the visual keyframes, the graph optimization and EKF complementary framework are used to obtain the optimized global map and motion state information. Step S5 also includes:

[0019] S51, sending the visual key frame to a preset global map auxiliary EKF feedback mechanism module to perform EKF state correction on the visual key frame;

[0020] S52, performing EFK map correction based on the visual keyframes, optimizing the initial global map using a graph optimization method to obtain an optimized global map, and then updating and expanding the state vector of the monocular REKF-VIO system based on the map to obtain motion estimation information;

[0021] S6. Send the motion estimation information to the optimized global map, and perform local map update and closed-loop optimization based on it.

[0022] This paper adopts a complementary framework that integrates graph optimization and EKF, allowing the system to improve front-end computational efficiency while maintaining good pose estimation accuracy. The proposed global map-assisted EKF feedback mechanism effectively utilizes the globally consistent map constructed by the system, eliminating the influence of linearization errors in existing technologies and improving the motion estimation accuracy of monocular REKF-VIO.

[0023] The present invention improves the positioning accuracy of indoor autonomous mobile robots in three-dimensional space while ensuring rapid algorithm response, can further improve the real-time performance and positioning accuracy of SLAM systems, and provide technical guarantees for indoor autonomous mobile robots to complete specific tasks.

[0024] In a more specific technical solution, step S21 includes:

[0025] S211, assuming that the current frame observes a map point, the state vector of the monocular REKF-VIO system is expressed as follows using the following logic:

[0026]

[0027] Where, Indicates the IMU state of the current frame, Indicates the status of a map point. Represents the rotation from the IMU coordinate system to the world coordinate system, Represent the position and velocity of the IMU coordinate system relative to the world coordinate system, are the zero bias of the gyroscope and accelerometer, Indicates the position of the lth map point in the world coordinate system expressed in inverse depth coordinates;

[0028] S212, using the inertial measurement unit IMU to measure its own angular velocity and acceleration ω relative to its own body coordinate system m and a m :

[0029] ω m =ω B +b g +n g

[0030]

[0031] Where, Represents the transformation matrix between the world coordinate system and the IMU coordinate system, g w is the known gravity vector in the world system, b g , b a is the sensor zero bias, n g , n a is zero-mean Gaussian white noise.

[0032] S213. Pre-score the inertial measurement unit (IMU) using the following logic:

[0033]

[0034]

[0035]

[0036] Where, They are the position, velocity and attitude quaternions of the inertial measurement unit IMU from to time, is quaternion multiplication, For acceleration observation, is the rotation matrix, Δt is the prediction time;

[0037] S214, multiply both sides of the above pre-integration equation by The following equation is obtained:

[0038]

[0039]

[0040]

[0041] In the above formula:

[0042]

[0043]

[0044]

[0045] and is the IMU pre-integration term that does not depend on the initial state.

[0046] The present invention reduces the influence of sensor zero bias and zero-mean Gaussian white noise on the self-angular velocity and acceleration and other measurement values ​​relative to the IMU body coordinate system through IMU pre-integration.

[0047] The present invention aims to solve the problem that the VIO algorithm in the prior art has to be recalculated each time the state changes. The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the need for initial rotation This solves the problem of excessive computation and improves the efficiency of image data processing.

[0048] In a more specific technical solution, S22 includes:

[0049] S221. Use inverse depth coordinates to represent the position of the lth map point And use the following logic to convert the lth map point position into the corresponding Euclidean XYZ coordinates

[0050]

[0051] S222: Based on the preset camera projection model, use the following logic to obtain the visual observation model of the lth map point on the current frame image plane:

[0052]

[0053] Where, Represents the actual observation value of the lth map point in the current frame, represents the visual observation noise,

[0054] Represent the rotation and translation between the camera coordinate system and the IMU coordinate system respectively;

[0055] S223, the state prior mean in the current frame At , the following first-order Taylor expansion logic is used to linearize the visual observation model to obtain the observation value on the current frame image plane

[0056]

[0057] Where, and J sl Respectively Relative to IMU status The lth map point location and the Jacobian matrix of the state vector, is the measurement matrix;

[0058] S224: Using the following logic, the observation value of the lth map point on the current frame image plane can be obtained: and the corresponding predicted value The measurement error and linearization estimate between:

[0059]

[0060] S225, the observation data of all map points in the state vector of the current frame is By stacking all measurement errors, measurement matrices, and visual observation noise To obtain the update equation of the state posterior mean and system covariance of the current frame:

[0061]

[0062]

[0063]

[0064] Where, and

[0065] In a more specific technical solution, step S23 includes:

[0066] S231. When the initial global map of the system is not established, assume that the current frame uses a new map point to expand the state vector, then its initial inverse depth coordinate is:

[0067]

[0068] S232. Based on the posterior mean of the IMU state of the current frame, the initial inverse depth coordinate camera position of the new map point is calculated using the following logic:

[0069]

[0070] S233, based on the observation value of the new map point in the current frame Use the following logic to calculate the initial inverse depth coordinate azimuth and elevation of the new map point:

[0071]

[0072] S234, determining parameters based on initial inverse depth Get the initial inverse depth coordinates and inverse depth values ​​of the new map point:

[0073]

[0074] S235. Based on the initial inverse depth coordinate and inverse depth value of the new map point, expand the system state posterior mean using the following logic:

[0075]

[0076] S236, using the following first-order Taylor expansion logic, respectively in the current frame inertial measurement unit IMU state posterior mean The actual observation value of the map point in the current frame And the parameters that determine the initial inverse depth value At the inverse depth coordinate of the new map point Doing linear estimation we get:

[0077]

[0078] In the above formula:

[0079]

[0080] This formula determines the parameter noise of the initial inverse depth value, and The status of the new map point Relative to the IMU state The Jacobian matrix of the observation value, the inverse depth value of the map point, and the state vector;

[0081] S237. Based on the Jacobian matrix, the expanded form of the state covariance matrix is ​​obtained using the following logic:

[0082]

[0083] The present invention's inverse depth coordinate representation of map point positions not only allows for a better Gaussian representation of the positional uncertainty of all near and far landmarks, but also enables delay-free feature initialization. The measurement model represented in inverse depth coordinates is highly linear across all parallaxes, thus providing more accurate motion estimation.

[0084] In a more specific technical solution, step S3 includes:

[0085] S31, extracting a preset number of ORB visual point features from the image of the current frame, and matching them with the visual point features of the reference frame;

[0086] S32. When the visual point feature is greater than a preset logarithmic threshold, the disparity of each visual point feature matching pair is calculated according to the pose of the reference frame and the current frame, and a group F of visual point feature matching pairs with applicable disparity is selected.

[0087] S33, when the number of visual point feature pairs in the applicable parallax visual point feature matching pair group F is greater than a preset logarithm threshold, triangulating all visual point feature matching pairs in the applicable parallax visual point feature matching pair group F according to the poses of the reference frame and the current frame to obtain a triangulated initial global map point number;

[0088] S34, when the number of triangulated initial global map points is greater than the preset map point threshold, the reference frame and the current frame are

[0089] Set as a key frame, combine it with the initial global map points to form the initial global map, and start optimizing the initial global map.

[0090] The present invention constructs an initial global map with scale information through system initialization, and the initialization of the global map is performed after the monocular REKF-VIO state converges, thereby reducing the impact of the accuracy of the initial global map on the accuracy of subsequent state estimation.

[0091] In a more specific technical solution, in step S4, the time interval and rotation angle between the current frame and the previous key frame are compared with a given threshold to select a visual key frame.

[0092] The present invention utilizes the comparison and selection of the interval between frames and the rotation angle to ensure the accuracy and integrity of the global map and ensure that the system constructs as many key frames as possible to improve the accuracy of the global map.

[0093] In a more specific technical solution, step S51 includes:

[0094] S511, using the state posterior mean of the inertial measurement unit IMU of the current frame As the initial value, use the following logic to solve the preset factor graph and optimize the IMU state of the current frame to obtain the optimized estimated value of the IMU state of the current frame

[0095]

[0096] Where, Represents the IMU pre-integration residual between the previous keyframe and the current frame, Represents the reprojection residual between the corresponding map point and the current frame of the visual point feature;

[0097] S512, optimize the estimated value according to the current frame IMU state and covariance Use the following logic to update the state mean and covariance of the monocular REKF-VIO system:

[0098]

[0099]

[0100]

[0101] Where, Is the IMU status Jacobian matrix with respect to the state vector. The measured value of the IMU state in the current frame and predicted value The error between them is linearized.

[0102] In a more specific technical solution, step S52 includes:

[0103] S521, according to the current frame IMU state estimation value obtained by optimization and the estimated location of the map point The inverse depth parameter of a map point is calculated using the following logic:

[0104]

[0105] S522, performing linearization processing on the inverse depth parameter of the map point to obtain the inverse depth parameter variance of the map point;

[0106] S523, perform EKF state correction according to the monocular REKF-VIO system to obtain the posterior mean of the IMU state of the current frame Observation value of the map point in the current frame image and the initial inverse depth parameter The calculated initial inverse depth coordinates of the map point And expand the state mean of the monocular REKF-VIO system;

[0107] S524, perform EKF state correction according to the monocular REKF-VIO, and obtain the state posterior covariance Observation covariance of map points in the current frame image And the initial inverse depth value of the map point determines the parameter standard deviation Expand the state covariance matrix of monocular REKF-VIO.

[0108] The present invention provides drift-free motion estimation of the map points maintained in the state vector based on the EKF monocular VIO. The map point position in the monocular REKF-VIO state vector is consistent with the global motion of the system. Figure 1 When the positioning accuracy is improved,

[0109] In a more specific technical solution, the inverse depth parameter of the map point is linearized by the following first-order Taylor expansion to obtain the variance of the inverse depth parameter of the map point:

[0110]

[0111] Where, and Represents the linearized estimate of the inverse depth parameter relative to the current frame IMU state and map point locations The Jacobian matrix of and They represent the covariance of the optimized IMU state estimate of the current frame and the corresponding map point position estimate.

[0112] In a more specific technical solution, the SLAM system that integrates monocular vision and IMU under the framework of graph optimization and EKF includes:

[0113] The data acquisition module is used to collect images and inertial data through the camera and inertial measurement unit (IMU) respectively, and send the images and inertial data to the monocular REKF-VIO system with preset frequency parameters;

[0114] The prediction expansion module is used to perform state prediction, update, and state expansion operations on the monocular REKF-VIO system to estimate the state posterior mean and system covariance of the current frame. The prediction expansion module is connected to the data acquisition module, wherein the prediction expansion module includes:

[0115] A state prediction module, which uses the monocular REKF-VIO system to perform monocular REKF-VIO state prediction based on image and inertial data;

[0116] The state update module is used to perform an update operation on the monocular REKF-VIO system to estimate the posterior mean of the current frame state and the current frame covariance;

[0117] A state covariance expansion module is used to execute preset state expansion logic to expand the system state posterior mean and system covariance of the monocular REKF-VIO system when the initial global map of the monocular REKF-VIO system is not established. The state covariance expansion module is connected to the state prediction module and the state update module;

[0118] Map point initialization module, used to initialize the monocular REKF-VIO system to construct the initial map points of the initial global map;

[0119] A key frame acquisition module is used to perform screening processing based on the initial map points to obtain visual key frames. The key frame acquisition module is connected to the map point initialization module;

[0120] The graph optimization and EKF complementary framework module is used to obtain an optimized global map and motion state information based on the visual keyframes using the graph optimization and EKF complementary framework. The graph optimization and EKF complementary framework module is connected to the keyframe acquisition module and further includes:

[0121] An EKF state correction module is used to send the visual keyframes to a preset global map-assisted EKF feedback mechanism module to perform EKF state correction on the visual keyframes;

[0122] The EFK map correction and motion estimation information acquisition module is used to perform EFK map correction based on the visual keyframes. The initial global map is optimized using a graph optimization method to obtain an optimized global map. The state vector of the monocular REKF-VIO system is then updated and expanded to obtain motion estimation information. The EFK map correction and motion estimation information acquisition module is connected to the EKF state correction module.

[0123] The local map update optimization module is used to send the motion estimation information into the optimized global map, and perform local map update and closed-loop optimization based on it. The local map update optimization module is connected to the EFK map correction and motion estimation information acquisition module.

[0124] This invention offers the following advantages over existing technologies: Its proposed framework, which integrates graph optimization and EKF, allows the system to improve front-end computational efficiency while maintaining good pose estimation accuracy. Its proposed global map-assisted EKF feedback mechanism, by effectively utilizing the system's globally consistent map, eliminates the influence of linearization errors in existing technologies and improves the motion estimation accuracy of the monocular REKF-VIO.

[0125] The present invention improves the positioning accuracy of indoor autonomous mobile robots in three-dimensional space while ensuring rapid algorithm response, can further improve the real-time performance and positioning accuracy of SLAM systems, and provide technical guarantees for indoor autonomous mobile robots to complete specific tasks.

[0126] The present invention reduces the influence of sensor zero bias and zero-mean Gaussian white noise on the self-angular velocity and acceleration and other measurement values ​​relative to the IMU body coordinate system through IMU pre-integration.

[0127] The present invention aims to solve the problem that the VIO algorithm in the prior art has to be recalculated each time the state changes. The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the need for initial rotation This solves the problem of excessive computation and improves the efficiency of image data processing.

[0128] The present invention's inverse depth coordinate representation of map point positions not only allows for a better Gaussian representation of the positional uncertainty of all near and far landmarks, but also enables delay-free feature initialization. The measurement model represented in inverse depth coordinates is highly linear across all parallaxes, thus providing more accurate motion estimation.

[0129] The present invention constructs an initial global map with scale information through system initialization, and the initialization of the global map is performed after the monocular REKF-VIO state converges, thereby reducing the impact of the accuracy of the initial global map on the accuracy of subsequent state estimation.

[0130] The present invention utilizes the comparison and selection of the interval between frames and the rotation angle to ensure the accuracy and integrity of the global map and ensure that the system constructs as many key frames as possible to improve the accuracy of the global map.

[0131] The present invention provides drift-free motion estimation of the map points maintained in the state vector based on the EKF monocular VIO. The map point position in the monocular REKF-VIO state vector is consistent with the global motion of the system. Figure 1 When the positioning accuracy is improved,

[0132] The present invention solves the technical problems existing in the prior art of low front-end image frame processing efficiency, error interference and low precision. BRIEF DESCRIPTION OF THE DRAWINGS

[0133] Figure 1 This is a system framework diagram of a SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to Example 1 of the present invention;

[0134] Figure 2 Schematic diagram of the basic steps of the SLAM method based on graph optimization and EKF framework fusing monocular vision and IMU in Example 1 of the present invention;

[0135] Figure 3 This is a schematic diagram of specific steps for system initialization according to Example 1 of the present invention;

[0136] Figure 4 Schematic diagram of the basic steps of EKF correction in Example 1 of the present invention;

[0137] Figure 5 This is a schematic diagram of the global map-assisted EKF feedback mechanism of Example 1 of the present invention. DETAILED DESCRIPTION

[0138] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0139] Example 1

[0140] The algorithm system framework diagram proposed by the invention is as follows Figure 1 As shown in the figure, it is divided into an EKF-based motion estimation module and a graph optimization-based global map construction and optimization module. Whenever the system's motion estimation module receives a new image frame and corresponding IMU measurement value, it first performs a monocular REKF-VIO state prediction and update process to estimate the posterior mean and covariance of the state of the current frame.

[0141] like Figure 2As shown in FIG, the SLAM method provided by the present invention based on graph optimization and fusion of monocular vision and IMU under the EKF framework includes the following basic steps:

[0142] S1. Image acquisition and inertial data acquisition are performed through a camera and an IMU inertial measurement device, respectively, and the information collected by the sensors is sent to the monocular REKF-VIO system for the next step. In this embodiment, the operating frequency of the camera can be, for example, 10 Hz, and the operating frequency of the IMU can be, for example, 200 Hz;

[0143] In this embodiment, the data acquisition and processing module uses a monocular inertial module for data acquisition (the positional relationship between the IMU and the camera is fixed. The IMU operates at a frequency of 200 Hz and the camera operates at a frequency of 10 Hz). To reduce the amount of computation, the camera is calibrated offline. The collected data is timestamped according to a fixed frame rate, and the IMU data must also be aligned.

[0144] S2. After obtaining the sensor measurement data in step S1, first perform the monocular REKF-VIO state prediction and update process to estimate the state posterior mean and covariance of the current frame. If the system initial global map has not been established, perform the state expansion method to expand the state mean and covariance of the monocular REKF-VIO;

[0145] In this embodiment, the monocular REFK-VIO algorithm executed in step S2 is designed based on the monocular VO algorithm proposed by scholars such as Civera. Monocular REFK-VIO first uses IMU measurements to predict the state, thereby obtaining the state prior probability density function of the current frame. Then the state is updated using the visual observation value of the current frame to obtain the state posterior probability density function of the current frame

[0146] Assuming that the current frame observes a map point, the state vector of the monocular REKF-VIO is:

[0147]

[0148] The meanings of the quantities in the formula are as follows:

[0149] Indicates the IMU state of the current frame;

[0150] Indicates the status of a map point;

[0151] Represents the rotation from the IMU coordinate system to the world coordinate system;

[0152] Represent the position and velocity of the IMU coordinate system relative to the world coordinate system respectively;

[0153] Represent the zero bias of the gyroscope and accelerometer respectively;

[0154] Indicates the position of the lth map point in the world coordinate system expressed in inverse depth coordinates.

[0155] According to the state representation of formula (1), the error state vector of monocular REKF-VIO is defined as:

[0156]

[0157] In this embodiment, state prediction (IMU pre-integration) includes:

[0158] The IMU uses a gyroscope and an accelerometer to measure its own angular velocity and acceleration relative to the IMU body coordinate system. These measurements are affected by the sensor's zero bias and zero-mean Gaussian white noise:

[0159]

[0160]

[0161] Where, Represents the transformation matrix between the world coordinate system and the IMU coordinate system, which is the known gravity vector in the world system.

[0162] The IMU's estimated score formula is as follows:

[0163]

[0164]

[0165]

[0166] in They are the position, velocity and attitude quaternions corresponding to the IMU from to time, is quaternion multiplication, Here we assume that the acceleration observation and rotation matrix It is constant in the time. From the above IMU integral, it can be seen that the integral term of the next moment depends on the value of the previous moment. If the state of the moment changes, the state of the moment must be recalculated. In the VIO algorithm based on optimization, each iteration of the state will change and must be recalculated, resulting in too much calculation. Therefore, in order to eliminate the initial rotation Dependence, multiply the integral by The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the dependence on the initial value. Multiply both sides of equation (5) by have:

[0167]

[0168]

[0169]

[0170] in:

[0171]

[0172]

[0173]

[0174] is the IMU pre-integration term that does not depend on the initial state.

[0175] In this embodiment, the status update includes:

[0176] According to formula (1), the position of the map point expressed in inverse depth coordinates can not only better represent the position uncertainty of all long and short distance landmarks with Gaussian distribution, but also achieve delay-free feature initialization. The measurement model expressed in inverse depth coordinates is highly linear under all parallaxes, thus providing more accurate motion estimation. The position of the lth map point expressed in inverse depth coordinates Can be converted to the corresponding Euclidean XYZ coordinates:

[0177]

[0178] From the camera projection model, we can know that the visual observation model of the lth map point represented by the inverse depth coordinate on the current frame image plane is:

[0179]

[0180] Where:

[0181] Represents the actual observation value of the lth map point in the current frame;

[0182] represents visual observation noise;

[0183] Represent the rotation and translation between the camera coordinate system and the IMU coordinate system respectively;

[0184] The prior mean of the state in the current frame At , the visual observation model shown in Equation (9) is linearized and estimated by the first-order Taylor expansion:

[0185]

[0186] In the formula Respectively Relative to IMU status The lth map point location The Jacobian matrix of the state vector is the measurement matrix. From this, the observation value of the lth map point on the current frame image plane can be obtained. The corresponding predicted value The error between and its linearization estimate:

[0187]

[0188] Assume that the observation data of all map points in the state vector of the current frame is By stacking all measurement errors, measurement matrix and measurement noise The update equations for the current frame state mean and covariance can be obtained as follows:

[0189]

[0190]

[0191]

[0192] in and

[0193] For IMU position, IMU velocity, gyroscope bias, accelerometer bias, and map point position, the + operator in the state mean update equation is equivalent to vector space addition.

[0194] For new images, the monocular REKF-VIO algorithm uses the ORB point feature tracking algorithm to track visual feature points and calculates the Hamming distance for each matched visual feature point. Only visual point features with a Hamming distance less than a given threshold are selected as visual observation values. Monocular REKF-VIO obtains the state posterior mean of the current frame by executing a 1-Point RANSAC filter update strategy. and covariance

[0195] In this embodiment, the state expansion includes:

[0196] If the initial global map of the system has not been established, the state mean and covariance of the monocular REKF-VIO need to be expanded through the state expansion method. Assuming that the state vector is expanded with a new map point in the current frame, its initial inverse depth coordinate is:

[0197]

[0198] According to the posterior mean of the IMU state of the current frame The camera position in initial inverse depth coordinates of a new map point can be calculated as:

[0199]

[0200] Then according to the observation value of the new map point in the current frame The azimuth and elevation angles in the initial inverse depth coordinates of the new map point can be calculated as:

[0201]

[0202] According to the parameters that determine the initial inverse depth The inverse depth value in the initial inverse depth coordinates of the new map point can be calculated as: Parameters that determine the initial inverse depth value The best setting of and its standard deviation is obtained by experiment. After obtaining the initial inverse depth coordinates of the new map point, the state mean is expanded as follows:

[0203]

[0204] Through the first-order Taylor expansion, the posterior mean of the IMU state in the current frame is The actual observation value of the map point in the current frame And the parameters that determine the initial inverse depth value At the inverse depth coordinate of the new map point Doing linear estimation we get:

[0205]

[0206] In the formula is the parameter noise that determines the initial inverse depth value,

[0207] and The status of the new map point Relative to IMU status The Jacobian matrix of the observation value, the inverse depth value of the map point, and the state vector. From this, the expanded form of the state covariance matrix can be obtained as:

[0208]

[0209] S3, then executing the system initialization process to construct the initial map points of the global map;

[0210] In this embodiment, system initialization is responsible for constructing an initial global map with scale information. Because the accuracy of the initial global map affects the accuracy of subsequent state estimation, global map initialization is performed after the monocular REKF-VIO state converges.

[0211] like Figure 3 As shown in the figure, the specific steps of system initialization include:

[0212] S31. Extract 1000 ORB visual point features from the current frame and match them with the visual point features of the reference frame. If the number of matched visual point feature pairs is greater than a given threshold, proceed to the next step; otherwise, set the current frame as the reference frame.

[0213] S32. Calculate the disparity of each visual point feature matching pair using the poses estimated by the monocular REKF-VIO for the reference frame and the current frame, and select a set of visual point feature matching pairs with sufficient disparity. If the number of visual point feature pairs in is greater than a given threshold, proceed to the next step.

[0214] S33. Using the poses estimated by the monocular REKF-VIO for the reference frame and the current frame, all visual point feature pairs are triangulated. If the number of successfully triangulated initial global map points exceeds a given threshold, the reference frame and the current frame are set as keyframes. These two keyframes and the constructed initial global map points constitute the initial global map. Finally, the initial global map is sent to the global map construction and optimization module for optimization.

[0215] S4, based on step S3, screening of visual key frames;

[0216] In this embodiment, the criteria for determining whether the current frame is a key frame include:

[0217] The time interval between the current frame and the previous keyframe is greater than a given threshold to ensure the accuracy of the global map.

[0218] The rotation angle between the current frame and the previous keyframe is greater than the given threshold to ensure that there are no gaps in the global map.

[0219] When a keyframe is selected, the sliding window map optimization performed by the global map construction and optimization module has ended, ensuring that the system constructs as many keyframes as possible to improve the accuracy of the global map.

[0220] S5. The key frame information filtered out in step S4 is fed into the designed global map-assisted EKF feedback mechanism module to perform EKF state correction and EFK map correction. The global map optimized by the graph optimization method is used to update and expand the state vector of the monocular REKF-VIO again.

[0221] like Figure 4 As shown in the figure, in this embodiment, the feedback mechanism improves the motion estimation accuracy of the monocular REKF-VIO by effectively utilizing the globally consistent map constructed by the system. When executing the feedback mechanism on the current frame, 1000 ORB visual point features are first extracted from the current frame image, and a set of visual point feature sets that match the global map points seen in the previous keyframe is searched.

[0222] like Figure 5 As shown, in this embodiment, based on the matched visual point feature set, the following two steps are performed respectively.

[0223] Step S51: EKF state correction.

[0224] First, the posterior mean of the current frame IMU state estimated by monocular REKF-VIO As the initial value, by solving Figure 2 The factor graph shown is used to optimize the current frame IMU state:

[0225]

[0226] in Represents the IMU pre-integration residual between the previous keyframe and the current frame, Represents the reprojection residual between the map point corresponding to the visual point feature and the current frame. Then the optimized IMU state estimate of the current frame is used and its covariance Update the state mean and covariance of the monocular REKF-VIO again. State estimation It is used as the current frame IMU state The new measurement value of is updated as follows:

[0227]

[0228]

[0229]

[0230] in Is the IMU status Jacobian matrix with respect to the state vector. The measured value of the IMU state in the current frame and predicted value The error between them is linearized.

[0231] Step S52: EKF map correction.

[0232] In this embodiment, the EKF-based monocular VIO provides drift-free motion estimation relative to the map points maintained in the state vector. Figure 1 Therefore, the key point of EKF map correction is to use the map points in the global map to expand the state vector of the monocular REKF-VIO.

[0233] First, according to the optimized current frame IMU state estimate and the estimated location of the map point To calculate the inverse depth parameter of the map point:

[0234]

[0235] And linearize it through the first-order Taylor expansion, so the inverse depth parameter variance of the map point can be obtained as:

[0236]

[0237] in and Represents the linearized estimate of the inverse depth parameter relative to the current frame IMU state and map point locations The Jacobian matrix of . and They represent the covariance of the optimized IMU state estimate of the current frame and the corresponding map point position estimate.

[0238] Then, the posterior mean of the current frame IMU state is obtained after performing EKF state correction based on the monocular REKF-VIO Observation value of the map point in the current frame image And the initial inverse depth parameter calculated using formula (21) The initial inverse depth coordinates of the map points calculated according to equations (14) and (15) are And expand the state mean of monocular REKF-VIO according to formula (16).

[0239] Finally, the state posterior covariance obtained after performing EKF state correction according to the monocular REKF-VIO is Observation covariance of map points in the current frame image And the standard deviation of the initial inverse depth value parameter of the map point calculated using formula (22) The state covariance matrix of the monocular REKF-VIO is expanded according to formula (18).

[0240] S6. Send the motion estimation information obtained above to the global map construction and optimization module for local map update and closed-loop optimization.

[0241] In summary, the complementary graph optimization and EKF fusion framework proposed in this paper allows the system to improve front-end computational efficiency while maintaining good pose estimation accuracy. The global map-assisted EKF feedback mechanism proposed in this paper effectively utilizes the globally consistent map constructed by the system, eliminating the influence of linearization errors in existing technologies and improving the motion estimation accuracy of monocular REKF-VIO.

[0242] The present invention improves the positioning accuracy of indoor autonomous mobile robots in three-dimensional space while ensuring rapid algorithm response, can further improve the real-time performance and positioning accuracy of SLAM systems, and provide technical guarantees for indoor autonomous mobile robots to complete specific tasks.

[0243] The present invention reduces the influence of sensor zero bias and zero-mean Gaussian white noise on the self-angular velocity and acceleration and other measurement values ​​relative to the IMU body coordinate system through IMU pre-integration.

[0244] The present invention aims to solve the problem that the VIO algorithm in the prior art has to be recalculated each time the state changes. The rotation matrix in the integral term becomes The initial value is the unit matrix, which eliminates the need for initial rotation This solves the problem of excessive computation and improves the efficiency of image data processing.

[0245] The present invention's inverse depth coordinate representation of map point positions not only allows for a better Gaussian representation of the positional uncertainty of all near and far landmarks, but also enables delay-free feature initialization. The measurement model represented in inverse depth coordinates is highly linear across all parallaxes, thus providing more accurate motion estimation.

[0246] The present invention constructs an initial global map with scale information through system initialization, and the initialization of the global map is performed after the monocular REKF-VIO state converges, thereby reducing the impact of the accuracy of the initial global map on the accuracy of subsequent state estimation.

[0247] The present invention utilizes the comparison and selection of the interval between frames and the rotation angle to ensure the accuracy and integrity of the global map and ensure that the system constructs as many key frames as possible to improve the accuracy of the global map.

[0248] The present invention provides drift-free motion estimation of the map points maintained in the state vector based on the EKF monocular VIO. The map point position in the monocular REKF-VIO state vector is consistent with the global motion of the system. Figure 1 When the positioning accuracy is improved,

[0249] The present invention solves the technical problems existing in the prior art of low front-end image frame processing efficiency, error interference and low precision.

[0250] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A SLAM method based on graph optimization and EKF framework that integrates monocular vision and IMU, characterized by: The method comprises: S1. Collect images and inertial data through a camera and an inertial measurement unit (IMU), respectively, and send the images and inertial data to a monocular REKF-VIO system with preset frequency parameters; S2. Performing state prediction, updating, and state expansion operations on the monocular REKF-VIO system to estimate the state posterior mean and system covariance of the current frame, wherein step S2 includes: S21, using the monocular REKF-VIO system to perform monocular REKF-VIO state prediction based on the image and the inertial data; S22, performing an update operation on the monocular REKF-VIO system to estimate the posterior mean of the current frame state and the current frame covariance; S23, when the initial global map of the monocular REKF-VIO system is not established, executing a preset state expansion logic to expand the system state posterior mean and the system covariance of the monocular REKF-VIO system; S3, initializing the monocular REKF-VIO system to construct initial map points of the initial global map; S4, performing screening processing based on the initial map points to obtain visual key frames; S5. According to the visual keyframes, an optimized global map and motion state information are obtained by using a graph optimization and EKF complementary framework. The step S5 further includes: S51, sending the visual key frame to a preset global map-assisted EKF feedback mechanism module to perform EKF state correction on the visual key frame; S52, performing EFK map correction according to the visual keyframe, optimizing the initial global map using a graph optimization method to obtain the optimized global map, and updating and expanding the state vector of the monocular REKF-VIO system based on the map to obtain the motion estimation information; S6. Send the motion estimation information to the optimized global map to perform local map update and closed-loop optimization.

2. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that The step S21 includes: S211: Assume that m map points are observed in the current frame. Then use the following logic to transform the state vector X of the monocular REKF-VIO system into S Expressed as: Where, Indicates the IMU state of the current frame, Indicates the status of a map point. Represents the rotation from the IMU coordinate system to the world coordinate system, Represent the position and velocity of the IMU coordinate system relative to the world coordinate system, are the zero bias of the gyroscope and accelerometer, Indicates the position of the lth map point in the world coordinate system expressed in inverse depth coordinates; S212, using the inertial measurement unit IMU to measure its own angular velocity and acceleration ω relative to its own body coordinate system m and a m : oh m =ω B +b g +n g Where, Represents the transformation matrix between the world coordinate system and the IMU coordinate system, g w is the known gravity vector in the world system, b g , b a is the sensor zero bias, n g , n a is zero-mean Gaussian white noise; S213: Pre-score the inertial measurement unit (IMU) using the following logic: Where, They correspond to the inertial measurement unit IMU from b k to b k+1 The position, velocity and attitude quaternions at each moment, is quaternion multiplication, am is the acceleration observation, is the rotation matrix, Δt is the prediction time; S214, multiply both sides of the above pre-integration equation by The following equation is obtained: In the above formula: and is the IMU pre-integration term that does not depend on the initial state.

3. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that The S22 includes: S221. Use inverse depth coordinates to represent the position of the lth map point And use the following logic to convert the lth map point position into the corresponding Euclidean XYZ coordinates S222: According to the preset camera projection model, use the following logic to obtain the lth map point in the current frame F s Visual observation model on the image plane: Where, Indicates the current frame F s The actual observation value of the lth map point, represents the visual observation noise, Represent the rotation and translation between the camera coordinate system and the IMU coordinate system respectively; S223, in the current frame F s The state prior mean At , the following first-order Taylor expansion logic is used to linearize the visual observation model to obtain the current frame F s Observations on the image plane Where, and J sl Respectively Relative to IMU status The lth map point location and the state vector X s The Jacobian matrix, J sl is the measurement matrix; S224: Using the following logic, the lth map point can be obtained in the current frame F s Observations on the image plane and the corresponding predicted value The measurement error and linearization estimate between: S225, the current frame F s The observation data of all map points in the state vector is By stacking all measurement errors r sl , measurement matrix J sl and the visual observation noise To obtain the update equation of the state posterior mean of the current frame and the system covariance: Where, and 4. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that The step S23 includes: S231: When the initial global map of the system is not established, the current frame F s Select a new map point α to expand the state vector, and its initial inverse depth coordinate is: S232. Calculate the initial inverse depth coordinate camera position of the new map point α based on the posterior mean of the IMU state of the current frame using the following logic: S233, according to the current frame F s The observation value of the new map point α The following logic is used to calculate the initial inverse depth coordinate azimuth and elevation of the new map point: S234, determining parameters based on initial inverse depth Get the initial inverse depth coordinate and inverse depth value of the new map point α: S235. Expand the system state posterior mean according to the initial inverse depth coordinate and inverse depth value of the new map point α using the following logic: S236, using the following first-order Taylor expansion logic, respectively in the current frame F s The posterior mean of the inertial measurement unit IMU state The actual observation value of the map point α in the current frame And the parameters that determine the initial inverse depth value At the inverse depth coordinate of the new map point α Doing linear estimation we get: In the above formula: This formula determines the parameter noise of the initial inverse depth value, and are the states of the new map point α Relative to the IMU state Observation value z sα , the inverse depth value ρ0 of the map point and the state vector X s The Jacobian matrix of ; S237. According to the Jacobian matrix, the expanded form of the state covariance matrix is ​​obtained using the following logic:

5. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that The step S3 comprises: S31, extracting a preset number of ORB visual point features from the image of the current frame, and comparing them with the reference frame F r Match the visual point features; S32: When the visual point feature is greater than a preset logarithmic threshold, r With the current frame F s The pose of each visual point feature matching pair is calculated, and the disparity of each visual point feature matching pair is selected, and a group F of visual point feature matching pairs with applicable disparity is selected; S33, when the number of visual point feature pairs in the applicable parallax visual point feature matching pair F is greater than the preset logarithm threshold, r With the current frame F s , triangulate all visual point feature matching pairs in the applicable parallax visual point feature matching pair group F to obtain the number of triangulated initial global map points; S34, when the number of points of the triangulated initial global map is greater than the preset map point threshold, the reference frame F r With the current frame F s Set as a key frame, based on which the initial global map points are combined to form the initial global map, and start optimizing the initial global map.

6. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that In step S4, the time interval and rotation angle between the current frame and the previous key frame are compared with a given threshold to select the visual key frame.

7. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that The step S51 includes: S511, taking the state posterior mean of the inertial measurement unit IMU of the current frame As the initial value, use the following logic to solve the preset factor graph and optimize the IMU state of the current frame to obtain the optimized estimated value of the IMU state of the current frame Where, Indicates the previous key frame F m With the current frame F s The IMU pre-integration residual between Indicates F s The visual point feature p corresponds to the map point and the current frame F s The reprojection residual between ; S512: Optimize the estimated value according to the current frame IMU state and covariance The state mean and covariance of the monocular REKF-VIO system are then updated using the following logic: Where, Is the IMU status Relative to the state vector X s The Jacobian matrix of ; The measured value of the IMU state in the current frame and predicted value The error between them is linearized.

8. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that: The S52 includes: S521, according to the current frame IMU state estimation value obtained by optimization and the estimated position of the map point α The inverse depth parameter of the map point α is calculated using the following logic: S522: performing linearization processing on the inverse depth parameter of the map point α to obtain the variance of the inverse depth parameter of the map point α; S523, performing EKF state correction according to the monocular REKF-VIO system to obtain the posterior mean of the IMU state of the current frame The observation value of the map point α in the current frame image And the initial inverse depth parameter Calculate the initial inverse depth coordinate of the map point α and expanding the state posterior mean of the monocular REKF-VIO system; S524, perform EKF state correction according to the monocular REKF-VIO, and obtain the state posterior covariance The observed covariance of the map point α in the current frame image And the initial inverse depth value of map point α determines the parameter standard deviation The state covariance matrix of monocular REKF-VIO is expanded accordingly.

9. The SLAM method based on graph optimization and fusion of monocular vision and IMU under the EKF framework according to claim 1 is characterized in that The inverse depth parameter of the map point α is linearized by the following first-order Taylor expansion to obtain the variance of the inverse depth parameter of the map point α: Where, and Represents the linearized estimate of the inverse depth parameter relative to the current frame IMU state and map point locations The Jacobian matrix of and They represent the covariance of the optimized IMU state estimate of the current frame and the corresponding map point position estimate.

10. A SLAM system based on graph optimization and EKF framework that integrates monocular vision and IMU, characterized by: The system comprises: A data acquisition module, configured to collect images and inertial data through a camera and an inertial measurement unit (IMU), respectively, and to feed the images and inertial data into a monocular REKF-VIO system with preset frequency parameters; A prediction and expansion module is configured to perform state prediction, update, and state expansion operations on the monocular REKF-VIO system to estimate the state posterior mean and system covariance of the current frame. The prediction and expansion module is connected to the data acquisition module, wherein the prediction and expansion module includes: a state prediction module for performing monocular REKF-VIO state prediction based on the image and the inertial data using the monocular REKF-VIO system; A state update module, configured to perform an update operation on the monocular REKF-VIO system to estimate a posterior mean of the current frame state and a current frame covariance; a state covariance expansion module, configured to execute a preset state expansion logic to expand the system state posterior mean and the system covariance of the monocular REKF-VIO system when an initial global map of the monocular REKF-VIO system is not established, wherein the state covariance expansion module is connected to the state prediction module and the state update module; A map point initialization module, used to initialize the monocular REKF-VIO system to construct initial map points of the initial global map; A key frame acquisition module, configured to perform screening processing based on the initial map points to obtain visual key frames, wherein the key frame acquisition module is connected to the map point initialization module; A graph optimization and EKF complementary framework module is used to obtain an optimized global map and motion state information based on the visual keyframe using the graph optimization and EKF complementary framework. The graph optimization and EKF complementary framework module is connected to the keyframe acquisition module and further includes: An EKF state correction module is used to send the visual key frame to a preset global map auxiliary EKF feedback mechanism module to perform EKF state correction on the visual key frame; an EFK map correction and motion estimation information acquisition module, configured to perform EFK map correction based on the visual keyframes, optimize the initial global map using a graph optimization method to obtain the optimized global map, and then update and expand the state vector of the monocular REKF-VIO system based on the map to obtain the motion estimation information. The EFK map correction and motion estimation information acquisition module is connected to the EKF state correction module; The local map update optimization module is used to send the motion estimation information to the optimized global map to perform local map update and closed-loop optimization. The local map update optimization module is connected to the EFK map correction and motion estimation information acquisition module.

Images