A method and chip for fusing and optimizing displacement based on IMU data and code disc data

Abstract

This application discloses a method and chip for optimizing displacement based on the fusion of IMU data and encoder data. The method includes: integrating IMU data based on manifold pre-integration to obtain the IMU pose and obtaining a first covariance matrix corresponding to the IMU pose; integrating encoder data based on group space closed integration to obtain a first encoder pose and obtaining a second covariance matrix corresponding to the first encoder pose; fusing the first covariance matrix, the second covariance matrix, the IMU pose, and the first encoder pose based on an error Kalman update process to obtain a first displacement error; optimizing and obtaining a second encoder pose based on the first encoder pose and the corresponding first displacement error; and integrating the second encoder pose based on group space closed integration to obtain the optimized current cumulative displacement. This application solves the problem of large encoder displacement errors caused by slippage by fusing IMU data and encoder data, effectively optimizing the accuracy of displacement.

Description

Technical Field

[0001] This application relates to the field of pose optimization, specifically to a method and chip for optimizing displacement based on the fusion of IMU data and encoder data. Background Technology

[0002] Currently, robots typically employ inertial navigation or visual navigation. Robots using inertial navigation rely on IMUs (Internal Measurement Units) for pose estimation. As the accuracy requirements for pose estimation increase, data from other sensors is often fused with the IMU to supplement and correct the IMU data, thereby improving the accuracy. In technologies that use encoder sensors to detect robot displacement, significant displacement errors occur when the robot slips, leading to inaccurate pose. Optimizing encoder displacement error to address robot slippage is a key technical challenge. Current methods for optimizing displacement error typically involve averaging displacement data from multiple sensors. However, this approach often results in poor optimization performance, with the final optimized displacement still exhibiting significant errors. Summary of the Invention

[0003] This application provides a method and chip for optimizing displacement based on the fusion of IMU data and encoder data. The specific technical solution is as follows:

[0004] A method for optimizing displacement based on the fusion of IMU data and encoder data specifically includes: integrating the IMU data based on manifold pre-integration to obtain the IMU pose and obtaining a first covariance matrix corresponding to the IMU pose; integrating the encoder data to obtain a first encoder pose and obtaining a second covariance matrix corresponding to the first encoder pose; fusing the first covariance matrix, the second covariance matrix, the IMU pose, and the first encoder pose based on an error Kalman update process to obtain a first displacement error; optimizing the first encoder pose based on the first encoder pose and the corresponding first displacement error to obtain a second encoder pose; and integrating the second encoder pose based on group space closed integration to obtain the optimized current cumulative displacement.

[0005] Furthermore, the step of optimizing and obtaining the second code disk pose based on the first code disk pose combined with the corresponding first displacement error specifically includes: obtaining the first code disk displacement of the current frame corresponding to the first code disk pose of the current frame; and using the sum of the first code disk displacement of the current frame and the first displacement error of the current frame as the second code disk displacement corresponding to the second code disk pose of the current frame, so as to optimize the second code disk pose.

[0006] Furthermore, the integration of the second code disk pose is based on group closed-space integration. The integration process specifically includes: obtaining the corresponding second code disk displacement of the current frame based on the second code disk pose of the current frame; obtaining the corresponding rotation increment of the current frame based on the IMU pose of the current frame; obtaining the rotation matrix of the current frame based on the rotation increment of the current frame through the rotation matrix transformation formula; calculating the second code disk pose change value of the current frame based on the rotation matrix of the current frame, the second code disk displacement of the current frame, and the Jacobian matrix; and using the Lie group right multiplication integral increment formula, taking the sum of the second code disk pose change value of the current frame and the cumulative displacement of the previous frame as the current cumulative displacement.

[0007] Further, the step of integrating the IMU data based on manifold pre-integration to obtain the IMU pose and obtaining the first covariance matrix corresponding to the IMU pose specifically includes: obtaining the angular velocity measurement value, angular velocity deviation value, angular velocity measurement noise, acceleration measurement value, acceleration deviation value, and acceleration measurement noise in the IMU data of the current frame; performing pre-integration based on the angular velocity measurement value, angular velocity deviation value, and angular velocity measurement noise to obtain the IMU attitude of the current frame; performing pre-integration based on the IMU attitude, acceleration measurement value, acceleration deviation value, and acceleration measurement noise to obtain the IMU velocity of the current frame; and performing pre-integration based on the IMU velocity, acceleration measurement value, acceleration deviation value, and acceleration measurement noise to obtain the IMU displacement of the current frame; wherein, the formula for pre-integration based on the angular velocity measurement value, angular velocity deviation value, and angular velocity measurement noise is: The formula for pre-integration based on IMU attitude, acceleration measurement value, acceleration deviation value, and acceleration measurement noise is as follows: The formula for pre-integration based on IMU velocity, acceleration measurements, acceleration deviation, and acceleration measurement noise is as follows: Where, ΔR ij IMU attitude calculated by pre-integration; It is the angular velocity measurement value of the current frame; It is the angular velocity deviation value of the current frame; Δt is the noise from the angular velocity measurement in the current frame; Δv is the change in time; Δt is the change in time. ij This is the IMU velocity calculated via pre-integration; Δp ij It is the IMU displacement calculated by pre-integration; the IMU pose includes IMU attitude, IMU velocity and IMU displacement; It is the measured acceleration value of the current frame. It is the acceleration deviation of the current frame. It is the acceleration measurement noise of the current frame.

