Method for correcting yaw angle based on encoder and IMU

Abstract

The present invention provides a method for correcting a yaw angle based on an encoder and an IMU, and relates to the technical field of laser radar 3D point cloud attitude calculation. In a stationary state, an IMU is used to perform static alignment calibration, and acceleration is obtained. The pitch angle and roll angle in the stationary state are calculated based on the acceleration, and the encoder rotation angle in the stationary state is read to obtain the encoder rotation angle, roll angle, pitch angle, and second yaw angle in the current state. A rotation matrix corresponding to the first yaw angle is calculated based on the pitch angle, roll angle, encoder rotation angle in the stationary state, and the rotation matrices corresponding to the encoder rotation angle, roll angle, and pitch angle in the current state. The first yaw angle is calculated based on the rotation matrix, and the first yaw angle is corrected using the second yaw angle to obtain the corrected yaw angle. The problem of being unable to obtain an accurate yaw angle indoors or in the absence of GNSS is solved. The present invention is applicable to a system that uses an encoder to replace GNSS to obtain device attitude information.

Description

Technical Field

[0001] The present invention relates to the technical field of laser radar 3D point cloud attitude calculation technology, and in particular to a method for correcting yaw angle based on an encoder and an IMU. Background Art

[0002] Currently, rapid, high-precision 3D big data modeling requires the use of 3D laser point clouds combined with device position and attitude. A common approach is to combine GNSS positioning information with an IMU and a rotation matrix to implement a cascaded solution to obtain position and attitude information. This is then combined with LiDAR 3D point cloud data to achieve 3D spatial modeling. For example, the research on coordinate solution methods for shipborne laser scanning point cloud data describes the calculation of geodetic 3D coordinate values ​​for shipborne laser scanning point cloud data.

[0003] Three rotation angles around different coordinate axes, such as the rotation angle around the Z axis Recorded as yaw angle, the rotation angle around the Y axis Recorded as pitch, the rotation angle around the X axis It is recorded as the roll angle (roll).

[0004] The rotation matrix around the Z axis is recorded as: , the rotation matrix around the Y axis is recorded as: , the rotation matrix around the X axis is recorded as: .

[0005] Then the total rotation matrix in the order of ZYX is .

[0006] Indoors or without GNSS, it is impossible to obtain accurate yaw angles, making it impossible to determine accurate position and attitude information. Summary of the Invention

[0007] The technical problem solved by the present invention is as follows: The present invention provides a method for correcting the yaw angle based on an encoder and an IMU, which solves the existing problem that an accurate yaw angle cannot be obtained indoors or without GNSS.

[0008] The present invention solves the above technical problems by adopting a technical solution: a method for correcting yaw angle based on an encoder and an IMU, which is applied to a system that uses an encoder to replace GNSS to obtain device attitude information, comprising the following steps:

[0009] S1. In a stationary state, use the IMU to perform static alignment calibration and obtain acceleration. Based on the acceleration, calculate the pitch and roll angles in the stationary state, and read the encoder rotation angle in the stationary state.

[0010] S2. Obtain the encoder rotation angle and the IMU attitude angle in the current state, where the IMU attitude angle includes the roll angle, pitch angle and second yaw angle in the current state;

[0011] S3, calculating a rotation matrix corresponding to the first yaw angle based on the rotation matrices corresponding to the pitch angle in the stationary state, the roll angle in the stationary state, the encoder rotation angle in the stationary state, the encoder rotation angle in the current state, the roll angle in the current state, and the pitch angle in the current state;

[0012] S4. Calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle;

[0013] S5. Correct the first yaw angle using the second yaw angle in the attitude angle of the IMU to obtain a corrected yaw angle.

[0014] Furthermore, in S1, the formula for calculating the pitch angle in a stationary state based on acceleration is: ,in, represents the pitch angle, represents acceleration, Represents the component of acceleration on the X-axis.

[0015] Furthermore, in S1, the formula for calculating the roll angle in a stationary state based on the acceleration is: ,in, represents the roll angle, represents acceleration, Indicates the acceleration The component on the axis, Represents the component of acceleration along the Z axis.

