Binocular vision-IMU-RTK fusion positioning method based on group algebra

Abstract

The invention relates to a binocular vision-IMU-RTK fusion positioning method based on group algebra, and belongs to the technical field of positioning. The method comprises the following steps: defining a state variable of the IMU and a corresponding state error variable; measuring values of the corresponding variables are obtained through the IMU so as to carry out pose state prediction; the pose state predicted by the low-precision IMU is updated by adopting an observation value of binocular vision, the pose of the camera is added into a system state, the system state and a covariance matrix are updated through a re-projection error, and the pose of the camera is estimated in real time; and the low-precision IMU predicted pose state updated by the binocular vision measurement value in the system is updated again through the RTK observation value, and tight coupling of the low IMU, the binocular vision and the RTK is realized based on group algebraic Kalman filtering, so that final pose information is obtained. According to the invention, the cost problem caused by the adoption of a high-precision IMU during positioning is solved, and the robustness and precision of positioning can be improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot and automatic driving positioning, and relates to a positioning method, and in particular to a binocular vision-IMU-RTK fusion positioning method based on group algebra. Background Art

[0002] The combined fusion method of binocular vision, IMU, and RTK is a new research direction for modern positioning technology. Binocular vision can obtain three-dimensional spatial information, IMU (inertial measurement unit) can provide real-time three-axis angular velocity and three-axis acceleration, and RTK (Real-time kinematic, real-time dynamic, carrier phase difference technology) can provide centimeter-level positioning accuracy.

[0003] IMU and RTK are mainly used in the Global Navigation Satellite System (GNSS). IMU can continuously track the position and velocity changes of the device by measuring acceleration and angular velocity, and plays an important role in environments with poor GNSS signals. However, due to the accumulation of sensor errors, the navigation performance will be significantly reduced over a long period of time. RTK, as a high-precision positioning technology in GNSS, can achieve centimeter-level accuracy, but it needs to communicate with a base station with a known location and is easily affected by signal blocking and multipath effects.

[0004] With the rapid development of unmanned driving and other needs, higher precision, stronger robustness and lower cost are required. A single positioning method can no longer meet these needs. Therefore, attempts have been made to integrate multiple technologies to jointly improve the positioning performance of equipment. Binocular vision-IMU-RTK fusion positioning based on group algebra is a new positioning method that effectively utilizes the advantages of the three to enhance robustness and precision while reducing costs.

[0005] Binocular vision can obtain two-dimensional or three-dimensional spatial information, IMU can provide real-time three-axis angular velocity and three-axis acceleration, and RTK can provide centimeter-level positioning accuracy.

[0006] Wuhan University of Technology proposed a binocular vision positioning and map construction method for mobile robots in agricultural scenarios. By adding static stereo matching points, the available map points are increased, the accuracy of pose estimation is improved, and the operating efficiency and accuracy of the system are improved.

[0007] Beijing University of Technology proposed an outdoor positioning method that integrates binocular vision and 2D laser radar. This algorithm overcomes the problems of sparse 2D laser radar point cloud and noise caused by dynamic targets, and successfully uses 2D laser radar to achieve high-precision positioning in outdoor traffic scenes. However, when this method is used in some large scenes without geometric shape changes, such as highways, there will be slightly larger positioning errors.

[0008] Zhengzhou University proposed a mobile robot positioning method that integrates binocular vision and inertial navigation. The algorithm uses the instantaneous high-precision measurement data of the IMU to compensate for the less robust visual data, and also uses the relative robustness of long-term visual data to suppress the cumulative drift error of the IMU data.

[0009] However, these methods all have their own limitations and cannot combine the advantages of the three.

[0010] Deficiencies of existing technology:

[0011] 1. Economic benefits

[0012] In the classic positioning system that integrates satellite positioning system (GNNS) and inertial measurement unit (IMU), when the satellite signal is blocked, it cannot provide continuous and reliable global positioning information. The positioning system relies on IMU measurements for track dead reckoning to achieve reliable positioning. In this case, the positioning accuracy of the system depends on the accuracy of the IMU sensor. However, high-precision IMUs are expensive (usually more than hundreds of thousands of yuan), making them difficult to apply to mobile robots on a large scale.

[0013] 2. Technical level

