Method and chip for optimizing displacement based on IMU data and coded disc data fusion

Through the fusion method of IMU and code disk data, manifold pre-integration and group space closed integration, combined with error Kalman update, the robot displacement error is optimized, the displacement inaccuracy problem caused by robot slip is solved, and the position accuracy is improved and credibility.

CN118999534AActive Publication Date: 2024-11-22AMICRO SEMICONDUCTOR CO LTD
View PDF 3 Cites 0 Cited by

Patent Information

Application Number
CN202310562660.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2023-05-18
Publication Date
2024-11-22
Estimated Expiration
2043-05-18

AI Technical Summary

Technical Problem

In the case of robot slipping, the displacement error of the code disc sensor detection is large, resulting in inaccurate position and poor optimization method effect.

Method used

Using a fusion method based on IMU data and code disk data, IMU and code disk poses are obtained through manifold pre-integration and group space closed integral, combined with the error Kalman update process, the displacement error is optimized and the estimation of the counterposition posture of the ground is reduced. The impact of

Benefits of technology

It improves position accuracy, reduces the error in the displacement amount of the code disk, enhances the credibility of the displacement amount, and solves the problem of large displacement error caused by the machine wheel being stuck.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118999534A_ABST
    Figure CN118999534A_ABST
Patent Text Reader

Abstract

The invention discloses a method and chip for optimizing displacement based on IMU data and coded disc data fusion, and the method comprises the steps: carrying out the integration of IMU data based on manifold pre-integration to obtain an IMU pose, and obtaining a first covariance matrix corresponding to the IMU pose; integrating the coded disc data based on group space closed integration to obtain a first coded disc pose, and obtaining a second covariance matrix corresponding to the first coded disc pose; fusing the first covariance matrix, the second covariance matrix, the IMU pose and the first coded disc pose based on an error Kalman updating process to obtain a first displacement error; based on the first code disc pose, combining with corresponding first displacement error optimization to obtain a second code disc pose; and integrating the second coded disc pose based on group space closed integration to obtain the optimized current accumulated displacement. According to the invention, through fusion of IMU data and code disc data, the problem of large code disc displacement error caused by slipping is solved, and the displacement precision is effectively optimized.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the field of posture optimization, and specifically to a method and chip for optimizing displacement based on the fusion of IMU data and encoder data. Background Art

[0002] At present, robots usually use inertial navigation or visual navigation. Among them, robots that use inertial navigation perform posture estimation based on IMU (Intertial Measurement Unit). With the increase in the requirements for posture accuracy, in order to improve the accuracy of posture estimation, other sensor data are usually fused based on IMU, so as to supplement and correct IMU data based on other sensor data, thereby improving the accuracy of posture estimation. In the technical solution of using a code disc sensor to detect the displacement of the robot, when the robot slips, the displacement error detected by the code disc sensor is large, resulting in inaccurate posture. How to optimize the displacement error of the code disc in the case of robot slipping is a technical problem in this field. At present, the calculation method for optimizing the displacement error is usually based on the mean calculation of the displacement data measured by multiple sensors. This optimization method has the problem of poor optimization effect and the displacement after final optimization still has a large error. 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 code disk data specifically includes: integrating the IMU data based on manifold pre-integration to obtain the IMU posture, and obtaining a first covariance matrix corresponding to the IMU posture; integrating the code disk data to obtain a first code disk posture, and obtaining a second covariance matrix corresponding to the first code disk posture; fusing the first covariance matrix, the second covariance matrix, the IMU posture and the first code disk posture based on an error Kalman update process to obtain a first displacement error; optimizing the second code disk posture based on the first code disk posture combined with the corresponding first displacement error; integrating the second code disk posture based on group space closed-form integration to obtain the optimized current accumulated displacement.

[0005] Furthermore, the method of optimizing and obtaining the second code disk posture based on the first code disk posture in combination with the corresponding first displacement error specifically includes: obtaining the first code disk displacement of the current frame corresponding to the first code disk posture of the current frame; taking 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 posture of the current frame, so as to optimize the second code disk posture.

