CN-118999534-B - IMU data and code disc data fusion-based displacement optimization method and chip

CN118999534BCN 118999534 BCN118999534 BCN 118999534BCN-118999534-B

Abstract

The application discloses a method and a chip for optimizing displacement based on IMU data and code wheel data fusion, wherein the method comprises the steps of integrating IMU data based on manifold pre-integration to obtain IMU pose, and obtaining a first covariance matrix corresponding to the IMU pose; the method comprises the steps of integrating code disc data based on group space closed integration to obtain a first code disc pose and a second covariance matrix corresponding to the first code disc pose, fusing the first covariance matrix, the second covariance matrix, the IMU pose and the first code disc pose based on an error Kalman updating process to obtain a first displacement error, optimizing the first displacement error based on the first code disc pose and the corresponding first displacement error to obtain a second code disc pose, and integrating the second code disc pose based on the group space closed integration to obtain the optimized current accumulated displacement. The application solves the problem of large displacement error of the code disc caused by slipping through fusion of the IMU data and the code disc data, and effectively optimizes the displacement accuracy.

Inventors

LI MING

Assignees

珠海一微半导体股份有限公司

Dates

Publication Date: 20260505
Application Date: 20230518

Claims (11)

1. The method for optimizing the displacement based on the fusion of the IMU data and the code disc data is characterized by comprising the following steps of: integrating the IMU data based on manifold pre-integration to obtain IMU pose, and obtaining a first covariance matrix corresponding to the IMU pose; Integrating the code wheel data to obtain a first code wheel pose, and obtaining a second covariance matrix corresponding to the first code wheel pose; Fusing the first covariance matrix, the second covariance matrix, the IMU pose and the first code disc pose based on an error Kalman updating process to acquire a first displacement error; Optimizing and acquiring the pose of the second code wheel based on the pose of the first code wheel and the corresponding first displacement error; Integrating the pose of the second code wheel based on the group space closed integration to obtain the optimized current accumulated displacement; the integration of the IMU data based on manifold pre-integration to obtain the IMU pose specifically comprises the following steps: Acquiring an angular velocity measurement value, an angular velocity deviation value, angular velocity measurement noise, an acceleration measurement value, an acceleration deviation value and acceleration measurement noise in IMU data of a current frame; pre-integrating based on the angular velocity measurement value, the angular velocity deviation value and the angular velocity measurement noise to obtain the IMU gesture of the current frame; pre-integrating based on the IMU gesture, the acceleration measurement value, the acceleration deviation value and the acceleration measurement noise to obtain the IMU speed of the current frame; Pre-integrating based on the IMU speed, the acceleration measurement value, the acceleration deviation value and the acceleration measurement noise to obtain IMU displacement of the current frame; Wherein, the formula for pre-integrating based on the angular velocity measurement value, the angular velocity deviation value, and the angular velocity measurement noise is: R ij ; The formula for pre-integrating based on the IMU gesture, the acceleration measurement value, the acceleration deviation value and the acceleration measurement noise is as follows: v ij ; the formula for pre-integrating based on the IMU speed, the acceleration measurement value, the acceleration deviation value and the acceleration measurement noise is as follows: p ij ; Wherein, the R ij is the IMU gesture calculated by pre-integration; k Is the angular velocity measurement of the current frame; Is the angular velocity deviation value of the current frame; is the angular velocity measurement noise of the current frame; t is the time variation; v ij is the IMU speed calculated by pre-integration; p ij is IMU displacement calculated by pre-integration, wherein the IMU pose comprises an IMU pose, an IMU speed and IMU displacement; k Is a measure of the acceleration of the current frame, Is the acceleration deviation of the current frame, Is the acceleration measurement noise of the current frame.
2. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 1, wherein the optimizing the acquisition of the second code wheel pose based on the first code wheel pose in combination with the corresponding first displacement error specifically comprises: Acquiring first code disc displacement of a current frame corresponding to the first code disc pose of the current frame; And taking the sum of the first code disc displacement of the current frame and the first displacement error of the current frame as the second code disc displacement corresponding to the second code disc pose of the current frame so as to optimize the second code disc pose.
3. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 2, wherein the integrating the second code wheel pose based on group space closed integration to obtain the optimized current accumulated displacement specifically comprises: acquiring a second code disc displacement of the corresponding current frame based on the second code disc pose of the current frame; Acquiring a corresponding rotation increment of the current frame based on the IMU pose of the current frame; acquiring a rotation matrix of the current frame through a rotation matrix conversion formula based on the rotation increment of the current frame; Calculating a second code disc attitude change value of the current frame based on the rotation matrix of the current frame and the second code disc displacement of the current frame in combination with the jacobian matrix; And taking the sum of the second code disc attitude change value of the current frame and the accumulated displacement of the previous frame as the current accumulated displacement based on the Li-group right product incremental formula.
4. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 3, wherein the integration of the code wheel data to obtain the first code wheel pose is implemented by adopting a group closed integration mode, and the integration process specifically includes: Acquiring a corresponding rotation increment of the current frame based on the IMU pose of the current frame; acquiring a rotation matrix of the current frame through a rotation matrix conversion formula based on the rotation increment of the current frame; acquiring a first code disc displacement of a current frame based on code disc data of the current frame; calculating a first code disc attitude change value based on the rotation matrix of the current frame and the first code disc displacement of the current frame in combination with the jacobian matrix; and calculating the first code disc pose of the current frame according to the first code disc pose of the previous frame and the code disc pose change value of the current frame based on the prune group right product incremental formula.
5. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 4, wherein the error-based kalman update process fuses the first covariance matrix, the second covariance matrix, the IMU pose and the first code wheel pose to obtain the first displacement error, and specifically comprises: Calculating an accumulated error covariance matrix of the current frame based on a first covariance matrix corresponding to the IMU pose of the current frame and a second covariance matrix corresponding to the first code disc pose of the current frame; calculating a pose difference value of the current frame based on the IMU pose of the current frame, the first code wheel 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 accumulated 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 disc pose of the current frame and the pose difference value of the current frame.
6. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 5, wherein the calculating the current frame accumulated error covariance matrix based on the first covariance matrix corresponding to the current frame IMU pose and the second covariance matrix corresponding to the current frame first code wheel pose specifically comprises: acquiring an accumulated error covariance matrix of a previous frame; calculating a transpose matrix of the accumulated error covariance matrix of the previous frame; calculating the product of the accumulated error covariance matrix of the previous frame and the transpose matrix of the previous frame to be used as a fourth covariance matrix; Calculating a sum of a first covariance matrix corresponding to the IMU pose of the current frame, a second covariance matrix corresponding to the first code disc pose of the current frame and an accumulated error covariance matrix of the previous frame to serve as a fifth covariance matrix; Taking the fourth covariance matrix as a dividend, taking the fifth covariance matrix as a divisor, and taking the quotient of the fourth covariance matrix and the fifth covariance matrix as a sixth covariance matrix; calculating the difference value between the last frame accumulated error covariance matrix and the sixth covariance matrix as the accumulated error covariance matrix of the current frame; wherein the initial frame of the accumulated error covariance matrix is an identity matrix.
7. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 6, wherein the calculating the current frame pose difference based on the current frame IMU pose, the current frame first code wheel pose and the previous frame first displacement error specifically comprises: The method comprises the steps of adding a first code disc pose of a current frame and a first displacement error of a previous frame to obtain a calibration pose value, calculating a difference value between the IMU pose of the current frame and the calibration pose value as a pose difference value of the current frame; wherein the initial frame of the first displacement error is a zero vector.
8. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 7, wherein the calculating the first displacement error of the current frame based on the last frame accumulated error covariance matrix, the last frame first displacement error, the first covariance matrix corresponding to the current frame IMU pose, the second covariance matrix corresponding to the current frame first code wheel pose, and the current frame pose difference value specifically comprises: Taking the product of the accumulated error covariance matrix of the previous frame and the pose difference value of the current frame as a seventh covariance matrix; taking the seventh covariance matrix as a dividend, taking the fifth covariance matrix as a divisor, and taking the quotient of the seventh covariance matrix and the fifth covariance matrix as an eighth covariance matrix; Taking 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.
9. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 7, wherein the method for optimizing displacement based on IMU data and code wheel data fusion is characterized by obtaining a second covariance matrix corresponding to a first code wheel pose, and specifically comprises the steps of configuring the second covariance matrix into a matrix with 3 rows and 3 columns, wherein only elements on a diagonal line in the second covariance matrix have values, and elements on other positions are all 0, wherein the values of the elements on the diagonal line of the second covariance matrix are obtained through calculation based on products of first code wheel displacement obtained by code wheel data and various configuration errors.
10. The method for optimizing displacement based on IMU data and code wheel data fusion according to claim 9, wherein the configuration errors at least comprise measurement errors, drift errors and slip errors, values of elements on a diagonal of the second covariance matrix are calculated and obtained based on products of first code wheel displacement obtained by code wheel data and various configuration errors, and the method specifically comprises the following steps: acquiring a first code disc displacement of a current frame based on code disc data of the current frame; Taking the sum of the measurement error and the slip error as a first error; determining a first row diagonal element of the second covariance matrix of the current frame to be equal to a product of a first error and a first code wheel displacement of the current frame; taking the sum of the drift error and the slip error as a second error; and determining a second row diagonal element and a third row diagonal element of the second covariance matrix of the current frame to be equal to the product of the second error and the first code disc displacement of the current frame.
11. A chip having a computer program stored therein, wherein the computer program stored therein is executed by a processor to perform the method of optimizing displacement based on IMU data and code wheel data fusion according to any one of claims 1 to 10.

