Monocular Vio fusion RTK system based on incremental graph optimization

Abstract

The present invention discloses a monocular VIO-fused RTK system based on incremental graph optimization, comprising: a data acquisition and synchronization module for collecting IMU, image, and RTK data and ensuring their timestamp synchronization; an initial state estimation module for combining IMU and visual data using the VIO algorithm to perform preliminary state estimation, and then using RTK data for global position calibration, with the initial state as the first node of the graph; an incremental state graph construction and constraint residual module for constructing the incremental state graph and calculating the residuals; and an incremental optimization module for calculating the sum of squares of all newly added constraint residuals, calculating the Jacobian matrix, and optimizing only the latest state node. The present invention avoids the need to rebuild the entire graph for each data update, improving computational efficiency and real-time performance. Fusion of RTK data as a precise absolute position reference improves positioning accuracy and resolves the issue of scale uncertainty.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and mapping, and in particular to a monocular VIO fusion RTK system based on incremental graph optimization. Background Art

[0002] Monocular vio is a positioning technology that combines data from a monocular camera and an inertial measurement unit (IMU). The basic principle is to estimate the camera's motion by extracting feature points from the image captured by the camera, while using the acceleration and angular velocity information provided by the IMU to supplement the visual information and improve the accuracy of motion estimation. However, monocular vio also has some problems. First, the monocular camera cannot directly obtain the depth information of the scene and can only estimate the depth, which leads to scale uncertainty and positioning drift over long periods of time. Moreover, the noise and deviation of the IMU will affect the long-term stability of the system. In particular, when running for a long time, cumulative errors may occur, resulting in a decrease in positioning accuracy.

[0003] Graph optimization is a technique that uses graph structures to represent variables and constraints in optimization problems. However, each time the data is updated, the graph needs to be rebuilt, resulting in large computational workload and low computational efficiency.

[0004] Real-time kinematic positioning (RTK) is a high-precision positioning technology based on satellite navigation systems that can provide accurate location information in open-air environments. Summary of the Invention

[0005] The purpose of the present invention is to overcome the shortcomings and deficiencies of the existing technology and propose a monocular VIO fusion RTK system based on incremental graph optimization, which avoids the need to rebuild the entire graph every time the data is updated, thereby improving computational efficiency and real-time performance; fusing RTK data as a precise absolute position reference, improves positioning accuracy, and solves the problem of scale uncertainty.

[0006] To achieve the above objectives, the present invention provides a technical solution: a monocular VIO fusion RTK system based on incremental graph optimization, comprising:

[0007] The data acquisition and synchronization module is used to collect IMU data from the inertial sensor, image data from the monocular camera, and RTK data from the GNSS sensor, and then use timestamps to match the data from different sensors to ensure their timestamp synchronization;

[0008] The initial state estimation module is used to combine the synchronized IMU and image data to perform preliminary state estimation using the vio algorithm, and then use RTK data for global position calibration, with the initial state as the first node of the state diagram;

[0009] The incremental state diagram construction and constrained residual module is used to construct an incremental state diagram based on the initial state diagram using the new state, and at the same time perform state prediction and calculate the constrained residual between the predicted value and the actual value;

[0010] The incremental optimization module is used to calculate the sum of squares of all newly added constraint residuals, calculate the Jacobian matrix, derive the intermediate variables in the incremental optimization process through the Jacobian matrix, and use the intermediate variables to optimize the latest state node.

[0011] Furthermore, the data acquisition and synchronization module specifically performs the following operations:

[0012] Get IMU, image and RTK data:

[0013] a=[a x ,a y ,a z ] T

[0014] w=[w x ,w y ,w z ] T

[0015] z visual =[u,v] T

[0016] p RTK =[x,y,z]

[0017] v RTK =[v x ,v y ,v z ]

[0018] q RTK =[q w ,q x ,q y ,q z ]