[0006] Furthermore, the integration of the second code disk posture is implemented based on group closed space integration, and the integration process specifically includes: obtaining the corresponding second code disk displacement of the current frame based on the second code disk posture of the current frame; obtaining the corresponding rotation increment of the current frame based on the IMU posture of the current frame; obtaining the rotation matrix of the current frame through the rotation matrix conversion formula based on the rotation increment of the current frame; calculating the second code disk posture 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; based on the Lie group right-multiplication integral increment formula, taking the sum of the second code disk posture change value of the current frame and the cumulative displacement of the previous frame as the current cumulative displacement.

[0007] Further, the integration of IMU data based on manifold pre-integration to obtain IMU posture and obtain a first covariance matrix corresponding to the IMU posture specifically includes: obtaining angular velocity measurement values, angular velocity deviation values, angular velocity measurement noise, acceleration measurement values, acceleration deviation values ​​and acceleration measurement noise in the IMU data of the current frame; pre-integrating based on the angular velocity measurement values, angular velocity deviation values ​​and angular velocity measurement noise to obtain the IMU posture of the current frame; pre-integrating based on the IMU posture, acceleration measurement values, acceleration deviation values ​​and acceleration measurement noise to obtain the IMU velocity of the current frame; pre-integrating based on the IMU velocity, acceleration measurement values, acceleration deviation values ​​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 values, angular velocity deviation values ​​and angular velocity measurement noise is: Among them, the formula for pre-integration based on IMU attitude, acceleration measurement value, acceleration deviation value and acceleration measurement noise is: Among them, the formula for pre-integration based on IMU velocity, acceleration measurement value, acceleration deviation value and acceleration measurement noise is: Among them, ΔR ij IMU attitude calculated by pre-integration; is the angular velocity measurement value 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 change; Δv ij is the IMU velocity calculated by pre-integration; Δp ij is the IMU displacement calculated by pre-integration; the IMU posture includes IMU attitude, IMU velocity and IMU displacement; is the measured value of the acceleration of the current frame, is the acceleration deviation of the current frame, is the acceleration measurement noise of the current frame.

[0008] Furthermore, the integrating the code disk data based on the group space closed-form integral to obtain the first code disk posture specifically includes: obtaining the corresponding rotation increment of the current frame based on the IMU posture 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 conversion formula; obtaining the first code disk displacement of the current frame based on the code disk data of the current frame; calculating the first code disk posture change value based on the rotation matrix of the current frame, the first code disk displacement of the current frame and the Jacobian matrix; calculating the first code disk posture of the current frame by combining the first code disk posture of the previous frame with the code disk posture change value of the current frame based on the Lie group right-multiplication integral increment formula.

[0009] Furthermore, the error Kalman update process fuses the first covariance matrix, the second covariance matrix, the IMU posture and the first code disk posture to obtain a first displacement error, specifically including: calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU posture of the current frame and the second covariance matrix corresponding to the first code disk posture of the current frame; calculating the posture difference of the current frame based on the IMU posture of the current frame, the first code disk posture of the current frame and the first displacement error of the previous frame; 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 posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the posture difference of the current frame.

[0010] Furthermore, the method of calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU posture of the current frame and the second covariance matrix corresponding to the first code disk posture of the current frame specifically includes: obtaining the cumulative error covariance matrix of the previous frame; calculating the transposed matrix 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 transposed matrix as the fourth covariance matrix; calculating the sum of the first covariance matrix corresponding to the IMU posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the cumulative error covariance matrix of the previous frame as the fifth covariance matrix; taking the fourth covariance matrix as the dividend, the fifth covariance matrix as the divisor, and 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 ​​the unit matrix.

[0011] Furthermore, the present invention calculates the current frame posture difference based on the current frame IMU posture, the current frame first code disk posture and the previous frame first displacement error, specifically including: adding the current frame first code disk posture and the previous frame first displacement error to obtain the calibration posture value; calculating the difference between the current frame IMU posture and the calibration posture value as the current frame posture difference; wherein, the initial frame of the first displacement error is a zero vector.

[0012] Furthermore, the method 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 posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the posture difference of the current frame specifically includes: taking the product of the cumulative error covariance matrix of the previous frame and the posture difference of the current frame as the seventh covariance matrix; taking the seventh covariance matrix as the dividend, the fifth covariance matrix as the divisor, and the quotient of the seventh covariance matrix and the fifth covariance matrix as the 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.

[0013] Furthermore, a second covariance matrix corresponding to the first code disk posture is obtained, specifically including: configuring the second covariance matrix as a matrix of 3 rows and 3 columns, and only the elements on the diagonal of the second covariance matrix have values, and the elements in other positions are all 0; wherein 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 based on the code disk data and various configuration errors.