[0014] A single sensor is difficult to adapt to complex outdoor environments. For example, RTK positioning equipment is affected by the strength of satellite signals. In scenes such as jungles, the satellite signal is blocked, resulting in reduced positioning accuracy or even positioning failure. Inertial sensors are not affected by external factors and only rely on their own measurements to estimate the carrier state. However, long-term positioning based on inertial sensors will result in cumulative errors, and high-precision inertial sensors are expensive, making them difficult to promote and apply on a large scale. Visual sensors can perceive environmental color and texture information and estimate the carrier's posture through image matching algorithms, but the algorithms are subject to environmental conditions and lighting constraints.

[0015] When the satellite signal is blocked, the positioning accuracy decreases. Although the carrier state can be estimated by relying on the high-precision IMU's own measurements, high-precision inertial sensors are expensive, which will incur high costs and hinder the development of the industry, especially the outdoor mobile robot industry.

[0016] In addition, RTK and IMU each exhibit unique advantages and limitations. In some special environments such as jungles, RTK may reduce positioning accuracy due to obstruction of satellite signals, or even fail to complete positioning. However, IMU has relatively strong stability and can work independently in a wide range of external environments, relying only on internal measurements to estimate the status of the device. However, when IMU is running for a long time, errors will accumulate, affecting the accuracy of positioning. Visual sensors have the ability to recognize color and texture information in the environment, and the image matching algorithm can be used to estimate the carrier's position and posture. However, the operation effect of this algorithm will be affected by the environmental state and lighting conditions. Summary of the invention

[0017] In view of this, the purpose of the present invention is to provide a binocular vision-IMU-RTK fusion positioning method based on group algebra. The present invention takes into account the typical scene requirements of low-speed mobile robots, adopts low-cost IMU and visual sensor for fusion, realizes reliable track calculation in a short time, replaces high-precision IMU, reduces the cost of positioning system, solves the problem of high-precision IMU being expensive in classic fusion positioning system, promotes the application of satellite positioning in outdoor mobile robots, and promotes the development of outdoor mobile robots. The multi-sensor fusion method is used to enhance the robustness and accuracy of the positioning system, and solves the problem that a single sensor cannot cope with complex outdoor environments.

[0018] In order to achieve the above object, the present invention provides the following technical solutions:

[0019] A binocular vision-IMU-RTK fusion positioning method based on group algebra, the method comprising the following steps:

[0020] S1. Define the state variables of IMU and the corresponding state error variables;

[0021] S2, obtain the measurement value of the corresponding variable through IMU to predict the posture state;

[0022] S3, using binocular vision observations to update the pose state predicted by the low-precision IMU, adding the camera pose to the system state, and then updating the system state and covariance matrix through the reprojection error to estimate the camera pose in real time;

[0023] S4. The low-precision IMU predicted posture state updated by the binocular vision measurement value in the system is updated again through the RTK observation value, and the tight coupling of the low-precision IMU with binocular vision and RTK is realized based on the group algebraic Kalman filter to obtain the final posture information.

[0024] Further, in step S1, the state variable x of the IMU is imu Defined as containing the IMU attitude quaternion q bw, position p w , speed v w , accelerometer bias coefficient b a and gyro bias coefficient b g A vector of:

[0025]

[0026] Where q represents the quaternion q=[q x q y q z q w ] T ;

[0027] The error model of IMU is defined as:

[0028]

[0029] in, Represents the error of IMU attitude in quaternion expression, represents the position error, represents the speed error, represents the accelerometer bias coefficient error, Gyroscope bias coefficient error;

[0030] The system state error variable is composed of the IMU state at time k and the camera pose at N historical moments. Only the pose of the camera on the left is stored in the sliding window, so the system state error variable is:

[0031]

[0032] in is the i-th left camera pose in the sliding window.

[0033] Further, in step S2, the IMU kinematic model at continuous time t is obtained by IMU:

[0034]

[0035] Among them, a m (t) and w m (t) is the measured value of linear acceleration and angular velocity, n wg and n wa are the random walk coefficients of the gyroscope and accelerometer, respectively. Ω(.) represents the function of converting a vector into an antisymmetric matrix. R(.) represents the mapping function from a quaternion vector to a rotation matrix. g is the gravity vector.

[0036] Rewrite the IMU kinematic model under the above continuous time t into matrix form:

[0037]

[0038] Among them, the matrices F and G are:

[0039]

[0040] in, represents the estimated angular velocity, represents the estimated value of acceleration, n imu =[n g n wg n a n wa ] T is a zero-mean Gaussian process noise, n g and n a are the noise densities of the gyroscope and accelerometer, respectively;

[0041] Integrating matrices F and G, we get the discrete-time state transfer matrix and noise covariance matrix:

[0042]

[0043] Among them, Φ imu (k) is the discrete time state transfer matrix at time k, Q imu (k) is the noise covariance matrix at time k.

[0044] Further, in step S3, the IMU kinematic model under continuous time t is integrated using the fourth-order Runge-Kutta method to obtain the position and posture of the IMU at time k, and the covariance matrix corresponding to the system is updated as:

[0045]

[0046] Among them, P II Represents the covariance matrix corresponding to the IMU state variables, P IC represents the covariance matrix between the IMU state variables and the camera state in the sliding window, P CC Describes the covariance matrix corresponding to the camera state in the sliding window.

[0047] Further, in step S3, when a new image is received, the left and right camera poses at time k are predicted based on the IMU pose at time k:

[0048]

[0049] in, and Respectively represent the attitude matrices of the left and right cameras, and Respectively represent the positions of the left and right cameras;

[0050] After calculating the left camera state at time k, add it to the sliding window and augment the system covariance matrix:

[0051]

[0052] Among them, the matrix J is:

[0053]

[0054] The matrix J is the derivative of the newly added left camera state with respect to the system state variables.

[0055] Further, in step S3, the process of updating the system state X and the covariance matrix by reprojection error is:

[0056] With 3D map point P j =[X j ,Y j ,Z j ] T Observed by the camera, according to the projection model of the pinhole camera, the map point P j The projection points in the left and right cameras satisfy:

[0057]

[0058] in, is the relative rotation and translation transformation between the left and right cameras, so the map point P j The current measurements for the left and right cameras are:

[0059]

[0060] Then define the map point P j The reprojection error is:

[0061]

[0062] The measurement model is linearized, and we have:

[0063]

[0064] in:

[0065]

[0066] and are the derivatives of the reprojection error with respect to the feature and camera pose errors, respectively, and is the pixel coordinate relative to the map point P j The Jacobian matrix of ;

[0067] When the map point P j When it is observed by multiple cameras at historical moments in the sliding window, the matrix and To stack:

[0068]

[0069] The reprojection error r of the map point j Projection to the observation matrix H f,j In the left null space of item:

[0070]

[0071] For all feature points observed at time k, the complete observation matrix and error vector are:

[0072]

[0073] right The matrix is ​​QR-decomposed to reduce the dimension of the matrix, and the error vector is updated as follows:

[0074]

[0075] According to the EKF update step, the system state variables are updated as:

[0076]

[0077] Among them, K is the gain matrix of Kalman filter, P k|k is the posterior estimate of the covariance matrix at time k.

[0078] Further, in step S4, based on the group algebra theory, the system state variables are further updated in combination with the latitude, longitude and altitude information output by the RTK module. First, the latitude and longitude coordinates output by the RTK module are converted to the east-north-sky coordinate system P RTK =[xyz 1]:

[0079] The measurement value update model under group algebraic Kalman filtering is as follows:

[0080]

[0081] Let the error between the satellite observation and the system state variable be It is expressed as:

[0082]

[0083] is a 7x1 vector, take Upper left corner:

[0084]

[0085] Under RTK observation, the status update is:

[0086]

[0087] At the same time, the covariance matrix of the system is updated as:

[0088]

[0089] in:

[0090]

[0091] Represents the gain matrix of the GNSS observation part at time k.

[0092] The beneficial effects of the present invention are:

[0093] 1. The present invention significantly reduces the cost of the product: the combined positioning system integrates binocular vision, inertial measurement unit (IMU) and real-time dynamic differential (RTK) positioning, and uses group algebra for processing. And it achieves substantial cost reduction under the premise of ensuring positioning accuracy.

[0094] 2. The present invention can improve the robustness and accuracy of the positioning system: According to the sensor fusion technology, the advantages of each sensor are utilized to greatly improve the adaptability and stability of the system in an uncertain environment, and further enhance the positioning accuracy. Such advantages ensure that the system can continuously and accurately provide accurate positioning in a complex and unstable environment.

[0095] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0097] Figure 1 It is the overall process framework of the binocular vision-IMU-RTK fusion positioning method based on group algebra of the present invention;

[0098] Figure 2 This is a diagram showing an example of a positioning device capable of carrying the present invention in a preferred embodiment;

[0099] Figure 3 It is a schematic diagram comparing the output trajectory of the binocular vision-IMU-RTK fusion positioning method based on group algebra of the present invention with the existing method. DETAILED DESCRIPTION