[0019] In the formula, a represents acceleration, a x 、a y 、a z They represent the acceleration components on the x, y, and z axes respectively; w represents the angular velocity, w x 、w y 、w z Represents the angular velocity components on the x, y, and z axes respectively; z visual Represents the coordinates of the feature points obtained from the image data, u and v represent the x-axis and y-axis coordinates of the feature points in the image respectively; [·] T represents transpose; p RTKIndicates the position coordinates in RTK data; v RTK Indicates the speed information in RTK data, v x 、v y 、v z Represents the velocity components on the x, y, and z axes respectively; q RTK Represents the attitude quaternion, q w Represents the scalar part of the quaternion, which is related to the cosine of the rotation angle, q x ,q y ,q z is the imaginary part of the quaternion, corresponding to the direction of the rotation axis and the sine part of the rotation angle;

[0020] After obtaining the above data, synchronize the data to ensure that all types of data have matching timestamps and output the data.

[0021] Furthermore, the initial state estimation module specifically performs the following operations:

[0022] 1) Use the VIO algorithm to combine the synchronized IMU and visual data to obtain the initial state estimate:

[0023] x0=[p0,v0,q0,z visual,0 ]

[0024] p0=[x p0 ,y p0 ,z p0 ] T

[0025] v0=[v x0 ,v y0 ,v z0 ] T

[0026]

[0027] z visual,0 =[u0,h0] T

[0028] In the formula, x0 represents the initial state; p0 represents the initial position, x p0 、y p0 、z p0 They represent the components of the initial position on the x, y, and z axes respectively; v0 represents the initial velocity, v x0 、v y0 、v z0 Respectively represent the components of the initial velocity on the x, y, and z axes; q0 represents the initial attitude quaternion, Indicates the angle of rotation, Represents the components of the rotation axis on the x, y, and z axes respectively; zvisual,0 Indicates the x- and y-axis coordinates (u0, h0) of the feature points in the initial image;

[0029] 2) Calibrate using the global position provided by RTK:

[0030]

[0031] Where p0′ is the initial estimated global position, is the accurate position provided by RTK, replace p0 in x0 with That is, p0′ is replaced to achieve global position calibration;

[0032] The replaced x0 is output as the initial state and also serves as the first node in constructing the incremental state graph.

[0033] Furthermore, the incremental state diagram construction and constraint residual module specifically performs the following operations:

[0034] 1) Use the IMU, image, and RTK data at time k-1 to predict the state at time k

[0035]

[0036] Where p k-1 represents the position at time k-1; v k-1 represents the speed at time k-1; q k-1 Represents the attitude quaternion at time k-1; h visual (·) represents the prediction equation for predicting the coordinates of the feature points in the image at the next moment; z visual,k-1 represents the coordinates of the feature points in the image at time k-1; a k-1 is the acceleration at time k-1; Δq(w k-1 ,Δt) represents the angular velocity w at time k-1 k-1 The incremental quaternion after Δt time; Represents the multiplication of two quaternions; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted position value; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted speed value; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted attitude quaternion value; Indicates the coordinate z of the feature point at time k according to time k-1 visual,k-1 The predicted value of the feature point coordinates;

[0037] 2) Update the state x at the current time k with the new IMU, image and RTK data k :

[0038] x k =[p k ,v k ,q k ,z visual,k ]

[0039] Where p k represents the position at time k; v k represents the speed at time k; q k Represents the attitude quaternion at time k; z visual,k Represents the coordinates of the feature points in the image at time k;

[0040] Every time a new state is added, a new node is added to the existing state diagram to implement an incremental state diagram;

[0041] 3) Calculate the new constraint residual based on the predicted value obtained at time k-1 and the true value obtained from the IMU data at time k:

[0042]

[0043] Where, Represents the state x at the current time k k The constrained residual between the predicted value and the true value of the location; Represents the state x at the current time k k The constrained residual between the speed prediction and the true value; Represents the state x at the current time k k The constrained residual between the attitude quaternion prediction value and the true value;

[0044] 4) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the image data at time k:

[0045]

[0046] Where z visual,k represents the actual feature point coordinates obtained through the image at time k; r visual,k (x k ) represents the state x at the current time k k The constrained residual between the predicted value and the true value of the feature point coordinates;

[0047] 5) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the RTK data at time k:

[0048]

[0049] Where, Indicates the real position provided by RTK; r RTK,k (x k ) represents the state x at the current time k k The constrained residual between the position prediction value and the true value provided by RTK.

[0050] Furthermore, the incremental optimization module specifically performs the following operations:

[0051] 1) Calculate the sum of squares of all newly constrained residuals:

[0052]

[0053] Where R represents the sum of squares of the constrained residuals;

[0054] 2) Calculate the partial derivative of each constraint residual with respect to the state variable to obtain a Jacobian matrix:

[0055]

[0056]

[0057] J=[J1,J2,...J i ,...J5]

[0058] Where J1 represents The vector after partial derivative of position p; J2 represents The vector after taking the partial derivative of velocity v; J3 represents The vector after partial derivative of the posture q; J4 represents r visual,k (x k ) The vector after partial derivative of the number of feature points n in the image; J5 represents r RTK,k (x k ) The vector after partial derivative of position p; J represents the vector obtained by J i The Jacobian matrix composed of

[0059] 3) Calculate the optimized intermediate variables using the Jacobian matrix and the sum of squares of the constraint residuals:

[0060] Δx=(J T J+λI) -1 J T R

[0061] Where Δx represents the control of x in the optimization process k The amount of change; λ is the damping factor; J T J is the regularization term of the Jacobian matrix; R represents the sum of squares of the newly added constraint residuals;

[0062] 4) Iterative optimization based on intermediate variables:

[0063]

[0064] Where, Indicates that it is used to replace during the optimization process The intermediate variable of Intermediate variables representing positions; An intermediate variable representing speed; Intermediate variables representing posture; Intermediate variables representing feature points in the image;

[0065] Will Replace the original predicted value:

[0066]

[0067]

[0068] Repeat steps 1), 2), and 3) for multiple optimizations until R converges and outputs

[0069] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0070] The present invention effectively solves the scale uncertainty problem of the monocular VIO system, improves the positioning accuracy of the system, and solves the problem of rebuilding the state graph every time the data is updated during the graph optimization process, thereby reducing the amount of calculation and ensuring computational efficiency and real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is an architecture diagram of the system of the present invention. DETAILED DESCRIPTION

[0072] The present invention will be described in further detail below with reference to the embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0073] like Figure 1 As shown, this embodiment discloses a monocular VIO fusion RTK system based on incremental graph optimization, including the following functional modules:

[0074] The data acquisition and synchronization module is used to collect IMU data from the inertial sensor, image data from the monocular camera, and RTK data from the GNSS sensor, and then use timestamps to match the data from different sensors to ensure their timestamp synchronization;

[0075] The initial state estimation module is used to combine the synchronized IMU and image data to perform preliminary state estimation using the vio algorithm, and then use RTK data for global position calibration, with the initial state as the first node of the state diagram;

[0076] The incremental state diagram construction and constrained residual module is used to construct an incremental state diagram based on the initial state diagram using the new state, and at the same time perform state prediction and calculate the constrained residual between the predicted value and the actual value;

[0077] The incremental optimization module is used to calculate the sum of squares of all newly added constraint residuals, calculate the Jacobian matrix, derive the intermediate variables in the incremental optimization process through the Jacobian matrix, and use the intermediate variables to optimize the latest state node.

[0078] Specifically, the data acquisition and synchronization module performs the following operations:

[0079] Get IMU, image and RTK data:

[0080] a=[a x ,a y ,a z ] T

[0081] w=[w x ,w y ,w z ] T

[0082] z visual =[u,v] T

[0083] p RTK =[x,y,z]

[0084] v RTK =[v x ,v y ,v z ]

[0085] q RTK =[q w ,q x ,q y ,q z ]

[0086] In the formula, a represents acceleration, a x 、a y 、a z They represent the acceleration components on the x, y, and z axes respectively; w represents the angular velocity, w x 、w y 、w zRepresents the angular velocity components on the x, y, and z axes respectively; z visual Represents the coordinates of the feature points obtained from the image data, u and v represent the x-axis and y-axis coordinates of the feature points in the image respectively; [·] T represents transpose; p RTK Indicates the position coordinates in RTK data; v RTK Indicates the speed information in RTK data, v x 、v y 、v z Represents the velocity components on the x, y, and z axes respectively; q RTK Represents the attitude quaternion, q w Represents the scalar part of the quaternion, which is related to the cosine of the rotation angle, q x ,q y ,q z is the imaginary part of the quaternion, corresponding to the direction of the rotation axis and the sine part of the rotation angle;

