A visual-inertial filtering method based on manifold processing

Abstract

The application discloses a visual inertial filtering method based on manifold processing and belongs to the field of mobile robot positioning, and comprises the following steps: obtaining updated pose information based on an inertial IMU sensor; performing Lie group construction based on the updated pose information and position information of a plurality of landmark points to obtain complete state of the robot at the current time containing deviation information; constructing uncertainty representation, a state model and a measurement model of invariant Kalman filtering based on UKF at the current time based on the Lie group and the deviation information; updating the state model of the next time based on the state model; updating the state covariance matrix decomposition factor of the next time based on the uncertainty of state propagation and a process noise covariance matrix; if there is visual observation data of a landmark at the next time, calculating an observation value based on the measurement model and updating the state and the state covariance matrix decomposition factor at the time in combination with the observation value, or otherwise, repeating the steps of updating the state model of the next time and the state covariance matrix decomposition factor.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mobile robot positioning, and particularly to a visual-inertial filtering method based on manifold processing. BACKGROUND

[0002] In recent years, with the development of science and technology, the research and application of positioning technology show a rapid development momentum. However, due to the difficulty of a single positioning system to meet the needs of actual positioning applications, the positioning service in a comprehensive environment needs a positioning technology with strong practicability and good stability. Research shows that the visual sensor has high precision but poor stability, while the inertial measurement unit (IMU) has high stability but is prone to cumulative error when used for a long time. The fusion of visual positioning and inertial positioning technology can complement each other's advantages and gradually develop into a research hotspot in the field of navigation and mobile robot positioning.

[0003] However, in visual-inertial positioning, the manifold-related problem is particularly critical, especially when dealing with rotation, pose and IMU data. The traditional linear space operation cannot meet the requirements of precision and robustness. Since the pose change in positioning involves rotation and displacement, the operation on the manifold is often more complex, and the calculation is also more complex. More accurate processing is needed in the special manifold structure of Lie group. By embedding the state space into the Lie group, the non-linear problems of rotation and pose can be effectively handled. SUMMARY

[0004] The purpose of the present application is to provide a visual-inertial filtering method based on manifold processing, which embeds the state space into the constructed Lie group, more accurately handles the non-linear problems in visual-inertial positioning in the special manifold structure of Lie group, and improves the stability and robustness of the system on the complex manifold structure. The present application is realized by the following technical solutions.

[0005] The present application provides a visual-inertial filtering method based on manifold processing, comprising:

[0006] S1, obtaining the pose information of the robot based on the inertial IMU sensor, performing integral operation on the pose information based on the manifold-based IMU pre-integration method to obtain the pose information increment, and obtaining the updated pose information based on the pose information increment;

[0007] S2, constructing Lie group based on the updated pose information and the position information of a plurality of landmark points, embedding the state space into the Lie group, and obtaining the complete state of the robot at the current time containing bias information;

[0008] S3, constructing the uncertainty representation, state model and measurement model of the current time of the invariant Kalman filter based on UKF based on the Lie group and the bias information;

[0009] S4, updating the state model of the next time based on the unbiased input and the state model of the current time; updating the state covariance matrix decomposition factor of the next time based on the uncertainty of the current state propagation and the process noise covariance matrix;

[0010] S5, if there is visual observation data of the landmark at the next time, calculating the visual observation value based on the measurement model and updating the state model and the state covariance matrix decomposition factor of the time combined with the visual observation value, otherwise jumping to step S4;

[0011] S6, after entering the next time, repeating steps S4-S5.

[0012] In actual application, the algorithm of the application is strictly implemented according to the above steps. First, the pose information of the mobile robot at the current time, i.e. the rotation matrix R, the velocity vector v and the position vector x, is obtained by using the inertial IMU sensor, and the pose information increment is obtained by using the manifold-based IMU pre-integration method. In actual application, the pose information of the mobile robot at the next time can be calculated by combining the pose information of the mobile robot at the current time and the pose information increment. The manifold-based IMU pre-integration method makes the processing of IMU data more accurate and simplifies the calculation complexity. By calculating the updated pose information of the mobile robot at the next time and the position information of a plurality of landmark points to construct the Lie group, the complete state of the mobile robot at the current time containing bias information can be obtained. By embedding the state space into the Lie group, the nonlinear problem of rotation and pose can be effectively handled.

[0013] Then, the uncertainty representation, the state model and the measurement model of the current time based on the UKF invariant Kalman filter are constructed by using the constructed Lie group and the bias information. The UKF invariant Kalman filter algorithm can ensure the high-precision performance of visual-inertial positioning in complex environments. The state model of the next time is updated based on the unbiased input and the state model of the current time; the state covariance matrix decomposition factor of the next time is updated based on the uncertainty of the current state propagation and the process noise covariance matrix. Finally, it is judged whether there is visual observation data of the mobile robot at the time by using the visual sensor. If there is, the observation data of the mobile robot at each landmark point is observed by using the visual sensor, and the state model and the state covariance matrix decomposition factor of the mobile robot at the time are updated combined with the obtained visual observation data, otherwise the steps of updating the state model and the state covariance matrix decomposition factor of the next time are repeated. By combining the visual observation data and using the landmark information provided by the visual sensor, the state estimation of the robot can be more accurate, the error of the system can be reduced, and the ability of the system to cope with complex and uncertain environments can be enhanced.