Description

IMU data and code disc data fusion-based displacement optimization method and chip Technical Field The application relates to the field of pose optimization, in particular to a method and a chip for optimizing displacement based on IMU data and code wheel data fusion. Background Currently robots generally adopt inertial navigation or visual navigation. The inertial navigation robot performs pose estimation based on an IMU (INTERTIAL MEASUREMENT UNIT, an inertial sensor), and other sensor data are fused based on the IMU to perform complementary correction on the IMU data based on the other sensor data along with the increase of pose accuracy requirements, so that the pose estimation accuracy is improved. In the technical scheme of detecting the displacement of the robot by adopting the code disc sensor, when the robot slips, the displacement error detected by the code disc sensor is large, so that the pose is inaccurate. How to optimize the displacement error of the code wheel for the situation of robot slipping is a technical problem in the art. The existing calculation mode for optimizing displacement errors is usually based on the mean value calculation of displacement data measured by various sensors, and the optimization mode has the problems of poor optimization effect and larger error of the finally optimized displacement. Disclosure of Invention The application provides a method and a chip for optimizing displacement based on IMU data and code disc data fusion, which have the following specific technical scheme: a method for optimizing displacement based on IMU data and code disc data fusion specifically comprises the steps of integrating IMU data based on manifold pre-integration to obtain IMU pose and obtain a first covariance matrix corresponding to the IMU pose, integrating code disc data to obtain a first code disc pose and obtain a second covariance matrix corresponding to the first code disc pose, fusing the first covariance matrix, the second covariance matrix, the IMU pose and the first code disc pose based on an error Kalman updating process to obtain a first displacement error, optimizing obtaining a second code disc pose based on the first code disc pose and combining the corresponding first displacement error, and integrating the second code disc pose based on group space closed integration to obtain the optimized current accumulated displacement. Further, the method for optimizing and obtaining the second code disc pose based on the first code disc pose and the corresponding first displacement error specifically comprises the steps of obtaining the first code disc displacement of the corresponding current frame in the first code disc poses of the current frame, and taking the sum of the first code disc displacement of the current frame and the first displacement error of the current frame as the second code disc displacement corresponding to the second code disc pose of the current frame so as to optimize the second code disc pose. The integration of the second code wheel pose is realized based on group closed space integration, and the integration process specifically comprises the steps of acquiring corresponding second code wheel displacement of a current frame based on the second code wheel pose of the current frame, acquiring corresponding rotation increment of the current frame based on the IMU pose of the current frame, acquiring a rotation matrix of the current frame based on the rotation increment of the current frame through a rotation matrix conversion formula, calculating a second code wheel pose change value of the current frame based on the rotation matrix of the current frame and the second code wheel displacement of the current frame in combination with a jacobian matrix, and taking the sum of the second code wheel pose change value of the current frame and the accumulated displacement of the previous frame as the current accumulated displacement based on a Li group right product partial increment formula. Further, the method includes integrating IMU data based on manifold pre-integration to obtain IMU pose, and obtaining a first covariance matrix corresponding to the IMU pose, wherein the method specifically includes obtaining an angular velocity measurement value, an angular velocity deviation value, an angular velocity measurement noise, an acceleration measurement value, an acceleration deviation value and an acceleration measurement noise in IMU data of a current frame, pre-integrating based on the angular velocity measurement value, the angular velocity deviation value and the angular velocity measurement noise to obtain IMU pose of the current frame, pre-integrating based on the IMU pose, the acceleration measurement value, the acceleration deviation value and the acceleration measurement noise to obtain IMU velocity of the current frame, pre-integrating based on the IMU velocity, the acceleration measurement value, the acceleration deviation value and the acceler