[0087] After obtaining the above data, synchronize the data to ensure that all types of data have matching timestamps and output the data.

[0088] Specifically, the initial state estimation module performs the following operations:

[0089] 1) Use the VIO algorithm to combine the synchronized IMU and visual data to obtain the initial state estimate:

[0090] x0=[p0,v0,q0,z visual,0 ]

[0091] p0=[x p0 ,y p0 ,z p0 ] T

[0092] v0=[v x0 ,v y0 ,v z0 ] T

[0093]

[0094] z visual,0 =[u0,h0] T

[0095] In the formula, x0 represents the initial state; p0 represents the initial position, x p0 、y p0 、z p0 They represent the components of the initial position on the x, y, and z axes respectively; v0 represents the initial velocity, v x0 、v y0、v z0 Respectively represent the components of the initial velocity on the x, y, and z axes; q0 represents the initial attitude quaternion, Indicates the angle of rotation, Represents the components of the rotation axis on the x, y, and z axes respectively; z visual,0 Indicates the x- and y-axis coordinates (u0, h0) of the feature points in the initial image;

[0096] 2) Calibrate using the global position provided by RTK:

[0097]

[0098] Where p0′ is the initial estimated global position, is the accurate position provided by RTK, replace p0 in x0 with That is, p0′ is replaced to achieve global position calibration;

[0099] The replaced x0 is output as the initial state and also serves as the first node in constructing the incremental state graph.

[0100] Specifically, the incremental state diagram construction and constraint residual module specifically performs the following operations:

[0101] 1) Use the IMU, image, and RTK data at time k-1 to predict the state at time k

[0102]

[0103]

[0104] Where p k-1 represents the position at time k-1; v k-1 represents the speed at time k-1; q k-1 Represents the attitude quaternion at time k-1; h visual (·) represents the prediction equation for predicting the coordinates of the feature points in the image at the next moment; z visual,k-1 represents the coordinates of the feature points in the image at time k-1; a k-1 is the acceleration at time k-1; Δq(w k-1 ,Δt) represents the angular velocity w at time k-1 k-1 The incremental quaternion after Δt time; Represents the multiplication of two quaternions; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted position value; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted speed value; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted attitude quaternion value; Indicates the coordinate z of the feature point at time k according to time k-1 visual,k-1 The predicted value of the feature point coordinates;

[0105] 2) Update the state x at the current time k with the new IMU, image and RTK data k :

[0106] x k =[p k ,v k ,q k ,z visual,k ]

[0107] Where p k represents the position at time k; v k represents the speed at time k; q k Represents the attitude quaternion at time k; z visual,k Represents the coordinates of the feature points in the image at time k;

[0108] Every time a new state is added, a new node is added to the existing state diagram to implement an incremental state diagram;

[0109] 3) Calculate the new constraint residual based on the predicted value obtained at time k-1 and the true value obtained from the IMU data at time k:

[0110]

[0111] Where, Represents the state x at the current time k k The constrained residual between the predicted value and the true value of the location; Represents the state x at the current time k k The constrained residual between the speed prediction and the true value; Represents the state x at the current time k k The constrained residual between the attitude quaternion prediction value and the true value;

[0112] 4) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the image data at time k:

[0113]

[0114] Where z visual,k represents the actual feature point coordinates obtained through the image at time k; r visual,k (x k ) represents the state x at the current time k kThe constrained residual between the predicted value and the true value of the feature point coordinates;

[0115] 5) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the RTK data at time k:

[0116]

[0117] Where, Indicates the real position provided by RTK; r RTK,k (x k ) represents the state x at the current time k k The constrained residual between the position prediction value and the true value provided by RTK.

[0118] Specifically, the incremental optimization module performs the following operations:

[0119] 1) Calculate the sum of squares of all newly constrained residuals:

[0120]

[0121] Where R represents the sum of squares of the constrained residuals;

[0122] 2) Calculate the partial derivative of each constraint residual with respect to the state variable to obtain a Jacobian matrix:

[0123]

[0124] J=[J1,J2,...J i ,...J5]