[0014] Optionally, in step S1, the pose information comprises a rotation matrix , a velocity vector , and a position vector ; the manifold-based IMU pre-integration method integrates the pose information to obtain the pose information increment, specifically through the following formula:

[0015]

[0016] wherein denote rotation increment, velocity increment and position increment of the robot from time i to time j , respectively, and denote rotation matrix of the robot at time i to time j , respectively, and denote velocity vector of the robot at time i to time j , respectively, and denote position vector of the robot at time i to time j , respectively; denotes rotation noise of the robot at time i to time j , i.e. angle error, which is zero-mean Gaussian noise; denotes mapping of angle error from Lie algebra back to Lie group using exponential mapping (exp), denote velocity noise and position noise of the robot from time i to time j , both of which are zero-mean Gaussian noise, denotes time interval from time i to time j , denotes gravity vector.

[0017] Optionally, in step S1, the updated pose information comprises an updated rotation matrix , an updated velocity vector , and an updated position vector .

[0018] In step S2, the Lie group construction based on the updated pose information and the plurality of landmark point position information comprises constructing the Lie group through the updated rotation matrix , the updated velocity vector , the updated position vector , and the positions of the plurality of landmark points ​Constructing Lie group , the Lie group represents the state vector of the robot at the current time n, the Lie group has the following expression:

[0019] ,

[0020] In the formula, represents a zero matrix of the Lie group , represents a unit matrix of the Lie group , represents the position information of the robot, represents the number of observed landmark points; m p In step S2, the complete state of the robot at the current time containing bias information includes that the complete state of the robot at the current time n containing bias information is represented as , wherein

[0021] is the bias vector of the inertial IMU sensor, and is represented as:

[0022] ,

[0023] In the formula, is the bias of the gyroscope, is the bias of the accelerometer, and the two are combined into a bias vector .

[0024] Optionally, in step S3, the uncertainty representation includes the state uncertainty representation and the bias uncertainty representation, the state uncertainty representation is represented by a state error , the bias uncertainty representation is represented by a bias error , the state error and the bias error are subject to a Gaussian distribution with a mean of 0 and a covariance of , and are represented as:

[0025] ,

[0026] The state covariance matrix describes the uncertainty of the state error and the bias error , and the Cholesky factor of the state covariance matrix can be obtained by Cholesky decomposition, and the decomposition process is as follows: ,

[0027] In the formula, S is the state covariance matrix ​​Cholesky factor obtained by Cholesky decomposition, which is a decomposition method of decomposing a positive definite matrix into a lower triangular matrix and its transpose. Compared with directly operating the covariance , Cholesky factor reduces the complexity of matrix operation during operation, and can significantly reduce operation error.

[0028] The operation of the state vector of the robot at the current time n is as follows:

[0029] ,

[0030] In the formula, represents the state vector of the robot at the current time n, represents the state mean of the state vector , and represents the state error, represents the state error mapped back to the Lie group from the Lie algebra through exponential mapping exp, and the state mean is corrected.

[0031] The operation of the bias vector of the inertial IMU sensor at the current time n is as follows:

[0032] ,

[0033] In the formula, represents the bias vector of the inertial IMU sensor at the current time n, represents the bias mean of the bias vector , and represents the bias error.

[0034] In step S3, the state model f expression is:

[0035] ,

[0036] represents the state vector of the robot at the previous time n-1, represents the bias vector of the inertial IMU sensor at the previous time n-1, represents the input vector of the state model f, including the angular velocity and acceleration measured by the inertial IMU sensor, represents the actual input of the state model f at the current time, which removes the influence of the bias of the inertial IMU sensor, represents the process noise, specifically the random noise of the system, which is subject to Gaussian distribution and is represented as , wherein Process noise The covariance matrix of .

[0037] Optionally, in step S3, the measurement model expression is:

[0038] ,

[0039] Where, represents the actual observation vector obtained by the visual sensor at the current time n, Represents the measurement model, describing the state vector n at the current moment and observation noise In the case of , the visual sensor's observation value of the landmark; observation noise Obey Gaussian distribution , represents the observation noise The covariance matrix of the measurement model Further expanded to:

[0040] ,

[0041] Where, Contains vision sensors for all The observation data of landmark points, p represents the number of landmark points, Represents the observation data of the i-th landmark, specifically the normalized pixel position of the landmark on the image plane in the camera coordinate system, and i represents the landmark point number.