[0008] Furthermore, the step of integrating the encoder data based on group space closed-loop integration to obtain the first encoder pose specifically includes: obtaining the corresponding rotation increment of the current frame based on the IMU pose of the current frame; obtaining the rotation matrix of the current frame based on the rotation increment of the current frame through the rotation matrix transformation formula; obtaining the first encoder displacement of the current frame based on the encoder data of the current frame; calculating the first encoder pose change value based on the rotation matrix of the current frame, the first encoder displacement of the current frame, and the Jacobian matrix; and calculating the first encoder pose of the current frame by combining the first encoder pose of the previous frame with the encoder pose change value of the current frame based on the Lie group right-multiplication integral increment formula.

[0009] Furthermore, the error-based Kalman update process fuses the first covariance matrix, the second covariance matrix, the IMU pose, and the first code disk pose to obtain the first displacement error. Specifically, this includes: calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU pose of the current frame and the second covariance matrix corresponding to the first code disk pose of the current frame; calculating the pose difference of the current frame based on the IMU pose of the current frame, the first code disk pose of the current frame, and the first displacement error of the previous frame; and calculating the first displacement error of the current frame based on the fused covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the pose difference of the current frame.

[0010] Further, the step of calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU pose of the current frame and the second covariance matrix corresponding to the first code disk pose of the current frame specifically includes: obtaining the cumulative error covariance matrix of the previous frame; calculating the transpose of the cumulative error covariance matrix of the previous frame; calculating the product of the cumulative error covariance matrix of the previous frame and its transpose as the fourth covariance matrix; calculating the sum of the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the cumulative error covariance matrix of the previous frame as the fifth covariance matrix; using the fourth covariance matrix as the dividend and the fifth covariance matrix as the divisor, the quotient of the fourth covariance matrix and the fifth covariance matrix as the sixth covariance matrix; calculating the difference between the cumulative error covariance matrix of the previous frame and the sixth covariance matrix as the cumulative error covariance matrix of the current frame; wherein, the initial frame of the cumulative error covariance matrix is ​​an identity matrix.

[0011] Further, the step of calculating the pose difference of the current frame based on the IMU pose of the current frame, the first code disk pose of the current frame, and the first displacement error of the previous frame specifically includes: adding the first code disk pose of the current frame to the first displacement error of the previous frame to obtain the calibration pose value; calculating the difference between the IMU pose of the current frame and the calibration pose value as the pose difference of the current frame; wherein, the initial frame of the first displacement error is the zero vector.

[0012] Further, the step of calculating the first displacement error of the current frame based on the cumulative error covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the pose difference of the current frame specifically includes: multiplying the cumulative error covariance matrix of the previous frame and the pose difference of the current frame as the seventh covariance matrix; using the seventh covariance matrix as the dividend, using the fifth covariance matrix as the divisor, and using the quotient of the seventh covariance matrix and the fifth covariance matrix as the eighth covariance matrix; and using the sum of the first displacement error of the previous frame and the eighth covariance matrix as the first displacement error of the current frame.

[0013] Further, obtaining the second covariance matrix corresponding to the first code disk pose specifically includes: configuring the second covariance matrix as a 3x3 matrix, wherein only the diagonal elements in the second covariance matrix have values, and the elements at other positions are all 0; wherein, the values ​​of the diagonal elements of the second covariance matrix are calculated based on the product of the first code disk displacement obtained from the code disk data and various configuration errors.

[0014] Furthermore, the configuration errors include at least: measurement error, drift error, and slippage error; the values ​​of the elements on the diagonal of the second covariance matrix are calculated based on the product of the first code disk displacement obtained from the code disk data and various configuration errors, specifically including: obtaining the first code disk displacement of the current frame based on the code disk data of the current frame; taking the sum of the measurement error and the slippage error as the first error; determining the first row of the diagonal elements of the second covariance matrix of the current frame as equal to the product of the first error and the first code disk displacement of the current frame; taking the sum of the drift error and the slippage error as the second error; and determining the second row and third row of the diagonal elements of the second covariance matrix of the current frame as equal to the product of the second error and the first code disk displacement of the current frame.