[0100] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0101] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0102] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0103] See also Figure 1 to Figure 3 , which is a binocular vision-IMU-RTK fusion positioning method based on group algebra.

[0104] like Figure 1 The overall process framework of the binocular vision-IMU-RTK fusion positioning method based on group algebra of the present invention shown includes the following steps:

[0105] S1. Define the state variables of the low-precision IMU and the corresponding state error variables;

[0106] S2, obtain the measurement value of the corresponding variable through the low-precision IMU to predict the posture state;

[0107] S3, using binocular vision observations to update the pose state predicted by the low-precision IMU, adding the camera pose to the system state, and then updating the system state X and covariance matrix through the reprojection error to estimate the camera pose in real time;

[0108] S4. The low-precision IMU predicted posture state updated by the binocular vision measurement value in the system is updated again through the RTK observation value, and the tight coupling of the low-precision IMU with binocular vision and RTK is realized based on the group algebraic Kalman filter to obtain the final posture information.

[0109] Example

[0110] In this embodiment, in step S1, the state variables of the IMU and the state error variables of the entire system are defined. imu Defined as a 16-dimensional vector containing the IMU attitude quaternion q bw , position p w , speed v w , accelerometer bias coefficient b a and gyro bias coefficient b g .

[0111]

[0112] Where q represents the quaternion q=[q x q y q z q w ] T .

[0113] The error model of IMU is defined as:

[0114]

[0115] in, Represents the IMU attitude quaternion error, represents the position error, represents the speed error, represents the accelerometer bias coefficient error, Gyroscope bias coefficient error.

[0116] The system state error variable is defined as the IMU state at time k and the camera pose at N historical moments. Since the extrinsic parameters of the left and right cameras are fixed and known, in order to reduce the dimension of the state variable, only the pose of the camera on the left is stored in the sliding window. Therefore, the system state error variable is:

[0117]

[0118] in is the i-th left camera pose in the sliding window.

[0119] In step S2, the present embodiment makes predictions through the defined IMU state variables. First, the IMU kinematic model under continuous time t is obtained through the low-precision IMU:

[0120]

[0121] Among them, a m (t) and w m (t) is the measured value of linear acceleration and angular velocity. wg and n wa are the random walk coefficients of the gyroscope and accelerometer, respectively. Ω(.) represents the function of converting a vector into an antisymmetric matrix. R(.) represents the mapping function from a quaternion vector to a rotation matrix, and g is the gravitational acceleration.

[0122] The above IMU kinematic model under continuous time t, that is, formula (4), is rewritten into matrix form:

[0123]

[0124] Among them, the matrices F and G are:

[0125]

[0126] in represents the estimated angular velocity, Represents the estimated value of acceleration. imu =[n g n wg n a n wa ] T is a zero-mean Gaussian process noise, n g and n a are the noise densities of the gyroscope and accelerometer, respectively.

[0127] By integrating matrices F and G, we can obtain the discrete-time state transfer matrix and noise covariance matrix as follows:

[0128]

[0129] Among them, Φ imu (k) is the discrete time state transfer matrix at time k, Q imu (k) is the noise covariance matrix at time k.

[0130] In this embodiment, in step S3, the posture state predicted by the low-precision IMU is updated through binocular vision.

[0131] The position and posture of the IMU at time k can be obtained by integrating equation (4) using the 4th-order Runge-Kutta method. The covariance matrix corresponding to the system is updated as:

[0132]

[0133] Among them, P II Represents the covariance matrix corresponding to the IMU state variables, P IC represents the covariance matrix between the IMU state variables and the camera state in the sliding window, P CC Describes the covariance matrix corresponding to the camera state in the sliding window.

[0134] Since the camera and IMU are rigidly connected and the external parameters between the camera and IMU are known, when a new image is received, the left and right camera poses at time k can be predicted based on the IMU pose at time k:

[0135]

[0136] in, and Respectively represent the attitude matrices of the left and right cameras, and Respectively represent the positions of the left and right cameras.

[0137] After calculating the left camera state at time k, add it to the sliding window and augment the system covariance matrix:

[0138]

[0139] Among them, the J matrix is ​​the derivative of the newly added left camera state with respect to the system state variable:

[0140]

[0141] Assume there is a 3D map point P j =[X j ,Y j ,Z j ] T Observed by the camera, according to the projection model of the pinhole camera, the projection points of the map points in the left and right cameras satisfy:

[0142]

[0143] in, is the relative rotation and translation transformation between the left and right cameras. Therefore, the map point P j The current measurements for the left and right cameras are:

[0144]

[0145] Therefore, define the map point P j The reprojection error is:

[0146]

[0147] By linearizing the measurement model, we can obtain:

[0148]

[0149] in:

[0150]

[0151] and are the derivatives of the reprojection error with respect to the feature and camera pose errors, respectively. and is the pixel coordinate relative to the map point P j The Jacobian matrix of .

[0152] When the map point P j When it is observed by multiple cameras at historical moments in the sliding window, the matrix and To stack:

[0153]

[0154] In order to eliminate the influence of map points on system state variables, the reprojection error r of the map points is j Projection to the observation matrix H f,j In the left null space of item:

[0155]

[0156] For all feature points observed at time k, the complete observation matrix and error vector are:

[0157]

[0158] right The matrix is ​​QR-decomposed to reduce the dimension of the matrix and the amount of computation required for subsequent matrix operations. Therefore, the error vector is updated as:

[0159]

[0160] According to the EKF update step, the system state variables are updated as:

[0161]

[0162] Where K is the Kalman filter gain matrix, P k|k is the posterior covariance matrix at time k.

[0163] In summary, when the IMU measurement data is received, the gyroscope and acceleration measurements are first integrated according to the kinematic model to update the IMU state variables. Then, the camera pose at time k is estimated by equation (9), and the left camera pose at time k is added to the system state X. Finally, the system state X and the covariance matrix are updated using the reprojection error of the 3D map points to estimate the camera pose in real time.

[0164] In step S4, the state variables in the system are updated again by using the RTK measurement values. Since the visual odometer and IMU are essentially cumulative positioning, there will inevitably be cumulative errors in long-term operation. Therefore, the longitude, latitude and altitude information output by the RTK module is used to further update the system state variables. First, the longitude and latitude coordinates output by the RTK module are converted to the East-North-Up coordinate system (East-North-Up, ENU)P RTK =[xyz 1].

[0165] The measurement value update model under group algebraic Kalman filtering is as follows:

[0166]

[0167] Let the error between the RTK observation and the system state variable be It can be expressed as:

[0168]

[0169] From the form is a 7x1 vector, take Upper left corner:

[0170]

[0171] Therefore, the status update under RTK observation is:

[0172]

[0173] At the same time, the covariance matrix of the system is updated as:

[0174]

[0175] in:

[0176]

[0177] The processing flow of the present invention enhances the accuracy of environmental perception and posture estimation, and ensures the scientificity and stability of system operation.

[0178] like Figure 2 As shown, this embodiment also provides a positioning device capable of implementing the method of the present invention, which includes a ZED binocular camera, a low-precision IMU unit and an RTK unit. The relative rotation and translation between the camera and the IMU are calibrated in an offline manner.

[0179] Figure 3 The fusion results of the binocular vision-IMU-RTK fusion positioning method based on group algebra of the present invention are shown. The three trajectory curves in the figure are the position of the RTK measurement value converted to the local coordinate system of the northeast sky, the value output by the visual odometer, and the value output by the fusion positioning method of the present invention. Figure 3 It can be seen that as the running time of the visual odometer increases, the pose of the algorithm trajectory gradually deviates from the real pose, and the error shows a cumulative increasing trend. After integrating the RTK measurement value, the trajectory is closer to the real trajectory.

[0180] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A binocular vision-IMU-RTK fusion positioning method based on group algebra, characterized by: The method comprises the following steps: S1. Define the state variables of IMU and the corresponding state error variables; S2, obtain the measurement value of the corresponding variable through IMU to predict the posture state; S3, using binocular vision observations to update the pose state predicted by the low-precision IMU, adding the camera pose to the system state, and then updating the system state and covariance matrix through the reprojection error to estimate the camera pose in real time; S4. The low-precision IMU predicted posture state updated by the binocular vision measurement value in the system is updated again through the RTK observation value, and the tight coupling of the low-precision IMU with binocular vision and RTK is realized based on the group algebraic Kalman filter to obtain the final posture information.