[0125] Where J1 represents The vector after partial derivative of position p; J2 represents The vector after taking the partial derivative of velocity v; J3 represents The vector after partial derivative of the posture q; J4 represents r visual,k (x k ) The vector after partial derivative of the number of feature points n in the image; J5 represents r RTK,k (x k ) The vector after partial derivative of position p; J represents the vector obtained by J i The Jacobian matrix composed of

[0126] 3) Calculate the optimized intermediate variables using the Jacobian matrix and the sum of squares of the constraint residuals:

[0127] Δx=(J T J+λI) -1 J T R

[0128] Where Δx represents the control of x in the optimization processk The amount of change; λ is the damping factor; J T J is the regularization term of the Jacobian matrix; R represents the sum of squares of the newly added constraint residuals;

[0129] 4) Iterative optimization based on intermediate variables:

[0130]

[0131] Where, Indicates that it is used to replace during the optimization process The intermediate variable of Intermediate variables representing positions; An intermediate variable representing speed; Intermediate variables representing posture; Intermediate variables representing feature points in the image;

[0132] Will Replace the original predicted value:

[0133]

[0134] Repeat steps 1), 2), and 3) for multiple optimizations until R converges and outputs

[0135] In summary, the present invention can effectively avoid the need to rebuild the entire map every time the data is updated, thereby improving computational efficiency and real-time performance. It also integrates RTK data as a precise absolute position reference, improves positioning accuracy, and solves the problem of scale uncertainty.