[0015] This application also discloses a chip that internally stores a computer program, which is executed by a processor to perform the aforementioned method for optimizing displacement based on the fusion of IMU data and code disk data.

[0016] This application describes a method and chip for optimizing displacement based on the fusion of IMU data and encoder data. The method corrects the encoder displacement based on IMU data, using the first displacement error obtained by fusing the IMU displacement and encoder displacement as a standard quantity to calibrate and optimize the encoder displacement based on encoder data. This solves the problem of large displacement errors in encoder acquisition caused by machine wheels getting stuck. The method utilizes closed-loop integration in group space to calculate the encoder displacement, effectively reducing the impact of uneven ground and undulations on encoder pose estimation results, improving pose accuracy, and thus enhancing the reliability of the displacement acquired from the encoder. Attached Figure Description

[0017] Figure 1 This is a flowchart illustrating a method for optimizing displacement based on the fusion of IMU data and encoder data, as described in one embodiment of this application. Detailed Implementation

[0018] The embodiments of this application will now be described in detail with reference to the accompanying drawings. It should be understood that the specific embodiments described below are for illustrative purposes only and are not intended to limit the scope of this application.

[0019] One embodiment of this application provides a method for optimizing displacement based on the fusion of IMU data and encoder data, aiming to obtain a more accurate pose by fusing IMU data and encoder data. Figure 1 As shown, the method for optimizing displacement based on the fusion of IMU data and encoder data specifically includes:

[0020] The IMU pose is obtained by pre-integrating the IMU data using manifolds, and the first covariance matrix corresponding to the IMU pose is obtained. The pre-integration refers to using the initial state of integration as a reference, and discarding the IMU data states of a series of processes within the equivalent time period after the integration operation, so that the pose change within the time period is presented as a single state of an equivalent variable reference. This integration method discards a large number of temporary states, leaving only strictly selected keyframes as optimization states during global optimization, thereby reducing the amount of computation while maintaining computational accuracy. The IMU refers to an Internal Measurement Unit, which is a device used to measure the three-axis attitude and acceleration of an object. It usually includes a three-axis gyroscope and a three-axis accelerometer. The IMU data usually includes three-axis angular velocity and three-axis acceleration.

[0021] The code disk data is integrated using a group space closed integral to obtain the first code disk pose, and the second covariance matrix corresponding to the first code disk pose is obtained. The group space closed integral refers to integrating the code disk data in Lie group space, which is an error-free closed integral and more accurate than other commonly used integrals. Preferably, in this application, the group space closed integral is performed in SE(3) space. This group space closed integral effectively reduces the impact of uneven ground and undulations on the pose estimation results, improving the accuracy of pose estimation.

[0022] The first covariance matrix, the second covariance matrix, the IMU pose, and the first code disk pose are fused based on the error Kalman update process to obtain the first displacement error. The error Kalman update process refers to the update process of the error-state Kalman filter (ESFK), a variant of the Kalman filter, which is a nonlinear filter for time-varying systems. In this application, the displacement error in the IMU pose and code disk pose is filtered based on the ESFK, which can achieve more accurate linearization.

[0023] The second code disk pose is obtained by optimizing the first code disk pose based on the first code disk pose and the corresponding first displacement error. Specifically, the calculation of the first displacement error takes the IMU pose into consideration, so that this step optimizes the displacement of the first code disk pose based on the IMU pose, thereby improving the reliability of the code disk displacement.

[0024] Integrating the second code disk pose yields the optimized current cumulative displacement. Preferably, the integration of the second code disk pose can be achieved, but is not limited to, through group space closed integration. Specifically, this step integrates the second code disk pose with the optimized displacement to obtain the corrected and optimized current cumulative displacement, eliminating the problem of inaccurate displacement acquisition by the code disk caused by factors such as slippage, and optimizing the displacement reliability.

[0025] In one implementation, the step of optimizing and obtaining the second code disk pose based on the first code disk pose combined with the corresponding first displacement error specifically includes: obtaining the first code disk displacement corresponding to the first code disk pose of the current frame; and using the sum of the first code disk displacement of the current frame and the first displacement error of the current frame as the second code disk displacement corresponding to the second code disk pose of the current frame, so as to optimize the second code disk pose. In this technical solution, the first code disk pose is corrected and optimized through the first displacement error. The preliminary optimization process mainly involves adding the first displacement error to the first code disk pose to obtain the preliminarily optimized second code disk pose. This achieves the correction of the code disk displacement based on IMU data, solving the problem of large displacement errors in code disk acquisition caused by the machine wheels being stuck.