[0014] Furthermore, the configuration error includes at least: measurement error, drift error and slip 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 based on 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 slip error as the first error; determining the first row diagonal elements of the second covariance matrix of the current frame to be 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 slip error as the second error; and determining the second row diagonal elements and the third row diagonal elements of the second covariance matrix of the current frame to be equal to the product of the second error and the first code disk displacement of the current frame.

[0015] The present application also discloses a chip having a computer program stored therein. When the computer program stored therein is executed by a processor, the method for optimizing displacement based on the fusion of IMU data and code disk data as described above is executed.

[0016] The present application describes a method and chip for optimizing displacement based on the fusion of IMU data and code disk data, which corrects the displacement of the code disk based on the IMU data, and uses the first displacement error after the fusion of the IMU displacement obtained by the IMU data and the code disk displacement obtained by the code disk data as the standard to calibrate and optimize the code disk displacement obtained based on the code disk data, thereby solving the problem of large displacement error obtained by the code disk due to the stuck machine wheel. The calculation of the code disk displacement through closed-form integration in group space can effectively reduce the impact of uneven ground and undulating ground on the code disk pose estimation results, improve pose accuracy, and thus improve the credibility of the displacement obtained based on the code disk. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 A flowchart of a method for optimizing displacement based on the fusion of IMU data and code disk data described in an embodiment of the present application. DETAILED DESCRIPTION

[0018] The following will describe the embodiments of the present application in detail in conjunction with the accompanying drawings. It should be understood that the specific embodiments described below are only used to explain the present application and are not used to limit the present application.

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

[0020] Based on manifold pre-integration, the IMU data is integrated to obtain the IMU posture, and the first covariance matrix corresponding to the IMU posture is obtained; wherein, the pre-integration refers to taking the initial state of the integration as a reference, and discarding the IMU data state of the series of processes within the equivalent time period after the integration operation, so as to realize that the posture 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, and only leaves the key frames that have been strictly screened as the optimization state during global optimization, thereby achieving the purpose of reducing the amount of calculation while maintaining the calculation accuracy; the IMU refers to the Intertial Measurement Unit, that is, a device for measuring the three-axis posture and acceleration of an object, usually including a three-axis gyroscope and a three-axis acceleration, and the IMU data usually includes a three-axis angular velocity and a three-axis acceleration.

[0021] Based on the group space closed-form integration, the code disk data is integrated to obtain the first code disk posture, and the second covariance matrix corresponding to the first code disk posture is obtained; wherein the group space closed-form integration refers to the integration of the code disk data on the Lie group space, which is an error-free closed-form integration and is more accurate than other commonly used integrations. Preferably, the group space closed-form integration in the present application is on the SE (3) space, and the group space closed-form integration can effectively reduce the impact of ground unevenness and ground undulation on the posture estimation results, thereby improving the accuracy of posture estimation.

[0022] Based on the error Kalman update process, the first covariance matrix, the second covariance matrix, the IMU posture and the first code disk posture are fused to obtain the first displacement error; wherein, the error Kalman update process refers to the update process of the state error Kalman filter (ESFK, Error-State Kalman Filter), and the state error Kalman filter is a variant application of the Kalman filter, which is a nonlinear filter for time-varying systems. In this application, the displacement errors in the IMU posture and the code disk posture are filtered based on the state error Kalman filter, which can be linearized more accurately.

[0023] The second code disk posture is obtained based on the first code disk posture combined with the corresponding first displacement error optimization; specifically, the calculation of the first displacement error takes the IMU posture into consideration, so that this step performs displacement optimization on the first code disk posture based on the IMU posture, thereby improving the credibility of the code disk displacement.

[0024] The second code disk posture is integrated to obtain the optimized current cumulative displacement; preferably, the second code disk posture is integrated by, but not limited to, group space closed-form integration. Specifically, this step integrates the second code disk posture after the displacement is optimized to obtain the corrected and optimized current cumulative displacement, thereby eliminating the problem of inaccurate displacement obtained by the code disk due to factors such as slippage, and optimizing the displacement credibility.