[0042] Optionally, in step S4, updating the state model at the next moment based on the unbiased input and the state model at the current moment specifically includes:

[0043] S4.1, input preparation; set the input of the current n-state model to the state mean , mean deviation , the state covariance matrix Cholesky factorization of , the input vector and process noise The covariance matrix of ;

[0044] Among them, the mean deviation of the previous moment From the input vector Remove it and get the unbiased input vector of n at the current moment ,but ;

[0045] The covariance matrix Cholesky factorization of With matrix construction, get is:

[0046] ,

[0047] In the formula, blkdiag represents constructing a block diagonal matrix, Cholesky decomposition of the process noise covariance matrix , the state mean value at the current time n is retained, and ;

[0048] S4.2, propagate the state, update the state mean value and the mean value of the deviation ; based on the unbiased input vector and the state vector , the state vector and the deviation are propagated through the state model , to obtain the updated state mean value and the mean value of the deviation :

[0049] ,

[0050] In the formula, and respectively represent the updated state mean value and the mean value of the deviation;

[0051] The updated state mean value is taken as the state vector of the robot at the next time, and the updated mean value of the deviation is taken as the deviation vector of the inertial IMU sensor at the next time, and the state model f is updated.

[0052] Optionally, in step S4, the state covariance matrix decomposition factor at the next time is updated based on the current state propagated uncertainty and the process noise covariance matrix, including:

[0053] S4.3, generate and propagate Sigma points; the Sigma points are a set of special points that can capture the uncertainty of the system state, and the covariance matrix 2 Sigma points can be generated, and the Sigma points reflect the uncertainty of the state, and the Sigma points are obtained through the following formula:

[0054] ,

[0055] In the formula, is a scaling coefficient for adjusting the range of the Sigma point, represents a matrix The first column, for generating Sigma points, respectively represent the generated state error Sigma points, bias error Sigma points and process noise Sigma points;

[0056] The generated Sigma points are propagated through the state model to obtain the propagated Sigma points, and the operation process is as follows:

[0057] ,

[0058] In the formula, Sigma points are mapped back to the Lie group from the Lie algebra by exponential mapping exp, and the state mean value is corrected In order to handle nonlinear systems, Sigma points need to be mapped from the linearized Lie algebra to the Lie group, so that the Sigma points can more accurately represent the uncertainty of the state; respectively represent the propagated state error and bias error Sigma points generated;

[0059] The propagated Sigma points are mapped back to the Lie algebra from the Lie group by the following formula,

[0060] ,

[0061] In the formula, Sigma points are mapped back to the Lie algebra from the Lie group, represent the Sigma points mapped back to the Lie algebra, and the Cholesky factor can be updated in the local linear space ;

[0062] S4.4, update the Cholesky factor ; based on the Sigma points mapped back to the Lie algebra from the Lie group, the propagated Sigma points and the covariance matrix of the process noise , the Cholesky factor is updated by QR decomposition to obtain the updated Cholesky factor of the next time, and the QR decomposition is obtained by the following formula:

[0063] , ,

[0064] where the qr function represents QR decomposition; through QR decomposition, the positive definiteness and numerical stability of the covariance matrix in state propagation can be guaranteed;

[0065] The updated Cholesky factor is used to update the state covariance matrix decomposition factor .

[0066] Optionally, if there is visual observation data of the landmark at the next time, a visual observation value is calculated based on the measurement model, and the state model update and the update of the state covariance matrix decomposition factor at the time are performed in combination with the visual observation value, including:

[0067] S5.1, first, according to the measurement model , the landmark observation value without observation noise is calculated:

[0068] ,

[0069] wherein is the observation noise, is the updated state mean in step S4.2;

[0070] S5.2, based on the updated Cholesky factor in step S4.4, a new Sigma point is generated:

[0071] ,

[0072] wherein respectively represent the generated Sigma points of state error and bias error , and the Sigma points in the Lie algebra are converted into Sigma points in the Lie group through exponential mapping, and are obtained through the following formula:

[0073] ,

[0074] wherein represents the Sigma point mapped back to the Lie group through exponential mapping exp, represents the Sigma point mapped back to the Lie group,

[0075] the Sigma point is substituted into the measurement model Sigma points of the landmark observations with noise

[0076] ,

[0077] wherein are the Sigma points of the landmark observations with noise, is the covariance matrix of the observation noise

[0078] S5.3, using a QR decomposition, the correction values of the state and of the bias are calculated by comparison of the actual measurements and the predicted measurements, while updating the Cholesky factor which ensures numerical stability of the covariance matrix, said QR decomposition being obtained by the following formula:

[0079] , ,

[0080] wherein is the actual observation, a value obtained directly from the vision sensor, is the noise-free predicted measurement, calculated from step S5.1, and are the Sigma points of the measurement with noise and of the state error, respectively, is the covariance matrix of the observation noise