[0136] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A monocular vio fusion rtk system based on incremental graph optimization, characterized by: include: The data acquisition and synchronization module is used to collect IMU data from the inertial sensor, image data from the monocular camera, and RTK data from the GNSS sensor, and then use timestamps to match the data from different sensors to ensure their timestamp synchronization; The initial state estimation module is used to combine the synchronized IMU and image data to perform preliminary state estimation using the vio algorithm, and then use RTK data for global position calibration, with the initial state as the first node of the state diagram; The incremental state diagram construction and constrained residual module is used to construct an incremental state diagram based on the initial state diagram using the new state, and at the same time perform state prediction and calculate the constrained residual between the predicted value and the actual value; The incremental optimization module is used to calculate the sum of squares of all newly added constraint residuals, calculate the Jacobian matrix, derive the intermediate variables in the incremental optimization process through the Jacobian matrix, and use the intermediate variables to optimize the latest state node; The incremental state diagram construction and constraint residual module specifically performs the following operations: 1) Use the IMU, image, and RTK data at time k-1 to predict the state at time k Where p k-1 represents the position at time k-1; v k-1 represents the speed at time k-1; q k-1 Represents the attitude quaternion at time k-1; h visual (·) represents the prediction equation for predicting the coordinates of the feature points in the image at the next moment; z visual,k-1 represents the coordinates of the feature points in the image at time k-1; a k-1 is the acceleration at time k-1; Δq(w k-1 ,Δt) represents the angular velocity w at time k-1 k-1 The incremental quaternion after Δt time; Represents the multiplication of two quaternions; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted position value; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted speed value; Indicates that the state x at time k is based on the state x-1 at time k. k-1 The predicted attitude quaternion value; Indicates the coordinate z of the feature point at time k according to time k-1 visual,k-1 The predicted value of the feature point coordinates; 2) Update the state x at the current time k with the new IMU, image and RTK data k : x k =[p k ,v k ,q k ,z visual,k ] Where p k represents the position at time k; v k represents the speed at time k; q k Represents the attitude quaternion at time k; z visual,k Represents the coordinates of the feature points in the image at time k; Every time a new state is added, a new node is added to the existing state diagram to implement an incremental state diagram; 3) Calculate the new constraint residual based on the predicted value obtained at time k-1 and the true value obtained from the IMU data at time k: Where, Represents the state x at the current time k k The constrained residual between the predicted value and the true value of the location; Represents the state x at the current time k k The constrained residual between the speed prediction and the true value; Represents the state x at the current time k k The constrained residual between the attitude quaternion prediction value and the true value; 4) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the image data at time k: Where z visual,k represents the actual feature point coordinates obtained through the image at time k; r visual,k (x k ) represents the state x at the current time k k The constrained residual between the predicted value and the true value of the feature point coordinates; 5) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the RTK data at time k: Where, Indicates the real position provided by RTK; r RTK,k (x k ) represents the state x at the current time k k The constrained residual between the position prediction value and the true value provided by RTK; The incremental optimization module specifically performs the following operations: 1) Calculate the sum of squares of all newly constrained residuals: Where R represents the sum of squares of the constrained residuals; 2) Calculate the partial derivative of each constraint residual with respect to the state variable to obtain a Jacobian matrix: J\[J1,J2,...J i ,…J5] Where J1 represents The vector after partial derivative of position p; J2 represents The vector after taking the partial derivative of velocity v; J3 represents The vector after partial derivative of the posture q; J4 represents r visual,k (x k ) The vector after partial derivative of the number of feature points n in the image; J5 represents The vector after partial derivative of position p; J represents the vector obtained by J i The Jacobian matrix composed of 3) Calculate the optimized intermediate variables using the Jacobian matrix and the sum of squares of the constraint residuals: Δx=(J T J+λI) -1 J T R In the formula, Δx represents the control of x in the optimization process k The amount of change; λ is the damping factor; J T J is the regularization term of the Jacobian matrix; R represents the sum of squares of the newly added constraint residuals; 4) Iterative optimization based on intermediate variables: Where, Indicates that it is used to replace during the optimization process The intermediate variable of Intermediate variables representing positions; An intermediate variable representing speed; Intermediate variables representing posture; Intermediate variables representing feature points in the image; Will Replace the original predicted value: Repeat steps 1), 2), and 3) for multiple optimizations until R converges and outputs 2. The monocular VIO fusion RTK system based on incremental graph optimization according to claim 1 is characterized in that The data acquisition and synchronization module specifically performs the following operations: Get IMU, image and RTK data: a=[a x ,a y ,a z ] T in=[in x ,In y ,In z ] T z visual =[u,v] T p RTK =[x,y,z] v RTK =[v x ,v y ,v z ] q RTK =[q w ,q x ,q y ,q z ] In the formula, a represents acceleration, a x 、a y 、a z They represent the acceleration components on the x, y, and z axes respectively; w represents the angular velocity, w x 、w y 、w z Represents the angular velocity components on the x, y, and z axes respectively; z visual Represents the coordinates of the feature points obtained from the image data, u and v represent the x-axis and y-axis coordinates of the feature points in the image respectively; [·] T represents transpose; p RTK Indicates the position coordinates in RTK data; v RTK Indicates the speed information in RTK data, v x 、v y 、v z Represents the velocity components on the x, y, and z axes respectively; q RTK Represents the attitude quaternion, q w Represents the scalar part of the quaternion, which is related to the cosine of the rotation angle, q x ,q y ,q z is the imaginary part of the quaternion, corresponding to the direction of the rotation axis and the sine part of the rotation angle; After obtaining the above data, synchronize the data to ensure that all types of data have matching timestamps and output the data.

3. The monocular VIO fusion RTK system based on incremental graph optimization according to claim 2 is characterized in that The initial state estimation module specifically performs the following operations: 1) Use the VIO algorithm to combine the synchronized IMU and visual data to obtain the initial state estimate: x0=[p0,v0,q0,z visual,0 ] p0=[x p0 ,y p0 ,z p0 ] T v0=[v x0 ,v y0 ,v z0 ] T with visual,0 =[u0,h0] T In the formula, x0 represents the initial state; p0 represents the initial position, x p0 、y p0 、z p0 They represent the components of the initial position on the x, y, and z axes respectively; v0 represents the initial velocity, v x0 、v y0 、v z0 Respectively represent the components of the initial velocity on the x, y, and z axes; q0 represents the initial attitude quaternion, Indicates the angle of rotation, Represents the components of the rotation axis on the x, y, and z axes respectively; z visual,0 Indicates the x- and y-axis coordinates (u0, h0) of the feature points in the initial image; 2) Calibrate using the global position provided by RTK: Where p0′ is the initial estimated global position, is the accurate position provided by RTK, replace p0 in x0 with That is, p0′ is replaced to achieve global position calibration; The replaced x0 is output as the initial state and also serves as the first node in constructing the incremental state graph.

Images