[0025] As one of the implementation modes, the optimization of obtaining the second code disk posture based on the first code disk posture in combination with the corresponding first displacement error specifically includes: obtaining the first code disk displacement of the current frame corresponding to the first code disk posture of the current frame; taking 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 posture of the current frame, so as to optimize the second code disk posture. In this technical solution, the first code disk posture is corrected and optimized by the first displacement error. The preliminary optimization process is mainly to obtain the second code disk posture after preliminary optimization by adding the first displacement error to the first code disk posture, thereby realizing the correction of the displacement of the code disk based on IMU data, and solving the problem of large displacement error obtained by the code disk due to the stuck wheel of the machine.

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

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

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

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

[0030] In this step, the ΔT in the formula refers to the change value of the second code disk posture in the current frame; the R refers to the rotation matrix of the current frame; the J refers to the Jacobian matrix; and the Δp refers to the displacement of the second code disk 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 of the current frame and the cumulative displacement of 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, the ΔT in the formula j refers to the cumulative displacement of the previous frame, and ΔT refers to the change value of the second code disk posture in the current frame. j+1 It 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. In this embodiment, the current cumulative displacement is calculated by using the optimized and corrected second code disk displacement to optimize the accuracy of the cumulative displacement.

[0032] As one implementation mode, the method for integrating IMU data based on manifold pre-integration to obtain IMU posture 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 the current frame of IMU data;

[0033] Pre-integrate the angular velocity measurement value, angular velocity deviation value and angular velocity measurement noise to obtain the IMU attitude of the current frame;

[0034] Pre-integrate 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-integrate based on IMU velocity, acceleration measurement value, acceleration deviation value 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, the angular velocity deviation value and the angular velocity measurement noise is:

[0037] Among them, ΔR ij IMU attitude calculated by pre-integration; is the angular velocity measurement value 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-to-time change.

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

[0039] Among them, ΔR ik The IMU attitude calculated by pre-integration for the current frame; is the measured value of the acceleration of the current frame, is the acceleration deviation of the current frame, is the acceleration measurement noise of the current frame; Δt is the change in time.

[0040] Among them, the formula for pre-integration based on IMU velocity, acceleration measurement value, acceleration deviation value and acceleration measurement noise is:

[0041] Among them, Δp ij is the pre-integrated IMU displacement; Δv ikis the IMU speed calculated by pre-integration; Δt is the change at the moment. It should be noted that the IMU posture in this embodiment includes at least the IMU attitude, the IMU speed and the IMU displacement. In order to avoid repeated integration of acceleration information and gyroscope angular velocity information in the IMU data during the optimization process, this embodiment processes the IMU data independently of the changes in the current state, constructs pre-integration variables, and greatly reduces the amount of calculation in the displacement optimization process. The rotation process is represented by Lie algebra, and the operational characteristics on the Lie group manifold are used to suppress error growth and improve the IMU displacement accuracy. At the same time, Lie algebra is used for calculation and optimization, which reduces the dimension of the state vector, reduces the overall amount of calculation, and improves the utilization of computing resources.

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

[0043] Based on the first covariance matrix corresponding to the IMU posture of the previous frame, it is calculated and obtained in combination with the first covariance matrix transfer formula; wherein the first covariance matrix transfer formula is:

[0044] in, The A j-1 T A j-1 The transposed matrix of

[0045] in, B j-1 T Refers to B j-1 The transposed matrix of

[0046] in, is the right Jacobian matrix of the IMU attitude of the previous frame, The IMU attitude calculated by pre-integration to ignore the 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 matrix with 9 rows and 9 columns, and only the elements on the diagonal of the first covariance matrix have values, and the remaining positions are all 0.

[0048] As one implementation manner, integrating the code disk data based on the group space closed-form integral to obtain the first code disk posture specifically includes:

[0049] Based on the IMU posture of the current frame, the corresponding rotation increment of the current frame is obtained; specifically, the rotation increment refers to the rotation angle around the X-axis, Y-axis and Z-axis in the IMU posture. Based on the rotation increment of the current frame, the rotation matrix of the current frame is obtained through the rotation matrix conversion formula; since the rotation matrix conversion formula is a commonly used formula in this field, it will not be repeated here.

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