[0081] S5.4, using the correction values of the state and of the bias the state mean value and the bias mean value are updated:

[0082] ,

[0083] After the state update process, the updated state mean value , bias mean value and the Cholesky factor of the updated state covariance matrix P are obtained;

[0084] The state model f is updated again with the updated state mean value as the state vector of the robot at the next time instant, and with the updated bias mean value as the bias vector of the inertial IMU sensor at the next time instant; ​​​​

[0085] with the updated Cholesky factor to the state covariance matrix decomposition factor is updated.

[0086] Advantages

[0087] (1) The present application proposes an IMU pre-integration method based on manifold, which can correctly process the manifold structure; at the same time, the processing of IMU data is more accurate and the calculation complexity is simplified; the present application proposes an invariant Kalman filtering algorithm based on UKF, which can dynamically adjust the uncertainty of the system through the weighted combination of observation data and state prediction, and finally obtain more accurate state estimation, ensuring the high-precision performance of visual inertial positioning in complex environment.

[0088] (2) The present application also embeds the state space into the constructed Lie group, and more accurately processes the nonlinear problem in visual inertial positioning in the special manifold structure of Lie group, thereby improving the stability and robustness of the system on the complex manifold structure. BRIEF DESCRIPTION OF DRAWINGS

[0089] Figure 1 Fig. 1 shows a flowchart of the visual inertial filtering method based on manifold processing in an embodiment of the present application. DETAILED DESCRIPTION

[0090] The following is further described in combination with the drawings and specific embodiments.

[0091] Embodiment 1 provides a visual inertial filtering method based on manifold processing, which combines Figure 1 , including the following steps:

[0092] S1, obtaining the pose information of the robot based on the inertial IMU sensor, performing integration operation on the pose information based on the IMU pre-integration method based on manifold to obtain the pose information increment, and obtaining the updated pose information based on the pose information increment;

[0093] S2, constructing Lie group based on the updated pose information and the position information of a plurality of landmark points, embedding the state space into the Lie group, and obtaining the complete state of the robot at the current time containing bias information;

[0094] S3, constructing the uncertainty representation, state model and measurement model of the current time of the invariant Kalman filter based on UKF based on the Lie group and the bias information;

[0095] S4, updating the state model of the next time based on the unbiased input and the state model of the current time; updating the state covariance matrix decomposition factor of the next time based on the uncertainty of the current state propagation and the process noise covariance matrix.

[0096] S5, if there is visual observation data of the landmark at the next time, calculate the visual observation value based on the measurement model, and update the state model and the state covariance matrix decomposition factor at this time combined with the visual observation value, otherwise jump to step S4;

[0097] S6, after entering the next time, repeat steps S4-S5.

[0098] In actual application, the algorithm described in the application is strictly implemented according to the above steps. First, the pose information of the mobile robot at the current time, i.e. the rotation matrix R, the velocity vector v and the position vector x, is obtained by using the inertial IMU sensor, and the pose information increment is obtained by using the manifold-based IMU pre-integration method. In actual application, the pose information of the mobile robot at the current time and the pose information increment can be combined to calculate the pose information of the mobile robot at the next time. The manifold-based IMU pre-integration method makes the processing of IMU data more accurate and simplifies the calculation complexity. By calculating the updated pose information of the mobile robot at the next time and the position information of a plurality of landmark points, the Lie group is constructed, and the complete state of the mobile robot at the current time containing bias information can be obtained. By embedding the state space into the Lie group, the nonlinear problem of rotation and pose can be effectively handled.

[0099] Then, the uncertainty representation, the state model and the measurement model of the current time based on the UKF invariant Kalman filter are constructed by using the constructed Lie group and the bias information. The UKF invariant Kalman filter algorithm can ensure the high-precision performance of visual-inertial positioning in complex environments. Based on the unbiased input and the state model at the current time, the state model at the next time is updated; based on the uncertainty of the current state propagation and the process noise covariance matrix, the state covariance matrix decomposition factor at the next time is updated.

[0100] Finally, at this time, it is judged whether there is visual observation data of the mobile robot by using the visual sensor. If there is, the observation data of the mobile robot at each landmark point is observed by using the visual sensor and the state model and the state covariance matrix decomposition factor of the mobile robot at this time are updated combined with the obtained visual observation data, otherwise the steps of updating the state model and the state covariance matrix decomposition factor at the next time are repeated. By combining the visual observation data, the landmark information provided by the visual sensor can make the state estimation of the robot more accurate, reduce the error of the system, and enhance the ability of the system to cope with complex and uncertain environments.

[0101] Example 2: Based on Example 1, this example introduces a specific implementation process of a visual inertial filtering method based on manifold processing, which specifically includes the following contents:

[0102] In step S1, the posture information includes the rotation matrix , velocity vector and position vector The manifold-based IMU pre-integration method performs an integration operation on the pose information to obtain the pose information increment, which is specifically obtained by the following formula:

[0103] ,

[0104] Where, Respectively represent the robot from time i At the time j The rotation increment, velocity increment and position increment, and Represents the robot at time i At the time j The rotation matrix of and Represents the robot at time i At the time j The velocity vector, and Represents the robot at time i At the time j The position vector of Indicates that the robot is at time i At the time j The rotation noise, that is, the angle error, is zero-mean Gaussian noise; Indicates the use of exponential mapping (exp) to convert the angle error Mapping from Lie algebra back to Lie group, Respectively represent the robot from time i At the time j The velocity noise and position noise are both zero-mean Gaussian noise. Indicates from time i At the time j time interval, Represents the gravity vector.

[0105] In step S1, the updated posture information includes the updated rotation matrix , velocity vector and position vector ;

[0106] In step S2, the Lie group construction based on the updated posture information and the position information of multiple landmark points includes the following steps: , velocity vector , position vector and the locations of multiple landmarks Constructing Lie groups , the Lie group represents the state vector of robot n at the current moment, the Lie group The expression is as follows:

[0107] ,

[0108] Where, Indicates a The zero matrix of Indicates a The identity matrix, m Indicates the robot's position information. p Indicates the number of observed landmark points;

[0109] In step S2, the complete state of the robot including the deviation information at the current moment is obtained, which includes: the complete state of the robot including the deviation information at the current moment n is expressed as ,in is the bias vector of the inertial IMU sensor, expressed as:

[0110] ,

[0111] Where, is the gyroscope deviation, is the deviation of the accelerometer, and the two are combined into a deviation vector .

[0112] In step S3, the uncertainty representation includes the uncertainty representation of the state and the uncertainty representation of the deviation. The uncertainty representation of the state is represented by the state error The uncertainty of the deviation is expressed by the deviation error Indicates that the state error and the deviation error Subject to mean 0 and covariance The Gaussian distribution of is expressed as:

[0113] ,

[0114] State covariance matrix Describes the state error and bias error The uncertainty size, state covariance matrix Cholesky decomposition can be used to obtain the Cholesky factor , the decomposition process is as follows: ,

[0115] Where S is the state covariance matrix The Cholesky factor obtained by Cholesky decomposition is a decomposition method that decomposes a positive definite matrix into a lower triangular matrix and its transpose. Compared with directly operating the covariance , Cholesky factor The complexity of matrix operations is reduced during calculations, which can significantly reduce calculation errors.

[0116] The state vector of the robot at the current time n The operation is as follows:

[0117] ,

[0118] Where, represents the state vector of the robot at the current time n, Represents the state vector The state mean, Indicates the state error, Indicates state error Mapping from Lie algebra back to Lie group through exponential mapping exp, correcting the state mean ;

[0119] The deviation vector of the inertial IMU sensor at the current time n The operation is as follows:

[0120] ,

[0121] Where, Represents the deviation vector of the inertial IMU sensor at the current time n, Denotes the deviation vector The mean deviation of Indicates bias error;

[0122] In step S3, the state model f is expressed as:

[0123] ,

[0124] represents the state vector of the robot at the previous moment n-1, Represents the deviation vector of the inertial IMU sensor at the previous moment n-1, an input vector representing the state model f, containing the angular velocity and acceleration measured by the inertial IMU sensor, an actual input of the state model f at the current time instant, removing the influence of the inertial IMU sensor bias, an actual input of the state model f at the current time instant, removing the influence of the inertial IMU sensor bias, a process noise, specifically a random noise of the system, subject to a Gaussian distribution, represented as a process noise a covariance matrix of the process noise.

[0125] In step S3, the measurement model expression is:

[0126]

[0127] wherein, an actual observation vector obtained by the visual sensor at the current time instant n, a measurement model describing the observation value of the visual sensor to the landmark under the condition of the state vector and the observation noise at the current time instant n; the observation noise is subject to a Gaussian distribution , a covariance matrix of the observation noise , the measurement model is further expanded as:

[0128]

[0129] wherein, contains the observation data of the visual sensor to all landmark points, p represents the number of landmark points, represents the observation data of the i-th landmark, specifically the normalized pixel position of the landmark on the image plane in the camera coordinate system, i represents the serial number of the landmark point.

[0130] In step S4, the state model of the next time instant is updated based on the unbiased input and the state model of the current time instant, specifically including:

[0131] S4.1, input preparation; setting the input of the state model of the current time instant n as the state mean , the bias mean , the Cholesky decomposition factor of the state covariance matrix , the input vector and the covariance matrix of the process noise ;

[0132] wherein, the bias mean of the previous time instant is​​​ from the input vector , to obtain an unbiased input vector at the current time n ;

[0133] Cholesky decomposition factor of the covariance matrix is constructed into a matrix with , to obtain is:

[0134] ,

