Displacement error calculation method based on IMU and coded disc, chip and robot
Through the combination of manifold pre-integration and group space closed integral combined with error Kalman update, the large displacement error problem caused by robot wheel slip is solved, the position estimation accuracy is improved, and more accurate displacement error calculation is achieved.
Patent Information
- Application Number
- CN202310562720.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-18
- Publication Date
- 2024-11-22
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In the prior art, when the robot wheel slips, there is a large error between the displacement acquired by the code disc and the actual displacement, making it difficult to accurately determine the displacement error, affecting the position estimation accuracy.
The displacement error calculation method based on IMU and code disk is adopted, and the IMU displacement and group space closed integral are obtained through manifold preintegration to obtain code disk displacement. Combined with the error Kalman update process, the covariance matrix and displacement data are fused to calculate the displacement error.
Effectively reduce noise propagation, improve position estimation accuracy, and accurately linearize displacement error calculation, solving the problem of large displacement error caused by robot wheel slippage.
Smart Images

Figure CN118999535A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of displacement calculation, and specifically to a displacement error calculation method, chip and robot based on IMU and code disk. 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, and the IMU data is supplemented and corrected based on other sensor data, thereby improving the accuracy of posture estimation. However, how to fuse the two types of sensor data has always been a difficult point in the industry, especially when the robot's wheels slip severely, there is a large error between the displacement obtained by the robot's code disk and the actual displacement of the robot. In the current prior art, the displacement obtained by a sensor is used as a standard value to calculate the difference between the code disk displacement as the displacement error, but the calculation accuracy of this displacement error is poor, and the displacement error cannot be accurately determined. Summary of the invention
[0003] This application provides a displacement error calculation method, chip and robot based on IMU and code disk. The specific technical solution is as follows:
[0004] A displacement error calculation method based on an IMU and a code disk comprises: integrating IMU data based on manifold pre-integration to obtain IMU displacement, and obtaining a first covariance matrix corresponding to the IMU displacement; integrating code disk data based on group space closed-form integration to obtain code disk displacement, and obtaining a second covariance matrix corresponding to the code disk displacement; fusing the first covariance matrix, the second covariance matrix, the IMU displacement and the code disk displacement based on an error Kalman update process to obtain the displacement error.
[0005] Furthermore, the method of fusing the first covariance matrix, the second covariance matrix, the IMU displacement and the code disk displacement based on the error Kalman update process to obtain the displacement error specifically includes: calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU displacement of the current frame and the second covariance matrix corresponding to the code disk displacement of the current frame; calculating the current frame posture difference based on the IMU displacement of the current frame, the code disk displacement of the current frame and the displacement error of the previous frame; calculating the displacement error of the current frame based on the cumulative error covariance matrix of the previous frame, the displacement error of the previous frame, the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement of the current frame and the posture difference of the current frame.
[0006] Furthermore, the calculation of the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU displacement of the current frame and the second covariance matrix corresponding to the code disk displacement 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 third covariance matrix; calculating the sum of the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement of the current frame and the cumulative error covariance matrix of the previous frame as the fourth covariance matrix; taking the third covariance matrix as the dividend, the fourth covariance matrix as the divisor, and the quotient of the third covariance matrix and the fourth covariance matrix as the fifth covariance matrix; calculating the difference between the cumulative error covariance matrix of the previous frame and the fifth 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.
[0007] Furthermore, the calculation of the current frame pose difference based on the current frame IMU displacement, the current frame code disk displacement and the previous frame displacement error specifically includes: adding the current frame code disk displacement and the previous frame displacement error to obtain a calibration pose value; calculating the difference between the current frame IMU displacement and the calibration pose value as the current frame pose difference; wherein the initial frame of the displacement error is a zero vector.
[0008] Furthermore, the method of calculating the displacement error of the current frame based on the cumulative error covariance matrix of the previous frame, the displacement error of the previous frame, the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement of the current frame and the posture difference of the current frame specifically includes: calculating the product of the cumulative error covariance matrix of the previous frame and the posture difference of the current frame as the sixth covariance matrix; taking the sixth covariance matrix as the dividend, the fourth covariance matrix as the divisor, and the quotient of the sixth covariance matrix and the fourth covariance matrix as the seventh covariance matrix; calculating the sum of the displacement error of the previous frame and the seventh covariance matrix as the displacement error of the current frame.
[0009] Further, the integrating the IMU data based on the manifold pre-integration to obtain the IMU displacement specifically includes: obtaining the angular velocity measurement value, the angular velocity deviation value and the angular velocity measurement noise in the IMU data; performing pre-integration based on the angular velocity measurement value, the angular velocity deviation value and the angular velocity measurement noise to obtain the IMU displacement; wherein 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, ΔR ij is the IMU displacement calculated by pre-integration; is the angular velocity measurement value of the kth frame; is the angular velocity deviation value of the kth frame; is the angular velocity measurement noise of the kth frame; Δt is the time-to-time variation.
[0010] Further, the integration of the code disk data based on the group space closed-form integration to obtain the code disk displacement specifically includes: obtaining the rotation increment in the IMU displacement obtained by the manifold pre-integration; obtaining the rotation matrix based on the rotation increment through the rotation matrix conversion formula; obtaining the moving distance based on the code disk data; calculating the first code disk displacement change value based on the rotation matrix, the moving distance and the left Jacobian matrix; calculating the current frame code disk displacement change value based on the first code disk displacement change value combined with the previous frame code disk displacement change value; calculating the current frame code disk displacement based on the previous frame code disk displacement combined with the current frame code disk displacement change value; wherein, the formula for calculating the first code disk displacement change value based on the rotation matrix, the moving distance and the left Jacobian matrix is: The formula for calculating the displacement change value of the current frame of the code disk based on the first code disk displacement change value combined with the displacement change value of the previous frame of the code disk is: ΔT j+1= ΔTΔT j ; Wherein, R refers to the rotation matrix; J refers to the left Jacobian matrix; Δp refers to the moving distance; ΔT refers to the displacement change value of the first code disk; ΔT j Refers to the displacement change value of the code disk in the previous frame; the ΔT j+1 Refers to the displacement change value of the encoder in the current frame.
[0011] Furthermore, the obtaining of the second covariance matrix corresponding to the code disk displacement 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 rest are 0; wherein the values of the elements on the diagonal of the second covariance matrix are obtained by calculating the product of the moving distance obtained by the code disk data and various configuration errors.
[0012] Furthermore, the configuration error includes: measurement error, drift error and slip error; the values of the elements on the diagonal of the second covariance matrix are determined based on the product of the moving distance obtained by the code disk data and various configuration errors, specifically including: 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; the first row diagonal elements of the second covariance matrix are equal to the product of the first error and the moving distance; taking the sum of the drift error and the slip error as the second error; the second row diagonal elements and the third row diagonal elements of the second covariance matrix are both equal to the product of the second error and the moving distance.
[0013] 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 displacement error calculation method based on the IMU and the code disk as described above is executed.
[0014] The present application also discloses a robot, which is equipped with an IMU sensor, a code disk, a processor and the chip as described above; wherein the IMU sensor is used to obtain IMU data; the code disk is used to obtain code disk data; the processor is used to run the chip, and when the chip is run by the processor, it executes the displacement error calculation method based on the IMU and code disk as described above based on the IMU data obtained by the IMU sensor and the code disk data obtained by the code disk.
[0015] The displacement error calculation method, chip and robot based on IMU and code disk described in the present application obtain IMU data by means of manifold pre-integration, effectively suppress noise propagation, reduce the influence of inertial zero drift on IMU positioning accuracy, integrate the code disk data by means of group space closed-form integration, effectively reduce the influence of ground unevenness and ground undulation on the pose estimation results, improve the pose estimation accuracy, filter the errors in IMU displacement and code disk displacement based on the error Kalman filter update process, linearize more accurately, and fuse IMU displacement and code disk displacement to improve the displacement error calculation accuracy, thus solving the problem of large displacement error obtained by the code disk due to robot wheel slippage. BRIEF DESCRIPTION OF THE DRAWINGS
[0016] Figure 1 A flow chart of a method for calculating displacement error based on an IMU and a code disk according to an embodiment of the present application. DETAILED DESCRIPTION
[0017] 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.
[0018] In order to achieve the fusion of code disk data and IMU data to improve the accuracy of pose estimation, an embodiment of the present application provides a displacement error calculation method based on IMU data and code disk data, which is used to calculate the pose estimation error so as to perform pose correction according to the pose estimation error. Specifically, Figure 1 As shown, the displacement error calculation method based on IMU data and code disk data specifically includes:
[0019] Based on manifold pre-integration, IMU data is pre-integrated to obtain IMU displacement, and a first covariance matrix corresponding to the IMU displacement 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 a series of processes within an 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; the first covariance matrix is calculated based on the covariance matrix transfer formula, and the covariance matrix transfer is used for the back-end optimization of the iteration of the covariance matrix corresponding to the IMU displacement.
[0020] Based on the group space closed-form integration, the code disk data is integrated to obtain the code disk displacement, and the second covariance matrix corresponding to the code disk displacement 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 pose estimation results, thereby improving the pose estimation accuracy.
[0021] Based on the error Kalman update process, the first covariance matrix, the second covariance matrix, the IMU displacement and the code disk displacement are fused and calculated to obtain the displacement error; wherein, the error Kalman update process refers to the update process of the state error Kalman filter (ESFK, Error-State Kalman Filter), the state error Kalman filter is a variant application of the Kalman filter, and is a nonlinear filter for time-varying systems. In this application, the errors in the IMU displacement and the code disk displacement are filtered based on the state error Kalman filter, which can be linearized more accurately.
[0022] As one of the implementation methods, the sampling time of the code disk data is the same as the sampling time of the IMU data, that is, the code disk data is sampled at the same time as each IMU data is sampled. The sampling time of the code disk data is adjusted to be consistent with the sampling time of the IMU data, so that the time nodes of the data can be matched, and more accurate simultaneous displacement correction can be achieved.
[0023] As one of the implementation modes, the method of fusing and calculating the first covariance matrix, the second covariance matrix, the IMU displacement and the code disk displacement based on the error Kalman update process to obtain the displacement error specifically includes: calculating the cumulative error covariance matrix of the current frame based on the first covariance matrix corresponding to the IMU displacement of the current frame and the second covariance matrix corresponding to the code disk displacement of the current frame; calculating the posture difference of the current frame based on the IMU displacement of the current frame, the code disk displacement of the current frame and the displacement error of the previous frame; recording the calculation of the posture difference as the change of the observation data in the error Kalman update process, calculating the attitude difference between the IMU displacement of the current frame and the code disk displacement of the current frame after eliminating the influence of the displacement error of the previous frame, so that the posture difference can be used to update the displacement error.
[0024] The displacement error of the current frame is calculated based on the cumulative error covariance matrix of the previous frame, the displacement error of the previous frame, the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement of the current frame, and the pose difference of the current frame.
[0025] As one implementation method, the first covariance matrix corresponding to the current frame IMU displacement and the second covariance matrix corresponding to the current frame code disk displacement are used to calculate the current frame cumulative error covariance matrix, specifically including:
[0026] 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.
[0027] Calculate the product of the cumulative error covariance matrix of the previous frame and its transposed matrix as the third covariance matrix; calculate the sum of the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement 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 may 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.
[0028] 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.
[0029] As one of the implementation modes, the current frame pose difference is calculated based on the current frame IMU displacement, the current frame code disk displacement and the previous frame displacement error, specifically including: adding the current frame code disk displacement to the previous frame displacement error to obtain the calibration pose value; calculating the difference between the current frame IMU displacement and the calibration pose value as the current frame pose difference; wherein the initial frame of the displacement error is a zero vector, that is, when the calculation of the first frame pose difference is started, the calibration pose value is equal to the first frame code disk displacement, and accordingly, the first frame pose difference is equal to the difference between the first frame IMU displacement and the first frame code disk displacement. In this implementation mode, the initial frame of the displacement error is limited to a zero vector, so that the initial frame of the pose difference is equal to the difference between the initial IMU displacement and the initial code disk displacement, the code disk displacement is corrected based on the displacement error to obtain the calibration pose value as an intermediate calculation value, and the difference between the code disk displacement and the IMU displacement is reduced based on the calibration pose value, so that the pose difference can more accurately feedback the error between the IMU displacement and the code disk displacement.
[0030] As one of the implementation modes, the displacement error of the current frame is calculated based on the cumulative error covariance matrix of the previous frame, the displacement error of the previous frame, the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement of the current frame and the posture difference of the current frame, specifically including: calculating the product of the cumulative error covariance matrix of the previous frame and the posture difference of the current frame as the sixth covariance matrix; taking the sixth covariance matrix as the dividend, the fourth covariance matrix as the divisor, and the quotient of the sixth covariance matrix and the fourth covariance matrix as the seventh covariance matrix; calculating the sum of the displacement error of the previous frame and the seventh covariance matrix as the displacement error of the current frame.
[0031] As one implementation method, the integration of IMU data based on manifold pre-integration to obtain IMU displacement specifically includes: obtaining angular velocity measurement values in IMU data Angular velocity deviation and angular velocity measurement noise Based on angular velocity measurements Angular velocity deviation and angular velocity measurement noise Perform pre-integration to obtain the IMU displacement.
[0032] Specifically, the angular velocity measurement value Angular velocity deviation and angular velocity measurement noise For pre-integration, the calculation formula used is:
[0033]
[0034] Among them, ΔR ij is the IMU displacement calculated by pre-integration; is the angular velocity measurement value of the kth frame; is the angular velocity deviation value of the kth frame; is the angular velocity measurement noise of the kth frame; Δt is the change at the moment. This implementation method performs pre-integration processing on the IMU data on the manifold, and by taking the noise into consideration, effectively suppresses the noise propagation and the error growth during operation, reduces the impact of inertial zero drift on the IMU positioning accuracy, and improves the IMU positioning accuracy. At the same time, Lie algebra is used for integration and optimization to reduce the overall calculation amount.
[0035] As one implementation manner, the method for obtaining the first covariance matrix corresponding to the IMU posture specifically includes:
[0036] 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:
[0037] in, The A j-1 T A j-1 The transposed matrix of
[0038] in, B j-1 T Refers to B j-1 The transposed matrix of
[0039] 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.
[0040] As one of the implementation modes, the code disk data is integrated based on the closed-form integration of the group space to obtain the code disk displacement, specifically including: obtaining the rotation increment in the IMU displacement obtained by manifold pre-integration; obtaining the rotation matrix based on the rotation increment through the rotation matrix conversion formula; obtaining the moving distance based on the code disk data; calculating the first code disk displacement change value based on the rotation matrix, the moving distance and the left Jacobian matrix; calculating the current frame code disk displacement change value based on the first code disk displacement change value combined with the previous frame code disk displacement change value; calculating the current frame code disk displacement based on the previous frame code disk displacement combined with the current frame code disk displacement change value; wherein, the rotation increment refers to the increase in the rotation angle compared with the previous frame of IMU data with the coordinate axis as the rotation axis; the rotation matrix conversion formula is a conventional calculation formula in the field and is not described here; the moving distance obtained based on the code disk data refers to obtaining the translation increment as the moving distance through code disk detection, which can be but not limited to being obtained based on the translation speed combined with the translation time integral; the left Jacobian matrix is
[0041] The formula for calculating the displacement change value of the first code disk based on the rotation matrix, the moving distance and the left Jacobian matrix is:
[0042] The formula for calculating the displacement change value of the current frame of the code disk based on the first code disk displacement change value combined with the displacement change value of the previous frame of the code disk is: ΔT j+1= ΔTΔT j
[0043] Wherein, R refers to the rotation matrix; J refers to the left Jacobian matrix; Δp refers to the moving distance; ΔT refers to the displacement change value of the first code disk; ΔT j Refers to the displacement change value of the code disk in the previous frame; the ΔT j+1 Refers to the displacement change value of the code disk in the current frame. This embodiment calculates the displacement of the code disk by adopting a group closed integral method, thereby eliminating the influence of the code disk data obtained under the condition of uneven or undulating ground on the calculation of the displacement of the code disk, and improving the attitude accuracy.
[0044] As one of the implementation modes, the obtaining of the second covariance matrix corresponding to the code disk displacement specifically includes: the second covariance matrix is configured as a matrix of 3 rows * 3 columns, and only the elements on the diagonal of the second covariance matrix have values, and the rest are 0; wherein, the values of the elements on the diagonal of the second covariance matrix are obtained by calculating the product of the moving distance obtained by the code disk data and various configuration errors.
[0045] Specifically, as one of the embodiments, the configuration error includes: 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 moving distance obtained by the code disk data and various configuration errors, specifically including: 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; the first row diagonal element of the second covariance matrix is equal to the product of the first error and the moving distance; taking the sum of the drift error and the slip error as the second error; the second row diagonal element and the third row diagonal element of the second covariance matrix are both equal to the product of the second error and the moving distance. 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. The measurement error is a value set for the error that may be caused by inaccurate measurement during the code disk measurement data process; the drift error is a value set for the error that may be caused by drift during the code disk measurement data process; the slip error is a value set for the error that may be caused by wheel slip during the code disk measurement data process.
[0046] In some embodiments of the present application, 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.
[0047] The present application also provides a chip having a computer program stored therein. When the computer program is executed by a processor, the displacement error calculation method based on the IMU and the code disk as described in any of the aforementioned embodiments is executed to achieve fusion of the IMU data and the code disk data and obtain the posture error of the two.
[0048] The present application also provides a robot, which is equipped with an IMU sensor, a code disk, a processor and a chip inside the robot; the IMU sensor is used to obtain IMU data; the code disk is used to obtain code disk data; a computer program is stored inside the chip, and when the computer program inside the chip is executed by the processor, the displacement error calculation method based on the IMU and the code disk as described in any of the aforementioned embodiments is executed based on the IMU data obtained by the IMU sensor and the code disk data obtained by the code disk.
[0049] Obviously, the above-mentioned embodiments are only a part of the embodiments of the present invention, rather than all the embodiments, and the technical solutions between the various embodiments can be combined with each other. In addition, 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, and therefore cannot be understood as limiting the present invention.
[0050] A person skilled in the art can understand that all or part of the steps in the above-mentioned embodiments can be completed by hardware related to program instructions, and the corresponding program can be stored in a computer-readable storage medium (such as ROM, RAM, magnetic disk or optical disk, etc., which can store program codes).
[0051] Although the embodiments of the present invention have been shown and described, it is understood by those skilled in the art that various changes, modifications, substitutions and variations may be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is limited by the appended claims and their equivalents. The above description is only the preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various changes and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. A displacement error calculation method based on IMU and code disc, characterized in that: The displacement error calculation method comprises: Integrate the IMU data based on manifold pre-integration to obtain the IMU displacement, and obtain a first covariance matrix corresponding to the IMU displacement; Integrate the code disk data based on the group space closed-form integral to obtain the code disk displacement, and obtain a second covariance matrix corresponding to the code disk displacement; The first covariance matrix, the second covariance matrix, the IMU displacement and the code disk displacement are fused based on the error Kalman update process to obtain the displacement error.
2. The displacement error calculation method based on IMU and code disc according to claim 1, characterized in that: The method for fusing the first covariance matrix, the second covariance matrix, the IMU displacement and the code disk displacement based on the error Kalman update process to obtain the displacement error specifically includes: Calculate the current frame cumulative error covariance matrix based on the first covariance matrix corresponding to the current frame IMU displacement and the second covariance matrix corresponding to the current frame code disk displacement; Calculate the current frame pose difference based on the current frame IMU displacement, the current frame encoder displacement and the previous frame displacement error; The displacement error of the current frame is calculated based on the cumulative error covariance matrix of the previous frame, the displacement error of the previous frame, the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the code disk displacement of the current frame, and the pose difference of the current frame.
3. The displacement error calculation method based on IMU and code disc according to claim 2 is 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 displacement and the second covariance matrix corresponding to the current frame code disk displacement 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 third covariance matrix; Calculate the sum of the first covariance matrix corresponding to the current frame IMU displacement, the second covariance matrix corresponding to the current frame code disk displacement, and the cumulative error covariance matrix of the previous frame as the fourth covariance matrix; The third covariance matrix is used as the dividend, the fourth covariance matrix is used as the divisor, and the quotient of the third covariance matrix and the fourth covariance matrix is used as the fifth covariance matrix; Calculate the difference between the cumulative error covariance matrix of the previous frame and the fifth 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.
4. The displacement error calculation method based on IMU and code disc according to claim 3 is characterized in that: The calculation of the current frame pose difference based on the current frame IMU displacement, the current frame code disk displacement and the previous frame displacement error specifically includes: Add the displacement of the current frame of the code disc to the displacement error of the previous frame to obtain the calibration pose value; Calculate the difference between the current frame IMU displacement and the calibration pose value as the current frame pose difference; The initial frame of the displacement error is a zero vector.
5. The displacement error calculation method based on IMU and code data according to claim 4 is characterized in that: The calculation of the displacement error of the current frame based on the cumulative error covariance matrix of the previous frame, the displacement error of the previous frame, the first covariance matrix corresponding to the IMU displacement of the current frame, the second covariance matrix corresponding to the encoder displacement of the current frame and the pose difference of the current frame specifically includes: Calculate the product of the cumulative error covariance matrix of the previous frame and the pose difference of the current frame as the sixth covariance matrix; The sixth covariance matrix is used as the dividend, the fourth covariance matrix is used as the divisor, and the quotient of the sixth covariance matrix and the fourth covariance matrix is used as the seventh covariance matrix; The sum of the displacement error of the previous frame and the seventh covariance matrix is calculated as the displacement error of the current frame.
6. The displacement error calculation method based on IMU and code disc according to claim 5, characterized in that: The integrating the IMU data based on the manifold pre-integration to obtain the IMU displacement specifically includes: Obtain angular velocity measurement values, angular velocity deviation values, and angular velocity measurement noise from IMU data; Pre-integrate based on the angular velocity measurement value, the angular velocity deviation value and the angular velocity measurement noise to obtain the IMU displacement; 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, ΔR ij is the IMU displacement calculated by pre-integration; is the angular velocity measurement value of the kth frame; is the angular velocity deviation value of the kth frame; is the angular velocity measurement noise of the kth frame; Δt is the time-to-time variation.
7. The displacement error calculation method based on IMU and code disc according to claim 6, characterized in that: The step of integrating the code disk data based on the group space closed-form integral to obtain the code disk displacement specifically includes: Get the rotation increment in the IMU displacement obtained by manifold pre-integration; Obtain the rotation matrix through the rotation matrix conversion formula based on the rotation increment; Obtain the moving distance based on the code disk data; Calculate the displacement change value of the first code disk based on the rotation matrix, the moving distance and the left Jacobian matrix; Calculate the displacement change value of the current frame of the code disk based on the first code disk displacement change value combined with the displacement change value of the previous frame of the code disk; Calculate the displacement of the current code disk frame based on the displacement of the previous code disk frame and the displacement change value of the current code disk frame; The formula for calculating the displacement change value of the first code disk based on the rotation matrix, the moving distance and the left Jacobian matrix is: The formula for calculating the current frame code disk displacement change value based on the first code disk displacement change value combined with the previous frame code disk displacement change value is: ΔT j+1= ΔTΔT j Wherein, R refers to the rotation matrix; J refers to the left Jacobian matrix; Δp refers to the moving distance; ΔT refers to the displacement change value of the first code disk; ΔT j Refers to the displacement change value of the code disk in the previous frame; the ΔT j+1 Refers to the displacement change value of the encoder in the current frame.
8. The displacement error calculation method based on IMU and code disc according to claim 7, characterized in that: The method of obtaining the second covariance matrix corresponding to the code disk displacement 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 rest are 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.
9. The displacement error calculation method based on IMU and code disc according to claim 8, characterized in that: The configuration error includes: measurement error, drift error and slip error; the value of the elements on the diagonal of the second covariance matrix is determined based on the product of the moving distance obtained by the code disk data and various configuration errors, specifically including: Obtain the moving distance based on the code disk data; The sum of the measurement error and the slip error is taken as the first error; The first row diagonal element of the second covariance matrix is equal to the product of the first error and the moving distance; The sum of the drift error and the slip error is taken as the second error; The second diagonal element of the second covariance matrix and the third diagonal element of the second covariance matrix are both equal to the product of the second error and the moving distance.
10. 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 displacement error calculation method based on the IMU and the code disk as described in claims 1 to 9 is executed.
11. A robot, characterized in that: An IMU sensor, a code disk, a processor and a chip as claimed in claim 10 are provided; wherein the IMU sensor is used to obtain IMU data; the code disk is used to obtain code disk data; the processor is used to run the chip, and when the chip is run by the processor, the displacement error calculation method based on the IMU and the code disk as claimed in claims 1 to 9 is performed based on the IMU data obtained by the IMU sensor and based on the code disk data obtained by the code disk.
Citation Information
Patent Citations
Stroke calibration method of mobile robot
CN106393104A
Four-wheel independent drive based IMU (inertial measurement unit) combined mileage calculation method
CN108036797A
Method for realizing centimeter-level accurate positioning based on tight coupling of encoder and inertial equipment
CN111207743A
Indoor robot positioning algorithm based on Kalman filtering
CN112584306A
SLAM method, SLAM system and intelligent robot
CN113379911A