[0051] Based on the Lie group right multiplication integral increment formula, the first code disk posture of the previous frame and the first code disk posture change value of the current frame are used to calculate the first code disk posture of the current frame; specifically, the Lie group right multiplication integral increment formula is: ΔT j+1 =ΔTΔT j In this step, the ΔT in the formula j refers to the first encoder position of the previous frame, and ΔT refers to the change value of the first encoder position of the current frame. j+1 Refers to the first encoder posture of the current frame. This embodiment calculates the encoder posture by adopting a group closed integral method, thereby eliminating the influence of the encoder data obtained on the encoder posture calculation under the condition of uneven or undulating ground, and improving the accuracy of the first encoder posture.

[0052] As one of the implementation modes, the error Kalman update process fuses the first covariance matrix, the second covariance matrix, the IMU posture and the first code disk posture to obtain the first displacement error, specifically including: calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU posture of the current frame and the second covariance matrix corresponding to the first code disk posture of the current frame; calculating the posture difference of the current frame based on the IMU posture of the current frame, the first code disk posture of the current frame and the first displacement error of the previous frame; 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 posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the posture difference of the current frame. This embodiment fuses the IMU posture with the first code disk posture during the error Kalman update process, and realizes the first code disk displacement in the first code disk posture based on the IMU posture calibration by calculating the first displacement error, thereby specifically solving the problem of large error in displacement obtained by the code disk due to wheel slippage of the robot, and effectively improving the credibility of displacement obtained 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 posture of the current frame and the second covariance matrix corresponding to the first code disk posture of the current frame specifically includes:

[0054] Obtain the cumulative error covariance matrix of the previous frame; calculate the transposed matrix of the cumulative error covariance matrix of the previous frame; wherein the transposed matrix refers to a new matrix obtained by exchanging rows and columns of the cumulative error covariance matrix of the previous frame.

[0055] Calculate the product of the cumulative error covariance matrix of the previous frame and the transposed matrix corresponding to the cumulative error covariance matrix of the previous frame as the third covariance matrix; calculate the sum of the first covariance matrix corresponding to the IMU posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the cumulative error covariance matrix of the previous frame as the fourth covariance matrix; it should be noted that the calculation steps of the third covariance matrix and the calculation steps of the fourth covariance matrix can be but are not limited to first executing the calculation steps of the third covariance matrix and then executing the calculation steps of the fourth covariance matrix, or first executing the calculation steps of the fourth covariance matrix and then executing the calculation steps of the third covariance matrix, or simultaneously executing the calculation steps of the third covariance matrix and the calculation steps of the fourth covariance matrix, and the order of the two calculation steps is not limited.

[0056] The third covariance matrix is ​​taken as the dividend, the fourth covariance matrix is ​​taken as the divisor, and the quotient of the third covariance matrix and the fourth covariance matrix 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 unit matrix, that is, when the calculation of the cumulative error covariance matrix of the first frame is started, the cumulative error covariance matrix of the previous frame is the unit matrix, and accordingly, the third covariance matrix is ​​equal to the product of the unit matrix and the unit matrix, the fourth covariance matrix is ​​equal to the sum of the first covariance matrix, the second covariance matrix and the unit matrix, and the cumulative error covariance matrix of the current frame is equal to the difference between the unit matrix and the fifth covariance matrix.

[0057] As one of the implementation modes, the current frame posture difference is calculated based on the current frame IMU posture, the current frame first code disk posture and the previous frame first displacement error, specifically including: adding the current frame first code disk posture and the previous frame first displacement error to obtain the calibration posture value; calculating the difference between the current frame IMU posture and the calibration posture value as the current frame posture difference; wherein the initial frame of the first displacement error is a zero vector. In this implementation mode, the initial frame of the first displacement error is limited to a zero vector, that is, when the calculation of the posture difference of the first frame is started, the calibration posture value is equal to the first code disk posture of the first frame, and the posture difference is equal to the difference between the IMU posture and the first code disk posture. Based on the first displacement error, the code disk posture is corrected to obtain the calibration posture value as an intermediate calculation value, and based on the difference between the calibration posture value and the IMU posture, the calculated posture difference can more accurately feedback the displacement error between the code disk posture and the IMU posture.

[0058] As one of the implementation modes, 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 posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the posture difference of the current frame, specifically including: taking the product of the cumulative error covariance matrix of the previous frame and the posture difference of the current frame as the seventh covariance matrix; taking the seventh covariance matrix as the dividend, the fifth covariance matrix as the divisor, and the quotient of the seventh covariance matrix and the fifth covariance matrix as the 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.

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