[0135] In the formula, blkdiag represents constructing a block diagonal matrix, represents Cholesky decomposition of the process noise covariance matrix , the state mean value at the current time n is retained, and .

[0136] S4.2, propagate the state, update the state mean value and the mean value of the deviation ; based on the unbiased input vector and the state vector , the state vector and the deviation are propagated through the state model to obtain the updated state mean value and the mean value of the deviation :

[0137] ,

[0138] In the formula, and represent the updated state mean value and the mean value of the deviation, respectively;

[0139] The updated state mean value is taken as the state vector of the robot at the next time, and the updated mean value of the deviation is taken as the deviation vector of the inertial IMU sensor at the next time, and the state model f is updated.

[0140] S4.3, generate and propagate Sigma points; the Sigma points are a set of special points that can capture the uncertainty of the system state, and the covariance matrix can generate 2 Sigma points, and the Sigma points reflect the uncertainty of the state, and the Sigma points are obtained through the following formula:

[0141] ,

[0142] In the formula, is a scaling factor for adjusting the range of Sigma points, represents the extraction of the first column from the matrix for generating Sigma points, respectively represent the generated Sigma points of state error , bias error and process noise ;

[0143] The generated Sigma points are propagated through the state model to obtain the propagated Sigma points, and the operation process is as follows:

[0144] ,

[0145] In the formula, respectively represent the generated Sigma points of propagated state error and bias error ;

[0146] The propagated Sigma points are mapped back to the Lie algebra from the Lie group by the following formula,

[0147] ,

[0148] In the formula, represents the mapping of Sigma points back to the Lie algebra, represents the Sigma points mapped back to the Lie algebra, and the Cholesky factor can be updated in the local linear space ;

[0149] S4.4, updating the Cholesky factor ; based on the information of the Sigma points mapped back to the Lie algebra from the Lie group, the propagated Sigma points and the covariance matrix of the process noise , the Cholesky factor is updated by QR decomposition, and the updated Cholesky factor of the next time is obtained, and the QR decomposition is obtained by the following formula:

[0150] , ,

[0151] where qr function represents QR decomposition. By QR decomposition, the positive definiteness and numerical stability of the covariance matrix in state propagation can be guaranteed. The updated Cholesky factor is used to update the state covariance matrix decomposition factor .

[0152] In step S5, if there is visual observation data of the landmark at the next time, the visual observation value is calculated based on the measurement model, and the state update and the update of the state covariance matrix decomposition factor at this time are performed in combination with the visual observation value, including:

[0153] S5.1, first, the landmark observation value without observation noise is calculated according to the measurement model :

[0154] ,

[0155] wherein is the observation noise, and is the updated state mean in step S4.2;

[0156] S5.2, based on the updated Cholesky factor in step S4.4, the new sigma point is generated:

[0157] ,

[0158] wherein respectively represent the Sigma points of the generated state error and bias error , and the Sigma points in the Lie algebra are converted into the Sigma points in the Lie group by exponential mapping, and the Sigma points in the Lie group are obtained by the following formula:

[0159] ,

[0160] wherein represents that the Sigma point is mapped back to the Lie group by exponential mapping exp, represents the Sigma point mapped back to the Lie group,

[0161] the Sigma point is substituted into the measurement model , and the Sigma point of the landmark observation value with noise is calculated:

[0162] ,

[0163] Where, is the Sigma point of the landmark observation with noise, Observation noise Sigma point, which is determined by the observation noise The covariance matrix of generate;

[0164] S5.3, using QR decomposition, calculate the correction value of the state by comparing the actual measurement value and the predicted measurement value Corrected value of deviation , while updating the Cholesky factor To ensure the numerical stability of the covariance matrix, the QR decomposition is obtained by the following formula:

[0165] , ,

[0166] Where, is the actual observation value, which is the value directly observed by the visual sensor. is the noise-free predicted measurement value, calculated in step S5.1, and are the Sigma points of the measurement and state errors with noise, and N is the observation noise The covariance matrix of

[0167] S5.4, Correction value for use status Corrected value of deviation Update state mean , mean deviation :

[0168] ,

[0169] After the state update process, the updated state mean is obtained , mean deviation And the updated state covariance matrix Cholesky factor ;

[0170] The updated state mean As the state vector of the robot at the next moment, the updated deviation mean As the deviation vector of the inertial IMU sensor at the next moment, the state model f is updated again;

[0171] With the updated Cholesky factor Decomposition factors of the state covariance matrix to update.

[0172] The embodiments of the present application are described above with reference to the accompanying drawings, but the present application is not limited to the above-described specific embodiments, and the above-described specific embodiments are merely illustrative, but not restrictive, and a person of ordinary skill in the art can make many forms under the inspiration of the present application without departing from the purpose of the present application and the scope protected by the claims, and these all belong to the protection of the present application.

Claims

