Visual inertia filtering method based on manifold processing

Abstract

The invention discloses a visual inertia filtering method based on manifold processing, and belongs to the field of mobile robot positioning, and the method comprises the steps: obtaining updated pose information based on an inertia IMU sensor; performing Lie group construction based on the updated pose information and the position information of the plurality of landmark points to obtain a complete state of the robot containing deviation information at the current moment; on the basis of the Lie group and the deviation information, uncertain representation, a state model and a measurement model of the current moment of UKF-based invariant Kalman filtering are constructed; updating the state model of the next moment based on the state model; updating a state covariance matrix decomposition factor at the next moment based on the uncertainty of state propagation and the process noise covariance matrix; and if the visual observation data of the landmark exists at the next moment, calculating an observation value based on the measurement model and updating the state and the state covariance matrix decomposition factor at the moment by combining the observation value, otherwise, repeating the step of updating the state model and the state covariance matrix decomposition factor at the next moment.

Description

Technical Field

[0001] The invention relates to the field of mobile robot positioning, and in particular to a visual inertial filtering method based on manifold processing. Background Art

[0002] In recent years, with the development of science and technology, the research and application of positioning technology has shown a rapid development momentum. However, since a single positioning system is difficult to meet the needs of actual positioning applications, positioning services in comprehensive environments require a positioning technology that is practical and stable. Studies have found that visual sensors have high accuracy but poor stability, while inertial measurement units (IMUs) have high stability but are prone to cumulative errors when used for a long time. The integration 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, manifold-related issues are particularly critical, especially when processing rotation, posture, and IMU data. Traditional linear space operations can no longer meet the requirements of accuracy and robustness. Since the posture changes in positioning involve rotation and displacement, and these operations are often more complex on the manifold, the calculation complexity becomes greater. More precise processing is required in the special manifold structure of Lie group. By embedding the state space into the Lie group, the nonlinear problems of rotation and posture can be effectively handled. Summary of the invention

[0004] The purpose of the present invention is to provide a visual inertial filtering method based on manifold processing, embed the state space into the constructed Lie group, more accurately process the nonlinear problem in visual inertial positioning in the special manifold structure of the Lie group, and improve the stability and robustness of the system on the complex manifold structure. The present invention is achieved by the following technical solutions.

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

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

[0007] S2, constructing a Lie group based on the updated posture information and the position information of multiple landmark points, embedding the state space into the Lie group, and obtaining the complete state of the robot including the deviation information at the current moment;

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

[0009] S4, updating the state model at the next moment based on the unbiased input and the state model at the current moment; updating the state covariance matrix decomposition factor at the next moment 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 moment, the visual observation value is calculated based on the measurement model and the state model and the state covariance matrix decomposition factor of the moment are updated in combination with the visual observation value, otherwise jump to step S4;

[0011] S6, after entering the next moment, repeat steps S4-S5.

[0012] In practical application, the algorithm described in the present invention is implemented strictly in accordance with the above steps. First, the inertial IMU sensor is used to obtain the posture information of the mobile robot at the current moment, that is, the rotation matrix R, the velocity vector v and the position vector x, and the manifold IMU pre-integration method is used to integrate the posture information to obtain the posture information increment. In practical applications, the posture information of the mobile robot at the current moment and the posture information increment can be calculated. The manifold-based IMU pre-integration method makes the processing of IMU data more accurate and simplifies the calculation complexity. By calculating the updated posture information of the mobile robot at the next moment and the position information of multiple landmark points to construct a Lie group, the complete state of the mobile robot at the current moment including the deviation information can be obtained. By embedding the state space into the Lie group, the nonlinear problems of rotation and posture can be effectively handled.