[0026] As one implementation method, the step of integrating the pose of the second code disk based on closed-loop integration in the group space to obtain the optimized current cumulative displacement specifically includes:

[0027] The second code disk displacement of the current frame is obtained based on the second code disk pose of the current frame; wherein, the second code disk pose is the code disk pose after the displacement is optimized by fusing IMU data and code disk data, and the second code disk displacement obtained based on the second code disk pose is also the optimized and corrected displacement.

[0028] The rotation increment of the current frame is obtained based on the IMU pose of the current frame; specifically, the rotation increment refers to the rotation angle around the X-axis, Y-axis and Z-axis in the IMU pose; the rotation matrix of the current frame is obtained based on the rotation increment of the current frame through the rotation matrix transformation formula; since the rotation matrix transformation formula is a commonly used formula in this field, it will not be elaborated here.

[0029] The second encoder attitude change value in the current frame is calculated based on the rotation matrix of the current frame, the second encoder displacement of the current frame, and the Jacobian matrix; wherein, the formula for calculating the Jacobian matrix is: In the formula for calculating the Jacobian matrix, θ is provided by the rotation angle of the IMU pose in the current frame, with the Z-axis as the rotation axis. Specifically, the formula used to calculate the second code disk attitude change value in the current frame based on the rotation matrix of the current frame, the second code disk displacement of the current frame, and the Jacobian matrix is ​​as follows:

[0030] In this step, ΔT in the formula refers to the second code disk attitude change value in the current frame; R refers to the rotation matrix in the current frame; J refers to the Jacobian matrix; and Δp refers to the second code disk displacement in the current frame.

[0031] Based on the Lie group right-multiplication integral increment formula, the sum of the second code disk attitude change value in the current frame and the cumulative displacement in the previous frame is taken as the current cumulative displacement; specifically, the Lie group right-multiplication integral increment formula is: ΔT j+1 =ΔTΔT j In this step, ΔT in the formula j ΔT refers to the cumulative displacement in the previous frame, while ΔT refers to the second encoder disk attitude change value in the current frame. j+1 This refers to the current cumulative displacement. It should be noted that the current cumulative displacement actually refers to the cumulative displacement of the current frame. This embodiment uses an optimized and corrected second code disk displacement to calculate the current cumulative displacement, thereby improving the accuracy of the cumulative displacement.

[0032] As one implementation method, the method for integrating IMU data based on manifold pre-integration to obtain IMU pose specifically includes: acquiring angular velocity measurement value, angular velocity deviation value, angular velocity measurement noise, acceleration measurement value, acceleration deviation value, and acceleration measurement noise from the IMU data of the current frame;

[0033] Pre-integration is performed based on angular velocity measurements, angular velocity deviations, and angular velocity measurement noise to obtain the IMU attitude of the current frame;

[0034] Pre-integration is performed based on IMU attitude, acceleration measurement value, acceleration deviation value and acceleration measurement noise to obtain the IMU velocity of the current frame;

[0035] Pre-integration is performed based on IMU velocity, acceleration measurement values, acceleration deviation values, and acceleration measurement noise to obtain the IMU displacement of the current frame;

[0036] Specifically, the formula for pre-integration based on the angular velocity measurement value, angular velocity deviation value, and angular velocity measurement noise is as follows:

[0037] Where, ΔR ij IMU attitude calculated by pre-integration; It is the angular velocity measurement value of the current frame; It is the angular velocity deviation value of the current frame; Δt is the angular velocity measurement noise of the current frame; Δt is the change in time.

[0038] Specifically, the formula for pre-integration based on IMU attitude, acceleration measurement value, acceleration deviation value, and acceleration measurement noise is as follows:

[0039] Where, ΔR ik The IMU pose is calculated using pre-integration for the current frame; It is the measured acceleration value of the current frame. It is the acceleration deviation of the current frame. Δt is the acceleration measurement noise of the current frame; Δt is the change in time.

[0040] The formula for pre-integration based on IMU velocity, acceleration measurements, acceleration deviation, and acceleration measurement noise is as follows:

[0041] Where, Δp ij It is the IMU displacement calculated by pre-integration; Δv ikΔt is the IMU velocity calculated by pre-integration; Δt is the change in time. It should be noted that the IMU pose described in this embodiment includes at least the IMU attitude, the IMU velocity, and the IMU displacement. In this embodiment, to avoid repeated integration of acceleration and gyroscope angular velocity information in the IMU data during optimization, changes in the IMU data unrelated to the current state are isolated, and pre-integrated variables are constructed. This significantly reduces the computational load of the displacement optimization process. Lie algebras are used to represent the rotation process, leveraging the operational properties of Lie group manifolds to suppress error growth and improve IMU displacement accuracy. Simultaneously, using Lie algebras for computation and optimization reduces the dimension of the state vector, decreases the overall computational load, and improves the utilization of computational resources.