1. A visual-inertial filtering method based on manifold processing, characterized in that, The method comprises the following steps: S1, obtaining pose information of a robot based on inertial IMU sensors, performing an integral operation on the pose information based on a manifold-based IMU pre-integration method to obtain pose information increments, and obtaining updated pose information based on the pose information increments; S2, performing Lie group construction based on the updated pose information and position information of a plurality of landmark points, embedding a state space into the Lie group, and obtaining a complete state of the robot at a current time point containing bias information; S3, constructing an uncertainty representation, a state model, and a measurement model of the UKF-based invariant Kalman filter at the current time point based on the Lie group and the bias information; S4, updating a state model at a next time point based on unbiased input and the state model at the current time point, and updating a state covariance matrix decomposition factor at the next time point based on uncertainty propagated by a current state and a process noise covariance matrix; S5, if there is visual observation data of a landmark at the next time point, calculating a visual observation value based on the measurement model and updating the state model and the state covariance matrix decomposition factor at the time point in combination with the visual observation value, otherwise, jumping to step S4; S6, after entering a next time point, repeating steps S4-S5.

2. The visual-inertial filtering method of claim 1, wherein In step S1, the pose information includes a rotation matrix , a velocity vector , and a position vector ; the manifold-based IMU pre-integration method integrates the pose information to obtain the pose information increment, specifically through the following formula: ， wherein and i denote the rotational increment, velocity increment and position increment of the robot from time j to time and denote the rotation matrices of the robot at time i to time j , and denote the velocity vectors of the robot at time i to time j , and denote the position vectors of the robot at time i to time j ; denotes the rotational noise, i.e. the angular error, of the robot from time i to time j and is zero-mean Gaussian noise; denotes the mapping of the angular error from the Lie algebra back to the Lie group using the exponential map (exp), denote the velocity noise and position noise of the robot from time i to time j and are zero-mean Gaussian noise, denotes the time interval from time i to time j , denotes the gravity vector.

3. The visual-inertial filtering method according to claim 1, characterized in that, In step S1, the updated pose information comprises an updated rotation matrix , a velocity vector and a position vector ; In step S2, the Lie group construction based on the updated pose information and the plurality of landmark point position information includes constructing a Lie group by using the updated rotation matrix , a velocity vector , a position vector , and positions of the plurality of landmark points The Lie group represents a state vector of the robot at the current time n, and an expression of the Lie group is as follows:​ ， In the formula, represents a zero matrix, represents a zero matrix, represents a unit matrix, represents a unit matrix, m represents position information of the robot, p represents the number of observed landmark points; In step S2, the obtaining the complete state of the robot containing bias information at the current time comprises: the complete state of the robot containing bias information at the current time n is represented as wherein is a bias vector of the inertial IMU sensor, and is represented as: ， wherein is the bias of the gyroscope, is the bias of the accelerometer, both combined in one bias vector .

4. The visual-inertial filtering method of claim 1, wherein, In step S3, the uncertainty representation comprises an uncertainty representation of the state and an uncertainty representation of the bias, the uncertainty representation of the state being represented by a state error , the uncertainty representation of the bias being represented by a bias error , the state error and the bias error being subject to a Gaussian distribution with mean 0 and covariance , denoted as: ， state covariance matrix The uncertainty in the state error and bias error is described by the state covariance matrix Cholesky factorization of the state covariance matrix results in Cholesky factors The decomposition process is as follows: , where S is the state covariance matrix Cholesky factors resulting from a Cholesky decomposition, which is a decomposition method that decomposes a positive definite matrix into a lower triangular matrix and its transpose.

5. The visual-inertial filtering method of claim 1, wherein the robot The state vector at the current time n is computed as follows: ， wherein denotes the state vector of the robot at the current time instant n, denotes the state vector the state mean of the state vector denotes the state error denotes the state error is mapped back from the Lie algebra to the Lie group by the exponential map exp, the state mean is corrected ; bias vector of the inertial IMU sensor at the current time instant n is computed as follows: ， wherein denotes the bias vector of the inertial IMU sensor at the current time instant n, denotes the bias vector of the bias mean, denotes the bias error; in step S3, the state model f is expressed as: ， denotes the state vector of the robot at the previous time instant n-1, denotes the bias vector of the inertial IMU sensors at the previous time instant n-1, denotes the input vector of the state model f, containing the angular velocity and acceleration measured by the inertial IMU sensors, denotes the actual input of the state model f at the current time instant, removing the influence of the inertial IMU sensor biases, denotes the process noise, in particular the stochastic noise of the system, which is Gaussian distributed and is denoted by where is the covariance matrix of the process noise .

6. The visual-inertial filtering method of claim 1, wherein, in step S3, the measurement model is expressed as: ， wherein, denotes the actual observation vector obtained by the vision sensor at the current time n, denotes the measurement model describing the observation of landmarks by the vision sensor at the current time n given the state vector and the observation noise ; the observation noise is subject to a Gaussian distribution , denotes the covariance matrix of the observation noise , the measurement model is further expanded as: ， wherein The observation data of all landmarks by the vision sensor is denoted by p, and the number of landmarks is denoted by The observation data of the i-th landmark is denoted by pi, which is specifically the normalized pixel position of the landmark on the image plane in the camera coordinate system, and i denotes the sequence number of the landmark.