[0013] Then, the uncertainty representation, state model and measurement model of the current moment of the invariant Kalman filter based on UKF are constructed through the constructed Lie group and deviation information. The invariant Kalman filter algorithm based on UKF can ensure the high-precision performance of visual inertial positioning in complex environments. The state model at the next moment is updated based on the unbiased input and the state model at the current moment; the state covariance matrix decomposition factor of the next moment is updated based on the uncertainty of the current state propagation and the process noise covariance matrix. Finally, at this moment, it is determined by the visual sensor whether there is visual observation data of the mobile robot. If so, the observation data of the mobile robot at each landmark point is observed by the visual sensor and the state model and state covariance matrix decomposition factor of the mobile robot at this moment are updated in combination with the acquired visual observation data. Otherwise, the steps of updating the state model and state covariance matrix decomposition factor of the next moment 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 made 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 posture information includes a rotation matrix , velocity vector and the position vector ; The manifold-based IMU pre-integration method integrates the posture information to obtain the posture information increment, which is specifically obtained by the following formula:

[0015] ,

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

[0017] Optionally, in step S1, the updated posture information includes an updated rotation matrix , velocity vector and the position vector ;

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

[0019] ,

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

[0021] In step S2, obtaining the complete state of the robot including the deviation information at the current moment 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:

[0022] ,

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

[0024] Optionally, in step S3, the uncertainty representation includes a state uncertainty representation and a deviation uncertainty representation, wherein the state uncertainty representation is represented by a state error The uncertainty of the deviation is expressed by the deviation error Indicates that the state error and the deviation error Subject to the mean of 0 and covariance of The Gaussian distribution of is expressed as:

[0025] ,

[0026] State covariance matrix Describes the state error and bias error The uncertainty size, state covariance matrix Cholesky factorization can be obtained by performing Cholesky decomposition , the decomposition process is as follows: ,

[0027] Where S is the state covariance matrix The Cholesky factor is obtained by performing Cholesky decomposition, which 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.

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

[0029] ,

[0030] In the formula, 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 ;

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

[0032] ,

[0033] In the formula, Represents the deviation vector of the inertial IMU sensor at the current time n, Denotes the deviation vector The mean deviation of Indicates the deviation error;

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

[0035] ,

[0036] 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, 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 moment, removing the influence of the inertial IMU sensor bias, represents process noise, specifically the random noise of the system, which obeys Gaussian distribution and is expressed as ,in is the process noise The covariance matrix of .

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

[0038] ,

[0039] In the formula, 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] In the formula, Contains vision sensors for all The observation data of landmark points, p represents the number of landmark points, It 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 from the equation to get the unbiased input vector n at the current moment ,but ;

[0045] The covariance matrix Cholesky factorization of and Construct the matrix and get for:

[0046] ,

[0047] In the formula, blkdiag means constructing a block diagonal matrix. Represents the process noise covariance matrix Cholesky decomposition of the current state n Keep and order ;

[0048] S4.2, propagation status, update status mean and the mean deviation ; Based on the unbiased input vector and the state vector , through the state model To propagate the state vector and deviation, get the updated state mean and the mean deviation :

[0049] ,

[0050] In the formula, and Respectively represent the updated state mean and deviation mean;

[0051] 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.

[0052] Optionally, in step S4, the updating of the state covariance matrix decomposition factor at the next moment based on the uncertainty of the current state propagation and the process noise covariance matrix includes:

[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, the covariance matrix Can generate 2 Sigma point, which reflects the uncertainty of the state, is obtained by the following formula:

[0054] ,

[0055] In the formula, 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;

[0056] The generated Sigma points are passed through the state model The propagation is performed to obtain the Sigma point after propagation. The calculation process is as follows:

[0057] ,

[0058] In the formula, Represents Sigma point Mapping from Lie algebra back to Lie group through exponential mapping exp, correcting the state mean ,In order to deal with nonlinear systems, Sigma points need to be mapped from linearized Lie algebras to Lie groups, so that Sigma points can more accurately represent the uncertainty of the state; Represent the state error after propagation and bias error Generated Sigma points;

[0059] The Sigma point after propagation Mapping back from the Lie group to the Lie algebra is done by the following formula,

[0060] ,

[0061] In the formula, Represents Sigma point Mapping from Lie groups back to Lie algebras, Represents the mapping back to the Sigma point in the Lie algebra, which can update the Cholesky factor in the local linear space ;

[0062] S4.4, Update Cholesky Factor ; Based on the mapping from Lie group back to Lie algebra Sigma point , Sigma point after propagation information and process noise The covariance matrix of , update the Cholesky factor by QR decomposition , get the Cholesky factor of the next moment after update , the QR decomposition is obtained by the following formula:

[0063] , ,

[0064] In the formula, qr function represents QR decomposition; QR decomposition can ensure that the covariance matrix in state propagation is The positive definiteness and numerical stability of

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

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

[0067] S5.1, first according to the measurement model , calculate the unobserved noise Landmark observations of:

[0068] ,

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

[0070] S5.2, based on the updated Cholesky factor from step S4.4 , generate new Sigma points:

[0071] ,

[0072] In the formula, 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 , which is obtained by the following formula:

[0073] ,

[0074] In the formula, Represents Sigma point Mapping from Lie algebra back to Lie group through the exponential map exp, represents the Sigma point mapped back to the Lie group,

[0075] Sigma Point Substitute into the measurement model , calculate the Sigma points of the noisy landmark observations:

[0076] ,

[0077] In the formula, is the Sigma point of the landmark observation with noise, The observation noise Sigma point, which is determined by the observation noise The covariance matrix of generate;

[0078] S5.3, using QR decomposition, by comparing the actual measurement value with the predicted measurement value, calculate the correction value of the state Correction 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:

[0079] , ,

[0080] In the formula, is the actual observed 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 with noise and state error, respectively, The observation noise The covariance matrix of

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

[0082] ,

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

[0084] 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;

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

[0086] Beneficial Effects

[0087] (1) This paper proposes a manifold-based IMU pre-integration method that can correctly process the manifold structure; at the same time, it makes the processing of IMU data more accurate and simplifies the computational complexity; this paper proposes an invariant Kalman filter 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 a more accurate state estimation, ensuring the high-precision performance of visual inertial positioning in complex environments;

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

[0090] Figure 1 FIG. 1 is a flow chart of a visual inertial filtering method based on manifold processing in an embodiment of the present invention. DETAILED DESCRIPTION

[0092] The invention is further described below with reference to the accompanying drawings and specific embodiments.

[0093] Embodiment 1: This embodiment provides a visual inertial filtering method based on manifold processing, combined with Figure 1 , including the following steps:

[0094] S1, obtaining the posture information of the robot based on the inertial IMU sensor, integrating the posture information based on the manifold-based IMU pre-integration method to obtain the posture information increment, and obtaining updated posture information based on the posture information increment;

[0095] S2, constructing a Lie group based on the updated posture information and the position information of multiple landmark points, embedding the state space into the Lie group, and obtaining the complete state of the robot including the deviation information at the current moment;

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

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

[0098] S5, if there is visual observation data of the landmark at the next moment, the visual observation value is calculated based on the measurement model and the state model and the state covariance matrix decomposition factor of the moment are updated in combination with the visual observation value, otherwise jump to step S4;

[0099] S6, after entering the next moment, repeat steps S4-S5.

[0100] In practical application, the algorithm described in the present invention is implemented strictly in accordance with the above steps. First, the inertial IMU sensor is used to obtain the posture information of the mobile robot at the current moment, that is, the rotation matrix R, the velocity vector v and the position vector x, and the manifold IMU pre-integration method is used to integrate the posture information to obtain the posture information increment. In practical applications, the posture information of the mobile robot at the current moment and the posture information increment can be calculated. The manifold-based IMU pre-integration method makes the processing of IMU data more accurate and simplifies the calculation complexity. By calculating the updated posture information of the mobile robot at the next moment and the position information of multiple landmark points to construct a Lie group, the complete state of the mobile robot at the current moment including the deviation information can be obtained. By embedding the state space into the Lie group, the nonlinear problems of rotation and posture can be effectively handled.

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

[0102] Finally, at this moment, the visual sensor is used to determine whether there is visual observation data of the mobile robot. If so, the visual sensor is used to observe the mobile robot at each landmark point. Observational data on The state model and state covariance matrix decomposition factors of the mobile robot at that moment are updated in combination with the acquired visual observation data, otherwise the steps of updating the state model and state covariance matrix decomposition factors at the next moment are repeated. By combining visual observation data and using the landmark information provided by the visual sensor, the robot's state estimation can be made more accurate, the system error can be reduced, and the system's ability to cope with complex and uncertain environments can be enhanced.

[0103] Embodiment 2, based on embodiment 1, this embodiment introduces a specific implementation process of a visual inertial filtering method based on manifold processing, which specifically includes the following contents:

[0104] In step S1, the posture information includes a rotation matrix , velocity vector and the position vector ; The manifold-based IMU pre-integration method integrates the posture information to obtain the posture information increment, which is specifically obtained by the following formula:

[0105] ,

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

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

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

[0109] ,

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

[0111] In step S2, obtaining the complete state of the robot including the deviation information at the current moment 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:

[0112] ,

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

[0114] 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 the mean of 0 and covariance of The Gaussian distribution of is expressed as:

[0115] ,

[0116] State covariance matrix Describes the state error and bias error The uncertainty size, state covariance matrix Cholesky factorization can be obtained by performing Cholesky decomposition , the decomposition process is as follows: ,

[0117] Where S is the state covariance matrix The Cholesky factor is obtained by performing Cholesky decomposition, which 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.

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

[0119] ,

[0120] In the formula, 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 ;

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

[0122] ,

[0123] In the formula, Represents the deviation vector of the inertial IMU sensor at the current time n, Denotes the deviation vector The mean deviation of Indicates the deviation error;

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

[0125] ,

[0126] 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, 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 moment, removing the influence of the inertial IMU sensor bias, represents process noise, specifically the random noise of the system, which obeys Gaussian distribution and is expressed as ,in is the process noise The covariance matrix of .

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

[0128] ,

[0129] In the formula, 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:

[0130] ,

[0131] In the formula, Contains vision sensors for all The observation data of landmark points, p represents the number of landmark points, It 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.

[0132] In step S4, the updating of the state model at the next moment based on the unbiased input and the state model at the current moment specifically includes:

[0133] 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 ;

[0134] Among them, the mean deviation of the previous moment From the input vector Remove it from the equation to get the unbiased input vector n at the current moment ,but ;

[0135] The covariance matrix Cholesky factorization of and Construct the matrix and get for:

[0136] ,

[0137] In the formula, blkdiag means constructing a block diagonal matrix. Represents the process noise covariance matrix Cholesky decomposition of the current state n Keep and order .

[0138] S4.2, propagation status, update status mean and the mean deviation ; Based on the unbiased input vector and the state vector , through the state model To propagate the state vector and deviation, get the updated state mean and the mean deviation :

[0139] ,

[0140] In the formula, and Respectively represent the updated state mean and deviation mean;

[0141] 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.

[0142] 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, the covariance matrix Can generate 2 Sigma point, which reflects the uncertainty of the state, is obtained by the following formula:

[0143] ,

[0144] In the formula, 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;

[0145] The generated Sigma points are passed through the state model The propagation is performed to obtain the Sigma point after propagation. The calculation process is as follows:

[0146] ,

[0147] In the formula, Represent the state error after propagation and bias error Generated Sigma points;

[0148] The Sigma point after propagation Mapping back from the Lie group to the Lie algebra is done by the following formula,

[0149] ,

[0150] In the formula, Represents Sigma point Mapping from Lie groups back to Lie algebras, Represents the mapping back to the Sigma point in the Lie algebra, which can update the Cholesky factor in the local linear space ;

[0151] S4.4, Update Cholesky Factor ; Based on the mapping from Lie group back to Lie algebra Sigma point , Sigma point after propagation information and process noise The covariance matrix of , update the Cholesky factor by QR decomposition , get the Cholesky factor of the next moment after update , the QR decomposition is obtained by the following formula:

[0152] , ,

[0153] In the formula, qr function represents QR decomposition. QR decomposition can ensure that the covariance matrix in state propagation The positive definiteness and numerical stability of Decomposition factor of the state covariance matrix to update.

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

[0155] S5.1, first according to the measurement model , calculate the unobserved noise Landmark observations of:

[0156] ,

[0157] In the formula, is the observation noise, is the state mean updated in step S4.2;

[0158] S5.2, based on the updated Cholesky factor from step S4.4 , generate new sigma points:

[0159] ,

[0160] In the formula, 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 , which is obtained by the following formula:

[0161] ,

[0162] In the formula, Represents Sigma point Mapping from Lie algebra back to Lie group through the exponential map exp, represents the Sigma point mapped back to the Lie group,

[0163] Sigma Point Substitute into the measurement model , calculate the Sigma points of the noisy landmark observations:

[0164] ,

[0165] In the formula, is the Sigma point of the landmark observation with noise, The observation noise Sigma point, which is determined by the observation noise The covariance matrix of generate;

[0166] S5.3, using QR decomposition, by comparing the actual measurement value with the predicted measurement value, calculate the correction value of the state Correction 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:

[0167] , ,

[0168] In the formula, is the actual observed 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 noisy measurement and state error, respectively, and N is the observation noise The covariance matrix of

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

[0170] ,

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

[0172] 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;

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

[0174] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the purpose of the present invention and the claims, which all fall within the protection of the present invention.

Claims

1. A visual inertial filtering method based on manifold processing, characterized in that: include: S1, obtaining the posture information of the robot based on the inertial IMU sensor, integrating the posture information based on the manifold-based IMU pre-integration method to obtain the posture information increment, and obtaining updated posture information based on the posture information increment; S2, constructing a Lie group based on the updated posture information and the position information of multiple landmark points, embedding the state space into the Lie group, and obtaining the complete state of the robot including the deviation information at the current moment; S3, constructing the uncertainty representation, state model and measurement model of the current moment of the invariant Kalman filter based on the UKF based on the Lie group and the deviation information; S4, updating the state model at the next moment based on the unbiased input and the state model at the current moment; updating the state covariance matrix decomposition factor at the next moment based on the uncertainty of the current state propagation and the process noise covariance matrix; S5, if there is visual observation data of the landmark at the next moment, the visual observation value is calculated based on the measurement model and the state model and the state covariance matrix decomposition factor of the moment are updated in combination with the visual observation value, otherwise jump to step S4; S6, after entering the next moment, repeat steps S4-S5.

2. The visual inertial filtering method according to claim 1, characterized in that In step S1, the posture information includes a rotation matrix , velocity vector and the position vector ; The manifold-based IMU pre-integration method integrates the posture information to obtain the posture information increment, which is specifically obtained by the following formula: ， In the formula, Respectively represent the robot from time i To time j The rotation increment, velocity increment and position increment, and Respectively represent the robot at time i To time j The rotation matrix of and Respectively represent the robot at time i To time j The velocity vector, and Respectively represent the robot at time i To time j The position vector of Indicates that the robot is at time i To time j The rotation noise, i.e. the angle error, is zero-mean Gaussian noise; Indicates the use of exponential mapping (exp) to convert the angle error Mapping from Lie algebras back to Lie groups, Respectively represent the robot from time i To time j The velocity noise and position noise are both zero-mean Gaussian noise. Indicates from time i To time j The time interval Represents the gravity vector.

3. The visual inertial filtering method according to claim 1, characterized in that: In step S1, the updated posture information includes the updated rotation matrix , velocity vector and the position vector ; In step S2, the Lie group construction based on the updated posture information and the position information of multiple landmark points includes: , velocity vector , position vector and the locations of multiple landmarks Constructing Lie groups , the Lie group represents the state vector of the robot n at the current moment, the Lie group The expression is as follows: ， In the formula, Indicates a The zero matrix of Indicates a The identity matrix of m Indicates the robot's position information. p represents the number of observed landmark points; In step S2, obtaining the complete state of the robot including the deviation information at the current moment 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: ， In the formula, is the gyroscope deviation, is the deviation of the accelerometer, and the two are combined into a deviation vector .

4. The visual inertial filtering method according to claim 1, characterized in that: 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 the mean of 0 and covariance of The Gaussian distribution of is expressed as: ， State covariance matrix Describes the state error and bias error The uncertainty size, state covariance matrix Cholesky factorization can be obtained by performing Cholesky decomposition , the decomposition process is as follows: , Where S is the state covariance matrix The Cholesky factor is obtained by performing Cholesky decomposition, where the Cholesky decomposition is a decomposition method for decomposing a positive definite matrix into a lower triangular matrix and its transpose.

5. The visual inertial filtering method according to claim 1, wherein the robot The state vector at the current time n The operation is as follows: ， In the formula, 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 ; The deviation vector of the inertial IMU sensor at the current time n The operation is as follows: ， In the formula, Represents the deviation vector of the inertial IMU sensor at the current time n, Denotes the deviation vector The mean deviation of Indicates the deviation error; In step S3, the state model f is expressed as: ， 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, 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 moment, removing the influence of the inertial IMU sensor bias, represents process noise, specifically the random noise of the system, which obeys Gaussian distribution and is expressed as ,in Process noise The covariance matrix of .

6. The visual inertial filtering method according to claim 1, characterized in that: In step S3, the measurement model expression is: ， In the formula, 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: ， In the formula, Contains vision sensors for all The observation data of landmark points, p represents the number of landmark points, It 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.

7. The visual inertial filtering method according to claim 5, characterized in that: In step S4, the updating of the state model at the next moment based on the unbiased input and the state model at the current moment specifically includes: 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 ; Among them, the mean deviation of the previous moment From the input vector Remove it from the equation to get the unbiased input vector n at the current moment ,but ; The covariance matrix Cholesky factorization of and Construct the matrix and get for: ， In the formula, blkdiag means constructing a block diagonal matrix. Represents the process noise covariance matrix Cholesky decomposition of the current state n Keep and order ; S4.2, Update state mean and the mean deviation ; Based on the unbiased input vector and the state vector , through the state model To propagate the state vector and deviation, get the updated state mean and the mean deviation : ， In the formula, and Respectively represent the updated state mean and deviation mean; 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.

8. The visual inertial filtering method according to claim 7, characterized in that: In step S4, the state covariance matrix decomposition factor of the next moment is updated based on the uncertainty of the current state propagation and the process noise covariance matrix, including: S4.3, 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 point, which reflects the uncertainty of the state, is obtained by the following formula: ， In the formula, 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 passed through the state model The propagation is performed to obtain the Sigma point after propagation. The calculation process is as follows: ， In the formula, Represent the state error after propagation and bias error Generated Sigma points; The Sigma point after propagation Mapping back from the Lie group to the Lie algebra is done by the following formula, ， In the formula, Represents Sigma point Mapping from Lie groups back to Lie algebras, Represents the mapping back to the Sigma point in the Lie algebra, which can update the Cholesky factor in the local linear space ; S4.4, Sigma points based on mapping from Lie groups back to Lie algebras , Sigma point after propagation information and process noise The covariance matrix of , update the Cholesky factor by QR decomposition , get the Cholesky factor of the next moment after update , the QR decomposition is obtained by the following formula: ， ， Where, qr function represents QR decomposition; With the updated Cholesky factor Decomposition factor of the state covariance matrix to update.

9. The visual inertial filtering method according to claim 8, characterized in that: If there is visual observation data of the landmark at the next moment, the visual observation value is calculated based on the measurement model and the state model and the state covariance matrix decomposition factor at the moment are updated in combination with the visual observation value, including: S5.1, first according to the measurement model , calculate without adding observation noise Landmark observations of: ， In the formula, is the observation noise, is the state mean updated in step S4.2; S5.2, based on the updated Cholesky factor from step S4.4 , generate new Sigma points: ， In the formula, 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 , which is obtained by the following formula: ， In the formula, Represents Sigma point Mapping from Lie algebra back to Lie group through the exponential map exp, Represents the Sigma point mapped back to the Lie group, Sigma Point Substitute into the measurement model , calculate the Sigma points of the noisy landmark observations: ， In the formula, is the Sigma point of the landmark observation with noise, The observation noise Sigma point, which is determined by the observation noise The covariance matrix of generate; S5.3, by comparing the actual measured value with the predicted measured value, calculate the correction value of the state Correction 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: ， ， In the formula, is the actual observed 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 with noise and state error, respectively, The observation noise The covariance matrix of S5.4, Correction value for use status Correction value of deviation Update state mean , mean deviation : ， After the state update process, the updated state mean is obtained , mean deviation And the updated state covariance matrix Cholesky factor ; 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; With the updated Cholesky factor Decomposition factor of the state covariance matrix to update.

Images