[0016] Furthermore, in S3, the formula used to calculate the rotation matrix corresponding to the first yaw angle is: ,in, represents the first yaw angle, represents the rotation matrix corresponding to the first yaw angle, , Indicates the roll angle at rest, Indicates the pitch angle at rest The rotation matrix of the axis, , Indicates the pitch angle when the editor is stationary. Indicates the pitch angle at rest The rotation matrix of the axis, , Indicates the encoder rotation angle in a stationary state, Indicates the encoder rotation angle in a stationary state The rotation matrix of the axis, , Indicates the roll angle in the current state, Indicates the roll angle in the current state The rotation matrix of the axis, , Indicates the pitch angle in the current state. Indicates the pitch angle in the current state The rotation matrix of the axis, , Indicates the encoder rotation angle in the current state, Indicates the encoder rotation angle in the current state. The rotation matrix about the axis.

[0017] Furthermore, in S4, the formula used to calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle is: ,in, represents the first yaw angle, Indicates the value corresponding to the second row and first column of the rotation matrix corresponding to the first yaw angle, Indicates the value corresponding to the first row and first column in the rotation matrix corresponding to the first yaw angle, express The corresponding coordinates on the plane ( , ) angle.

[0018] Furthermore, in S5, the formula for correcting the first yaw angle using the second yaw angle in the IMU attitude angle is: ,in, represents the corrected yaw angle, represents the first yaw angle, represents the second yaw angle, Represents the weight parameter, ranging from 0 to 1.

[0019] Beneficial effects of the present invention: The present invention provides a method for correcting yaw angle based on encoder and IMU, which, in a stationary state, uses IMU for static alignment calibration and obtains acceleration, calculates pitch angle and roll angle in the stationary state based on acceleration, and reads encoder rotation angle in the stationary state to obtain encoder rotation angle in the current state and attitude angle of IMU, wherein the attitude angle of IMU includes roll angle, pitch angle and second yaw angle in the current state, calculates rotation matrix corresponding to the first yaw angle based on rotation matrices corresponding to pitch angle in the stationary state, roll angle in the stationary state, encoder rotation angle in the stationary state, encoder rotation angle in the current state, roll angle in the current state and pitch angle in the current state, calculates first yaw angle based on rotation matrix corresponding to the first yaw angle, corrects the first yaw angle using the second yaw angle in the attitude angle of IMU to obtain the corrected yaw angle, and solves the existing problem that accurate yaw angle cannot be obtained indoors or without GNSS. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 The present invention provides a flow chart of a method for correcting yaw angle based on an encoder and an IMU. DETAILED DESCRIPTION

[0021] The present invention aims to solve the problem that accurate yaw angle cannot be obtained indoors or without GNSS. It provides a method for correcting yaw angle based on encoder and IMU, which is applied to a system that uses encoder to replace GNSS to obtain device attitude information. The method is as follows: Figure 1 As shown, the following steps are included:

[0022] S1. In a stationary state, use the IMU to perform static alignment calibration and obtain acceleration. Based on the acceleration, calculate the pitch and roll angles in the stationary state, and read the encoder rotation angle in the stationary state.

[0023] Specifically, the formula for calculating the pitch angle in a stationary state based on acceleration is: ,in, represents the pitch angle, represents acceleration, Indicates the component of acceleration on the X-axis. The formula for calculating the roll angle at rest based on acceleration is: ,in, represents the roll angle, represents acceleration, Indicates the acceleration The component on the axis, Represents the component of acceleration along the Z axis.

[0024] S2. Obtain the encoder rotation angle and the IMU attitude angle in the current state, where the IMU attitude angle includes the roll angle, pitch angle, and second yaw angle in the current state.

[0025] Specifically, the roll angle, pitch angle, and second yaw angle in the current state are obtained through position and attitude recursion through the IMU.

[0026] S3. Calculate a rotation matrix corresponding to the first yaw angle based on the rotation matrices corresponding to the pitch angle in the stationary state, the roll angle in the stationary state, the encoder rotation angle in the stationary state, the encoder rotation angle in the current state, the roll angle in the current state, and the pitch angle in the current state.

[0027] Specifically, the formula used to calculate the rotation matrix corresponding to the first yaw angle is: ,in, represents the first yaw angle, represents the rotation matrix corresponding to the first yaw angle, , Indicates the roll angle at rest, Indicates the pitch angle at rest The rotation matrix of the axis, , Indicates the pitch angle when the editor is stationary. Indicates the pitch angle at rest The rotation matrix of the axis, , Indicates the encoder rotation angle in a stationary state, Indicates the encoder rotation angle in a stationary state The rotation matrix of the axis, , Indicates the roll angle in the current state, Indicates the roll angle in the current state The rotation matrix of the axis, , Indicates the pitch angle in the current state. Indicates the pitch angle in the current state The rotation matrix of the axis, , Indicates the encoder rotation angle in the current state, Indicates the encoder rotation angle in the current state. The rotation matrix about the axis.