7. The visual-inertial filtering method of claim 5, wherein, in step S4, the state model at the next time point is updated based on unbiased input and the state model at the current time point, and specifically comprises: S4.1, set the input of the current n-state model to the state mean , mean deviation , the state covariance matrix Cholesky factorization of , the input vector and process noise The covariance matrix of ; wherein the bias mean of the previous time instant is removed from the input vector to obtain the unbiased input vector at the current time instant n ; The Cholesky decomposition factor of the covariance matrix is calculated as The matrix construction is performed with to obtain ​ ， where blkdiag denotes constructing a block diagonal matrix, denotes the Cholesky decomposition of the process noise covariance matrix , the state mean at the current time n is preserved, and ; S4.2, update state mean and bias mean ; based on unbiased input vector and state vector , propagate state vector and bias through the state model to obtain updated state mean and bias mean : ， wherein and respectively denote the updated state mean and bias mean; with the updated mean as the state vector of the robot at the next time instant, with the updated mean update the state model f as the bias vector of the inertial IMU sensor at the next time instant.

8. The visual-inertial filtering method of claim 7, wherein, in step S4, the state covariance matrix decomposition factor at the next time point is updated based on uncertainty propagated by a current state and a process noise covariance matrix, and comprises: S4.3, the sigma points are a set of special points that can capture the uncertainty of the system state, and the covariance matrix 2 sigma points that reflect the uncertainty of the state can be generated, and the sigma points are obtained by the following formula: ， Where, is the scaling factor, used to adjust the range of Sigma points. Represents the matrix Extract the Column, used to generate Sigma points, Represent the generated state errors Sigma point, deviation error Sigma points and process noise Sigma point; The generated Sigma points are propagated through the state model to obtain propagated Sigma points, the operation process being as follows: ， wherein respectively represent the propagated state error and bias error generated Sigma points; The propagated Sigma points are mapped back into the Lie algebra by the following equation, ， wherein denotes the Sigma point from the Lie group back to the Lie algebra, denotes the Sigma point mapped back to the Lie algebra, the Cholesky factor can be updated in the local linear space ; S4.4, update the Cholesky factor based on the Sigma points mapped back from the Lie group to the Lie algebra , the information of the propagated Sigma points and the covariance matrix of the process noise , update the Cholesky factor by QR decomposition , obtain the updated Cholesky factor for the next time step , the QR decomposition is obtained by the following formula:​ ， ， wherein the qr function represents QR decomposition; with the updated Cholesky factor the state covariance matrix decomposition factors are updated.

9. The visual-inertial filtering method of claim 8, wherein, if there is visual observation data of a landmark at the next time point, the visual observation value is calculated based on the measurement model, and the state model and the state covariance matrix decomposition factor at the time point are updated in combination with the visual observation value, and comprises: S5.1, first compute landmark observations without observation noise according to the measurement model ​ ， wherein is the observation noise, is the updated state mean in step S4.2; S5.2, updating the Cholesky factor based on step S4.4 , generating new Sigma points: ， Where, Represent the generated state errors and bias error Sigma point, and then use the exponential mapping to transform the Sigma point in Lie algebra , converted to a Sigma point in the Lie group , specifically obtained through the following formula: ， wherein denotes the Sigma point mapping back from the Lie algebra to the Lie group by the exponential map exp, denotes the Sigma point mapped back to the Lie group, The Sigma points are substituted into the measurement model , the Sigma points of the landmark observation with noise are calculated: ， wherein is the Sigma point for the landmark observation with noise, is the observation noise is the Sigma point for the landmark observation with noise, and is the covariance matrix of the observation noise is generated; S5.3, calculate the correction value of the state by comparison of the actual measurement value and the predicted measurement value and the correction value of the deviation simultaneously update the Cholesky factor ensure numerical stability of the covariance matrix, the QR decomposition is obtained by the following formula: ， ， wherein is the actual observed value, obtained directly from the vision sensor, is the noise-free predicted measurement, calculated in step S5.1, and are the Sigma points with measurement and state errors, respectively, is the observation noise covariance matrix; S5.4, correction value for the state of use and the correction value for the deviation updating the mean value of the state , the mean value of the deviation : ， After the state update process, the updated state mean , bias mean , and Cholesky factor of the updated state covariance matrix are obtained.​ with the updated mean as the state vector of the robot at the next time instant, with the updated mean with the updated mean as the bias vector of the inertial IMU sensor at the next time instant, the state model f is updated again; with the updated Cholesky factor the state covariance matrix decomposition factors are updated.

Images