[0042] As one implementation method, the method for obtaining the first covariance matrix corresponding to the IMU pose specifically includes:

[0043] The first covariance matrix is ​​calculated based on the first covariance matrix corresponding to the IMU pose of the previous frame, combined with the first covariance matrix transfer formula; wherein, the first covariance matrix transfer formula is:

[0044] in, The A j-1 T Refers to A j-1 The transpose of the matrix;

[0045] in, The B j-1 T It refers to B j-1 The transpose of the matrix;

[0046] in, The right Jacobian matrix is ​​the IMU pose of the previous frame. The IMU attitude is calculated by pre-integration to ignore angular velocity measurement noise. It should be noted that the calculation of the initial frame of the first covariance matrix corresponding to the IMU attitude needs to refer to the noise value recorded in the IMU chip manual corresponding to the IMU used.

[0047] Preferably, the first covariance matrix is ​​a 9x9 matrix, and only the diagonal elements of the first covariance matrix have values, while the rest are all 0.

[0048] As one implementation method, the step of integrating the code disk data based on group space closed-loop integration to obtain the first code disk pose specifically includes:

[0049] The rotation increment for the current frame is obtained based on the IMU pose of the current frame; specifically, the rotation increment refers to the rotation angle around the X-axis, Y-axis, and Z-axis in the IMU pose. The rotation matrix for the current frame is obtained based on the rotation increment using a rotation matrix transformation formula; since the rotation matrix transformation formula is a commonly used formula in this field, it will not be elaborated here.

[0050] The first encoder displacement of the current frame is obtained based on the encoder data of the current frame; the first encoder attitude change value is calculated based on the rotation matrix of the current frame, the first encoder displacement of the current frame, and the Jacobian matrix; specifically, the formula used to calculate the first encoder attitude change value of the current frame based on the rotation matrix of the current frame, the first encoder displacement of the current frame, and the Jacobian matrix is ​​as follows: In this embodiment, ΔT in the formula refers to the first code disk attitude change value of the current frame; R refers to the rotation matrix of the current frame; J refers to the Jacobian matrix; and Δp refers to the first code disk displacement of the current frame.

[0051] Based on the Lie group right-multiplication integral increment formula, the first code disk pose in the current frame is calculated by comparing the first code disk pose in the previous frame with the change in the first code disk pose in the current frame; specifically, the Lie group right-multiplication integral increment formula is: ΔT j+1 =ΔTΔT j In this step, ΔT in the formula j The first encoder disk pose refers to the first encoder disk pose in the previous frame, and ΔT refers to the change in the first encoder disk pose in the current frame. j+1 This refers to the first code disk pose in the current frame. This implementation method calculates the code disk pose by using group closed integral, eliminating the influence of code disk data acquisition on code disk pose calculation under uneven or undulating ground conditions, thereby improving the accuracy of the first code disk pose.

[0052] In one implementation, the error-based Kalman update process fuses the first covariance matrix, the second covariance matrix, the IMU pose, and the first code disk pose to obtain the first displacement error. Specifically, this includes: calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU pose of the current frame and the second covariance matrix corresponding to the first code disk pose of the current frame; calculating the pose difference of the current frame based on the IMU pose of the current frame, the first code disk pose of the current frame, and the first displacement error of the previous frame; and calculating the first displacement error of the current frame based on the fused covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the pose difference of the current frame. This embodiment integrates the IMU pose with the first code disk pose during the error Kalman update process, and calculates the first displacement error to calibrate the first code disk displacement based on the IMU pose. This specifically solves the problem of large displacement errors caused by robot wheel slippage, and effectively improves the reliability of displacement acquisition through the code disk.

[0053] As one implementation method, the method for calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU pose of the current frame and the second covariance matrix corresponding to the first code disk pose of the current frame specifically includes:

[0054] Obtain the cumulative error covariance matrix of the previous frame; calculate the transpose matrix of the cumulative error covariance matrix of the previous frame; wherein, the transpose matrix is ​​the new matrix obtained by interchanging the rows and columns of the cumulative error covariance matrix of the previous frame.

[0055] The product of the cumulative error covariance matrix of the previous frame and the transpose of the cumulative error covariance matrix of the previous frame is calculated as the third covariance matrix. The sum of the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the cumulative error covariance matrix of the previous frame is calculated as the fourth covariance matrix. It should be noted that the calculation steps of the third covariance matrix and the fourth covariance matrix can be, but are not limited to, performing the calculation steps of the third covariance matrix first and then the calculation steps of the fourth covariance matrix, or performing the calculation steps of the fourth covariance matrix first and then the calculation steps of the third covariance matrix, or performing the calculation steps of the third covariance matrix and the fourth covariance matrix simultaneously. The order of the two calculation steps is not limited.