[0028] S4. Calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle.

[0029] Specifically, the formula used to calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle is: ,in, represents the first yaw angle, Indicates the value corresponding to the second row and first column of the rotation matrix corresponding to the first yaw angle, Indicates the value corresponding to the first row and first column in the rotation matrix corresponding to the first yaw angle, express The corresponding coordinates on the plane ( , ) angle.

[0030] S5. Correct the first yaw angle using the second yaw angle in the attitude angle of the IMU to obtain a corrected yaw angle.

[0031] Specifically, the formula for correcting the first yaw angle using the second yaw angle in the IMU attitude angle is: ,in, represents the corrected yaw angle, represents the first yaw angle, represents the second yaw angle, Represents the weight parameter, ranging from 0 to 1.

[0032] The corrected yaw angle is used to calculate the accurate position and attitude information, so that the system that uses the encoder to replace the GNSS to obtain the device attitude information can obtain accurate position and attitude information.

Claims

1. A method for correcting yaw angle based on encoder and IMU, characterized in that: Applicable to a system that uses an encoder to replace GNSS to obtain device attitude information, the method includes the following steps: S1. In a stationary state, use the IMU to perform static alignment calibration and obtain acceleration. Based on the acceleration, calculate the pitch and roll angles in the stationary state, and read the encoder rotation angle in the stationary state. S2. Obtain the encoder rotation angle and the IMU attitude angle in the current state, where the IMU attitude angle includes the roll angle, pitch angle and second yaw angle in the current state; S3. Calculate the rotation matrix corresponding to the first yaw angle based on the rotation matrices corresponding to the pitch angle in the stationary state, the roll angle in the stationary state, the encoder rotation angle in the stationary state, the encoder rotation angle in the current state, the roll angle in the current state, and the pitch angle in the current state. The formula used to calculate the rotation matrix corresponding to the first yaw angle is: ,in, represents the first yaw angle, represents the rotation matrix corresponding to the first yaw angle, , Indicates the roll angle at rest, Indicates the roll angle at rest The rotation matrix of the axis, , Indicates the pitch angle when the editor is stationary. Indicates the pitch angle at rest The rotation matrix of the axis, , Indicates the encoder rotation angle in a stationary state, Indicates the encoder rotation angle in a stationary state The rotation matrix of the axis, , Indicates the roll angle in the current state, Indicates the roll angle in the current state The rotation matrix of the axis, , Indicates the pitch angle in the current state. Indicates the pitch angle in the current state The rotation matrix of the axis, , Indicates the encoder rotation angle in the current state, Indicates the encoder rotation angle in the current state. The rotation matrix of the axis; S4. Calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle; S5. Correct the first yaw angle using the second yaw angle in the attitude angle of the IMU to obtain a corrected yaw angle.

2. The method for correcting yaw angle based on encoder and IMU according to claim 1, characterized in that: In S1, the formula for calculating the pitch angle at rest based on acceleration is: ,in, represents the pitch angle, represents acceleration, Represents the component of acceleration on the X-axis.

3. The method for correcting yaw angle based on encoder and IMU according to claim 1, characterized in that: In S1, the formula for calculating the roll angle at rest based on acceleration is: ,in, represents the roll angle, represents acceleration, Indicates the acceleration The component on the axis, Represents the component of acceleration along the Z axis.

4. The method for correcting yaw angle based on encoder and IMU according to claim 1, characterized in that: In S4, the formula used to calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle is: ,in, represents the first yaw angle, Indicates the value corresponding to the second row and first column of the rotation matrix corresponding to the first yaw angle, Indicates the value corresponding to the first row and first column in the rotation matrix corresponding to the first yaw angle, express The corresponding coordinates on the plane ( , ) angle.

5. The method for correcting yaw angle based on encoder and IMU according to claim 1, characterized in that: In S5, the formula for correcting the first yaw angle using the second yaw angle in the IMU attitude angle is: ,in, represents the corrected yaw angle, represents the first yaw angle, represents the second yaw angle, Represents the weight parameter, ranging from 0 to 1.

Images