[0060] As one of the implementation modes, the configuration error at least includes: measurement error, drift error and slip error, etc.; wherein the measurement error refers to a correction value set for an error that may be caused by inaccurate measurement during the code disk measurement data process; the drift error refers to a correction value set for an error that may be caused by data drift during the code disk measurement data process; the slip error refers to a correction value set for an error caused by machine wheel slippage during the code disk measurement data process. It should be noted that the measurement error, drift error and slip error are all pre-configured values ​​for correcting the code disk data error.

[0061] As one of the implementation modes, the method for calculating and obtaining the elements on the diagonal of the second covariance matrix based on the product of the moving distance obtained by the code disk data and various configuration errors specifically includes: obtaining the moving distance based on the code disk data; taking the sum of the measurement error and the slip error as the first error; determining the first row diagonal elements of the second covariance matrix as the product of the first error and the moving distance; taking the sum of the slip error and the drift error as the second error; determining the second row diagonal elements of the second covariance matrix and the third row diagonal elements of the second covariance matrix as the product of the second error and the moving distance; wherein the first row diagonal elements of the second covariance matrix refer to the elements in the first row and first column of the second covariance matrix; the second row diagonal elements of the second covariance matrix refer to the elements in the second row and second column of the second covariance matrix; similarly, the third row diagonal elements of the third covariance matrix refer to the elements in the third row and third column of the second covariance matrix.

[0062] As one implementation manner, the measurement error is set to be equal to 0.001, the drift error is set to be equal to 0.0001, and the slip error is set to be equal to 0.001.

[0063] In some embodiments of the present application, a chip is provided, wherein a computer program is stored inside the chip, and when the computer program stored inside the chip is executed by a processor, the method for optimizing displacement based on the fusion of IMU data and code disk data as described above is executed.

[0064] Obviously, the above embodiments are only some embodiments of the present invention, not all embodiments, and the technical solutions between the various embodiments can be combined with each other. If the terms "first", "second", "third" and the like appear in the embodiments, they are for the convenience of distinguishing the relevant features and cannot be understood 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 above-mentioned implementation methods can be completed by instructing the relevant hardware through a program, and the program is stored in a storage medium, including several instructions for making a single-chip microcomputer, a chip or a processor (processor) perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0066] It should be noted that any process or method description in the flowchart or otherwise described herein may be understood to represent a module, fragment or portion of a code comprising one or more executable instructions for implementing the steps of a specific logical function or process, and that the scope of the preferred embodiments of the present invention includes alternative implementations in which the functions may not be performed in the order described or discussed, including performing the functions in a substantially simultaneous manner or in the reverse order according to the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention belong.

[0067] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention. The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages and disadvantages 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: Integrate the IMU data based on manifold pre-integration to obtain the IMU pose, and obtain a first covariance matrix corresponding to the IMU pose; Integrate the code disk data to obtain a first code disk posture, and obtain a second covariance matrix corresponding to the first code disk posture; The first covariance matrix, the second covariance matrix, the IMU posture and the first code disk posture are fused based on the error Kalman update process to obtain a first displacement error; Obtaining a second code disk posture based on the first code disk posture in combination with the corresponding first displacement error optimization; The second code disk position is integrated based on the group space closed-form integration to obtain the optimized current cumulative displacement.

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

3. The method for optimizing displacement based on IMU data and code disk data fusion according to claim 2, characterized in that: The step of integrating the second code disk posture based on the group space closed-form integration to obtain the optimized current cumulative displacement specifically includes: Based on the second code disk posture of the current frame, the corresponding second code disk displacement of the current frame is obtained; Based on the IMU posture of the current frame, obtain the corresponding rotation increment of the current frame; Based on the rotation increment of the current frame, the rotation matrix of the current frame is obtained through the rotation matrix conversion formula; Calculate the attitude change value of the second code disk in the current frame 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 posture change value of the current frame and the cumulative displacement of the previous frame is taken as the current cumulative displacement.