[0056] Using the third covariance matrix as the dividend and the fourth covariance matrix as the divisor, the quotient of the third and fourth covariance matrices is taken as the fifth covariance matrix. The difference between the cumulative error covariance matrix of the previous frame and the fifth covariance matrix is ​​calculated as the cumulative error covariance matrix of the current frame. It should be noted that the initial frame of the cumulative error covariance matrix is ​​the identity matrix. That is, when the calculation of the cumulative error covariance matrix of the first frame begins, the cumulative error covariance matrix of the previous frame is the identity matrix. Accordingly, the third covariance matrix is ​​equal to the product of two identity matrices, the fourth covariance matrix is ​​equal to the sum of the first, second, and identity matrices, and the cumulative error covariance matrix of the current frame is equal to the difference between the identity matrix and the fifth covariance matrix.

[0057] In one implementation, the step of calculating the pose difference of the current frame based on the IMU pose of the current frame, the first code disk pose of the current frame, and the first displacement error of the previous frame specifically includes: adding the first code disk pose of the current frame to the first displacement error of the previous frame to obtain a calibrated pose value; and calculating the difference between the IMU pose of the current frame and the calibrated pose value as the pose difference of the current frame; wherein the initial frame of the first displacement error is a zero vector. This implementation limits the initial frame of the first displacement error to a zero vector, meaning that when the calculation of the pose difference of the first frame begins, the calibrated pose value is equal to the first code disk pose of the first frame, and the pose difference is equal to the difference between the IMU pose and the first code disk pose. By correcting the code disk pose based on the first displacement error to obtain the calibrated pose value as an intermediate calculation value, and by using the difference between the calibrated pose value and the IMU pose, the calculated pose difference can more accurately reflect the displacement error between the code disk pose and the IMU pose.

[0058] In one implementation, the step of calculating the first displacement error of the current frame based on the cumulative error covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the pose difference of the current frame specifically includes: multiplying the cumulative error covariance matrix of the previous frame and the pose difference of the current frame as the seventh covariance matrix; using the seventh covariance matrix as the dividend, using the fifth covariance matrix as the divisor, and using the quotient of the seventh covariance matrix and the fifth covariance matrix as the eighth covariance matrix; and using the sum of the first displacement error of the previous frame and the eighth covariance matrix as the first displacement error of the current frame.

[0059] As one implementation method, the method for obtaining the second covariance matrix corresponding to the first encoder posture specifically includes: configuring the second covariance matrix as a 3x3 matrix, wherein only the diagonal elements in the second covariance matrix have values, and the elements in the remaining positions are all 0; wherein the values ​​of the diagonal elements of the second covariance matrix are calculated based on the product of the moving distance of the encoder data acquisition and various configuration errors.

[0060] In one implementation, the configuration errors include at least: measurement error, drift error, and slippage error; wherein, the measurement error refers to a correction value set for errors that may arise during the code disk measurement data process due to inaccurate measurement; the drift error refers to a correction value set for errors that may arise during the code disk measurement data process due to data drift; and the slippage error refers to a correction value set for errors that may arise during the code disk measurement data process due to machine wheel slippage. It should be noted that the measurement error, drift error, and slippage error are all pre-configured values ​​used to correct errors in the code disk data.

[0061] As one implementation method, the method for calculating the elements on the diagonal of the second covariance matrix based on the product of the movement distance obtained from the encoder data and various configuration errors specifically includes: obtaining the movement distance based on the encoder data; taking the sum of the measurement error and the slippage error as a first error; determining the first row diagonal element of the second covariance matrix as the product of the first error and the movement distance; taking the sum of the slippage error and the drift error as a second error; and determining the second row diagonal element and the third row diagonal element of the second covariance matrix as the product of the second error and the movement distance; wherein, the first row diagonal element of the second covariance matrix refers to the element in the first row and first column of the second covariance matrix; the second row diagonal element of the second covariance matrix refers to the element in the second row and second column of the second covariance matrix; similarly, the third row diagonal element of the third covariance matrix refers to the element in the third row and third column of the second covariance matrix.

[0062] In one embodiment, the measurement error is set to 0.001, the drift error is set to 0.0001, and the slippage error is set to 0.001.

[0063] In some embodiments of this application, a chip is provided, wherein the chip internally stores a computer program, and the computer program stored internally is executed by a processor to perform the aforementioned method for optimizing displacement based on the fusion of IMU data and code disk data.