2. The binocular vision-IMU-RTK fusion positioning method based on group algebra according to claim 1, characterized in that: In step S1, the state variable x of the IMU is imu Defined as containing the IMU attitude quaternion q bw , position p w , speed v w , accelerometer bias coefficient b a and gyro bias coefficient b g A vector of: Where q represents the quaternion q=[q x q y q z q w ] T , where T represents the transposition of the matrix, q x q y q z The x, y, z components of the quaternion, q w represents the real part of the quaternion, Represents vector x imu The dimension of is 16; The error model of IMU is defined as: in, Represents the IMU attitude quaternion error, represents the position error, represents the speed error, represents the accelerometer bias coefficient error, Gyroscope bias coefficient error, Representation vector The dimension of is 15; The system state error variable is composed of the IMU state at time k and the camera pose at N historical moments. Only the pose of the camera on the left is stored in the sliding window, so the system state error variable is: in is the i-th left camera pose in the sliding window, are attitude error and position error respectively.

3. The binocular vision-IMU-RTK fusion positioning method based on group algebra according to claim 2, characterized in that: In step S2, the IMU kinematic model at continuous time t is obtained through the IMU: Among them, a m (t) and w m (t) is the measured value of linear acceleration and angular velocity, n wg and n wa are the random walk coefficients of the gyroscope and accelerometer, respectively. Ω(.) represents the function of converting a vector into an antisymmetric matrix. R(.) represents the mapping function from a quaternion vector to a rotation matrix. g is the gravitational acceleration. Rewrite the IMU kinematic model under the above continuous time t into matrix form: Among them, the matrices F and G are: in, represents the estimated angular velocity, represents the estimated value of acceleration, n imu =[n g n wg n a n wa ] T is a zero-mean Gaussian process noise, n g and n a are the noise densities of the gyroscope and accelerometer, respectively; Integrating matrices F and G, we get the discrete-time state transfer matrix and noise covariance matrix: Among them, Φ imu (k) is the discrete time state transfer matrix at time k, Q imu (k) is the noise covariance matrix at time k.

4. The binocular vision-IMU-RTK fusion positioning method based on group algebra according to claim 3 is characterized in that: In step S3, the IMU kinematic model under continuous time t is integrated using the fourth-order Runge-Kutta method to obtain the position and posture of the IMU at time k, and the covariance matrix corresponding to the system is updated as: Among them, P II Represents the covariance matrix corresponding to the IMU state variables, P IC represents the covariance matrix between the IMU state variables and the camera state in the sliding window, P CC Describes the covariance matrix corresponding to the camera state in the sliding window.

5. The binocular vision-IMU-RTK fusion positioning method based on group algebra according to claim 4 is characterized in that: In step S3, when a new image is received, the left and right camera poses at time k are predicted based on the IMU pose at time k: in, and Respectively represent the attitude matrices of the left and right cameras, and Respectively represent the positions of the left and right cameras; After calculating the left camera state at time k, add it to the sliding window and augment the system covariance matrix: Among them, the matrix J is: The matrix J is the derivative of the newly added left camera state with respect to the system state variables.

6. The binocular vision-IMU-RTK fusion positioning method based on group algebra according to claim 5, characterized in that: In step S3, the process of updating the system state X and the covariance matrix by reprojection error is: With 3D map point P j =[X j ,Y j ,Z j ] T Observed by the camera, according to the projection model of the pinhole camera, the map point P j The projection points in the left and right cameras satisfy: in, is the relative rotation and translation transformation between the left and right cameras, so the map point P j The current measurements for the left and right cameras are: Then define the map point P j The reprojection error is: The measurement model is linearized, and we have: in: and are the derivatives of the reprojection error with respect to the feature and camera pose errors, respectively, and is the pixel coordinate relative to the map point P j The Jacobian matrix of ; When the map point P j When it is observed by multiple cameras at historical moments in the sliding window, the matrix and To stack: The reprojection error r of the map point j Projection to the observation matrix H f,j In the left null space of item: For all feature points observed at time k, the complete observation matrix and error vector are: right The matrix is ​​QR-decomposed to reduce the dimension of the matrix, and the error vector is updated as follows: According to the EKF update step, the system state variables are updated as: Where K is the Kalman filter gain matrix, P k|k is the posterior covariance matrix at time k.

7. The binocular vision-IMU-RTK fusion positioning method based on group algebra according to claim 6, characterized in that: In step S4, the latitude, longitude and altitude information output by the RTK module is used to further update the system state variables. First, the latitude and longitude coordinates output by the RTK module are converted to the east-north-sky coordinate system P RTK =[xyz 1]: The measurement value update model under group algebraic Kalman filtering is as follows: Let the error between the RTK observation and the system state variable be It is expressed as: is a 7x1 vector, take Upper left corner: Under RTK observation, the status update is: At the same time, the covariance matrix of the system is updated as: in: Represents the gain matrix of the RTK observation part at time k.

Images