4. The method for optimizing displacement based on IMU data and code disk data fusion according to claim 3, characterized in that: The step of integrating the IMU data based on the manifold pre-integration to obtain the IMU pose specifically includes: Obtain 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 current frame of IMU data; Pre-integrate the angular velocity measurement value, angular velocity deviation value and angular velocity measurement noise to obtain the IMU attitude of the current frame; Pre-integrate based on IMU attitude, acceleration measurement value, acceleration deviation value and acceleration measurement noise to obtain the IMU velocity of the current frame; Pre-integrate based on IMU velocity, acceleration measurement value, acceleration deviation value 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, the angular velocity deviation value, and the angular velocity measurement noise is: Among them, the formula for pre-integration based on IMU attitude, acceleration measurement value, acceleration deviation value and acceleration measurement noise is: Among them, the formula for pre-integration based on IMU velocity, acceleration measurement value, acceleration deviation value and acceleration measurement noise is: Among them, ΔR ij IMU attitude calculated by pre-integration; is the angular velocity measurement value 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 change; Δv ij is the IMU velocity calculated by pre-integration; Δp ij is the IMU displacement calculated by pre-integration; the IMU posture includes IMU attitude, IMU velocity and IMU displacement; is the measured value of the acceleration of the current frame, is the acceleration deviation of the current frame, is the acceleration measurement noise of the current frame.

5. The method for optimizing displacement based on fusion of IMU data and encoder data according to claim 4, characterized in that: The integration of the code disk data to obtain the first code disk posture is implemented by adopting a group closed integration method, and the integration process specifically includes: Based on the IMU posture of the current frame, obtain the corresponding rotation increment of the current frame; Based on the rotation increment of the current frame, the rotation matrix of the current frame is obtained through the rotation matrix conversion formula; Obtaining a first code disk displacement of a current frame based on the code disk data of a current frame; Calculate the first code disk attitude change value based on the rotation matrix of the current frame, the first code disk displacement of the current frame and the Jacobian matrix; Based on the Lie group right multiplication integral increment formula, the first code disk posture of the previous frame and the code disk posture change value of the current frame are used to calculate the first code disk posture of the current frame.

6. The method for optimizing displacement based on the fusion of IMU data and code disk data according to claim 5, 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 a first displacement error, specifically including: Calculate the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU posture of the current frame and the second covariance matrix corresponding to the first code disk posture of the current frame; Calculate the pose difference of the current frame 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 fusion covariance matrix of the previous frame, the first displacement error of the previous frame, the first covariance matrix corresponding to the IMU posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame and the posture difference of the current frame.

7. The method for optimizing displacement based on fusion of IMU data and encoder data according to claim 6, characterized in that: The method of calculating the current frame cumulative error covariance matrix based on the first covariance matrix corresponding to the current frame IMU posture and the second covariance matrix corresponding to the current frame first code disk posture specifically includes: Get the cumulative error covariance matrix of the previous frame; Calculate the transposed matrix of the cumulative error covariance matrix of the previous frame; Calculate the product of the cumulative error covariance matrix of the previous frame and its transposed matrix as the fourth covariance matrix; Calculate the sum of the first covariance matrix corresponding to the IMU posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame, and the cumulative error covariance matrix of the previous frame 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; Calculate 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; The initial frame of the cumulative error covariance matrix is ​​a unit matrix.

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 current frame pose difference based on the current frame IMU pose, the current frame first code disk pose and the previous frame first displacement error specifically includes: Add the first encoder posture of the current frame to the first displacement error of the previous frame to obtain the calibration posture value; calculate the difference between the IMU posture of the current frame and the calibration posture value as the posture difference of the current frame; The initial frame of the first displacement error is a zero vector.

9. The method for optimizing displacement based on the fusion of IMU data and encoder data according to claim 8, characterized in that: 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 posture of the current frame, the second covariance matrix corresponding to the first code disk posture of the current frame, and the posture difference of the current frame, specifically including: 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.

10. The method for optimizing displacement based on fusion of IMU data and encoder data according to claim 9, characterized in that: Obtaining a second covariance matrix corresponding to the first code disk posture, specifically comprising: configuring the second covariance matrix as a matrix of 3 rows and 3 columns, and only the elements on the diagonal of the second covariance matrix have values, and the elements in other positions are all 0; wherein 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 based on the code disk data and various configuration errors.

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

12. A chip having a computer program stored therein, characterized in that: When the computer program stored in the chip is executed by the processor, the method for optimizing displacement based on the fusion of IMU data and code disk data as described in any one of claims 1 to 11 is executed.

Citation Information

Patent Citations

  • Stroke calibration method of mobile robot

    CN106393104A

  • Pose estimation method of mobile robot and computer readable storage medium

    CN112815939A

  • Attitude measurement method based on IMU and robot body parameter fusion

    CN114216456A