[0064] Obviously, the above embodiments are only some embodiments of the present invention, and not all embodiments. The technical solutions of various embodiments can be combined with each other. If terms such as "first," "second," and "third" appear in the embodiments, they are for the purpose of distinguishing related features and should not be construed as indicating or implying their relative importance, order, or number of technical features.

[0065] Those skilled in the art will understand that all or part of the steps in the methods described above can be implemented by a program instructing related hardware. This program is stored in a storage medium and includes several instructions to cause a microcontroller, chip, or processor to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, portable hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk.

[0066] It should be noted that any process or method description in the flowchart or otherwise described herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, and the scope of the preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order described or discussed, including substantially simultaneously or in reverse order according to the functions involved, as should be understood by those skilled in the art to which the embodiments of the invention pertain.

[0067] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention. The sequence numbers of the above embodiments of the present invention are merely for description and do not represent the superiority or inferiority of the embodiments.

Claims

1. A method for optimizing displacement based on the fusion of IMU data and encoder data, characterized in that, The method for optimizing displacement based on the fusion of IMU data and encoder data specifically includes: The IMU data is integrated based on manifold pre-integration to obtain the IMU pose, and the first covariance matrix corresponding to the IMU pose is obtained. Integrate the encoder data to obtain the first encoder pose, and obtain the second covariance matrix corresponding to the first encoder pose; The first covariance matrix, the second covariance matrix, the IMU pose, and the first code disk pose are fused based on the error Kalman update process to obtain the first displacement error; The second code disk pose is obtained by optimizing the first code disk pose and the corresponding first displacement error. The pose of the second code disk is integrated based on the group space closed integral to obtain the optimized current cumulative displacement. Specifically, the step of integrating the IMU data based on manifold pre-integration to obtain the IMU pose includes: Acquire the angular velocity measurement value, angular velocity deviation value, angular velocity measurement noise, acceleration measurement value, acceleration deviation value, and acceleration measurement noise from the IMU data of the current frame; Pre-integration is performed based on angular velocity measurements, angular velocity deviations, and angular velocity measurement noise to obtain the IMU attitude of the current frame; Pre-integration is performed based on IMU attitude, acceleration measurement value, acceleration deviation value and acceleration measurement noise to obtain the IMU velocity of the current frame; Pre-integration is performed based on IMU velocity, acceleration measurement values, acceleration deviation values, and acceleration measurement noise to obtain the IMU displacement of the current frame; The formula for pre-integration based on the angular velocity measurement value, angular velocity deviation value, and angular velocity measurement noise is as follows: R ij ； The formula for pre-integration based on IMU attitude, acceleration measurement value, acceleration deviation value, and acceleration measurement noise is as follows: v ij ； The formula for pre-integration based on IMU velocity, acceleration measurements, acceleration deviation, and acceleration measurement noise is as follows: p ij ； in, R ij IMU attitude calculated by pre-integration; k It is the angular velocity measurement value of the current frame; It is the angular velocity deviation value of the current frame; This is the noise from the angular velocity measurement in the current frame; t is the change over time; v ij It is the IMU speed calculated by pre-integration; p ij It is the IMU displacement calculated by pre-integration; the IMU pose includes IMU attitude, IMU velocity and IMU displacement; k It is the measured acceleration value of the current frame. It is the acceleration deviation of the current frame. It is the acceleration measurement noise of the current frame.

2. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 1, characterized in that, The process of optimizing and obtaining the second code disk pose based on the first code disk pose and the corresponding first displacement error specifically includes: Obtain the displacement of the first code disk in the current frame corresponding to the pose of the first code disk in the current frame; The sum of the first code disk displacement in the current frame and the first displacement error in the current frame is used as the second code disk displacement corresponding to the second code disk pose in the current frame, so as to optimize the second code disk pose.

3. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 2, characterized in that, The integration of the second code disk pose based on group space closed integral to obtain the optimized current cumulative displacement specifically includes: Obtain the corresponding displacement of the second code disk in the current frame based on the pose of the second code disk in the current frame; Obtain the corresponding rotation increment for the current frame based on the IMU pose of the current frame; The rotation matrix of the current frame is obtained by using the rotation matrix transformation formula based on the rotation increment of the current frame. The attitude change value of the second code disk in the current frame is calculated based on the rotation matrix of the current frame, the displacement of the second code disk in the current frame, and the Jacobian matrix. Based on the Lie group right-multiplication integral increment formula, the sum of the second code disk attitude change value in the current frame and the cumulative displacement in the previous frame is taken as the current cumulative displacement.

4. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 3, characterized in that, The integration of the encoder data to obtain the first encoder pose is achieved using a group closed integration method. The integration process specifically includes: Obtain the corresponding rotation increment for the current frame based on the IMU pose of the current frame; The rotation matrix of the current frame is obtained by using the rotation matrix transformation formula based on the rotation increment of the current frame. Obtain the first encoder displacement of the current frame based on the encoder data of the current frame; The attitude change value of the first encoder is calculated based on the rotation matrix of the current frame, the displacement of the first encoder in the current frame, and the Jacobian matrix. Based on the Lie group right-multiplication integral increment formula, the first code disk pose of the current frame is calculated by combining the first code disk pose of the previous frame with the change value of the code disk pose of the current frame.

5. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 4, characterized in that, The error-based Kalman update process fuses the first covariance matrix, the second covariance matrix, the IMU pose, and the first code disk pose to obtain the first displacement error, specifically including: The cumulative error covariance matrix of the current frame is calculated based on the first covariance matrix corresponding to the IMU pose of the current frame and the second covariance matrix corresponding to the first code disk pose of the current frame. The pose difference of the current frame is calculated based on the IMU pose of the current frame, the first encoder pose of the current frame, and the first displacement error of the previous frame. The first displacement error of the current frame is calculated based on the cumulative error covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first code disk pose of the current frame, and the pose difference of the current frame.

6. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 5, characterized in that, The calculation of the cumulative error covariance matrix for the current frame based on the first covariance matrix corresponding to the IMU pose in the current frame and the second covariance matrix corresponding to the first encoder pose in the current frame specifically includes: Obtain the cumulative error covariance matrix of the previous frame; Calculate the transpose of the cumulative error covariance matrix of the previous frame; The product of the cumulative error covariance matrix of the previous frame and its transpose is calculated as the fourth covariance matrix; The sum of the first covariance matrix corresponding to the IMU pose in the current frame, the second covariance matrix corresponding to the first code disk pose in the current frame, and the cumulative error covariance matrix of the previous frame is used as the fifth covariance matrix. The fourth covariance matrix is ​​used as the dividend, the fifth covariance matrix is ​​used as the divisor, and the quotient of the fourth covariance matrix and the fifth covariance matrix is ​​used as the sixth covariance matrix. The difference between the cumulative error covariance matrix of the previous frame and the sixth covariance matrix is ​​used as the cumulative error covariance matrix of the current frame. The initial frame of the cumulative error covariance matrix is ​​an identity matrix.

7. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 6, characterized in that, The calculation of the pose difference in the current frame based on the IMU pose in the current frame, the first encoder pose in the current frame, and the first displacement error in the previous frame specifically includes: Add the first code disk pose of the current frame to the first displacement error of the previous frame to obtain the calibrated pose value; calculate the difference between the IMU pose of the current frame and the calibrated pose value as the pose difference of the current frame. Wherein, the initial frame of the first displacement error is the zero vector.

8. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 7, characterized in that, The calculation of the first displacement error of the current frame based on the cumulative error covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU pose of the current frame, the second covariance matrix corresponding to the first encoder pose of the current frame, and the pose difference of the current frame specifically includes: The product of the cumulative error covariance matrix of the previous frame and the pose difference of the current frame is used as the seventh covariance matrix. The seventh covariance matrix is ​​used as the dividend, the fifth covariance matrix is ​​used as the divisor, and the quotient of the seventh covariance matrix and the fifth covariance matrix is ​​used as the eighth covariance matrix. The sum of the first displacement error of the previous frame and the eighth covariance matrix is ​​used as the first displacement error of the current frame.

9. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 7, characterized in that, Obtaining the second covariance matrix corresponding to the first code disk pose specifically includes: configuring the second covariance matrix as a 3x3 matrix, wherein only the diagonal elements in the second covariance matrix have values, and the elements at other positions are all 0; wherein, the values ​​of the diagonal elements of the second covariance matrix are calculated based on the product of the first code disk displacement obtained from the code disk data and various configuration errors.

10. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 9, characterized in that, The configuration errors include at least: measurement error, drift error, and slippage error; the values ​​of the diagonal elements of the second covariance matrix are calculated based on the product of the first code disk displacement obtained from the code disk data and various configuration errors, specifically including: Obtain the first encoder displacement of the current frame based on the encoder data of the current frame; The sum of the measurement error and the slippage error is taken as the first error; The first row of the diagonal elements of the second covariance matrix in the current frame is determined to be equal to the product of the first error and the first code disk displacement in the current frame; The sum of drift error and slippage error is taken as the second error; The diagonal elements of the second row and the third row of the second covariance matrix in the current frame are both determined to be equal to the product of the second error and the displacement of the first code disk in the current frame.

11. A chip internally storing a computer program, characterized in that, The computer program stored inside the chip is executed by the processor as described in any one of claims 1 to 10, which is a method for optimizing displacement based on the fusion of IMU data and code disk data.

Images