Method for correcting yaw angle based on encoder and IMU

Abstract

The invention provides a method for correcting a yaw angle based on an encoder and an IMU (Inertial Measurement Unit), and relates to the technical field of laser radar 3D point cloud attitude calculation. In a static state, static alignment calibration is performed by using the IMU, an acceleration is obtained, a pitch angle and a roll angle in the static state are calculated according to the acceleration, and a rotation angle of the encoder in the static state is read; acquiring a rotation angle, a roll angle, a pitch angle and a second yaw angle of the encoder in the current state, and calculating a rotation matrix corresponding to the first yaw angle based on rotation matrixes corresponding to the pitch angle, the roll angle and the rotation angle of the encoder in the static state and rotation matrixes corresponding to the rotation angle, the roll angle and the pitch angle of the encoder in the current state; the first yaw angle is calculated based on the rotation matrix, the first yaw angle is corrected by utilizing the second yaw angle, the corrected yaw angle is obtained, the problem that the accurate yaw angle cannot be obtained indoors or under the condition that no GNSS exists is solved, and the method is suitable for a system for obtaining the equipment attitude information by adopting the encoder to replace the GNSS.

Description

Technical Field

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

[0002] Currently, in the process of rapid and high-precision 3D big data modeling, it is necessary to use 3D lidar point cloud combined with the device position and attitude to achieve high-precision 3D big data modeling. The current common method is to use GNSS positioning information combined with IMU, adopt the rotation matrix, and achieve cascade solution to obtain information such as position and attitude, and then combine the lidar 3D point cloud data to achieve three-dimensional space modeling. For example, the calculation of the geodetic three-dimensional coordinate values of shipborne lidar scanning point cloud data recorded in the research on the coordinate solution method of shipborne lidar scanning point cloud data.

[0003] Three rotation angles around different coordinate axes, such as the rotation angle around the Z axis is denoted as the yaw angle, the rotation angle around the Y axis is denoted as the pitch angle, and the rotation angle around the X axis is denoted as the roll angle.

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

[0005] Then the total rotation matrix in the order of Z - Y - X is .

[0006] In an indoor environment or without GNSS, an accurate yaw angle cannot be obtained, so that accurate position and attitude information cannot be solved. Summary of the Invention

[0007] The technical problem to be solved by the present invention: The present invention provides a method for correcting the yaw angle based on an encoder and an IMU, and solves the problem that an accurate yaw angle cannot be obtained in an indoor environment or without GNSS.

[0008] The technical solution adopted by the present invention to solve the above technical problem: A method for correcting the 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, and includes the following steps: S1. In a static state, use the IMU for static alignment calibration, obtain the acceleration, calculate the pitch angle and roll angle in the static state based on the acceleration, and read the encoder rotation angle in the static state; S2. Obtain the rotation angle of the encoder and the attitude angles of the IMU in the current state. The attitude angles of the IMU include 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 at rest, the roll angle at rest, the encoder rotation angle at rest, the encoder rotation angle in the current state, the roll angle in the current state, and the pitch angle in the current state respectively. S4. Calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle. S5. Use the second yaw angle in the attitude angles of the IMU to correct the first yaw angle to obtain the corrected yaw angle.

[0009] Further, in S1, the formula for calculating the pitch angle at rest according to the acceleration is: , where represents the pitch angle, represents the acceleration, represents the component of the acceleration on the X-axis.

[0010] Further, in S1, the formula for calculating the roll angle at rest according to the acceleration is: , where represents the roll angle, represents the acceleration, represents the acceleration on the axis, represents the component of the acceleration on the Z-axis.

[0011] Further, in S3, the formula for calculating the rotation matrix corresponding to the first yaw angle is: , where represents the first yaw angle, represents the rotation matrix corresponding to the first yaw angle, , represents the roll angle at rest, represents the rotation matrix of the pitch angle at rest around the axis, , represents the pitch angle at rest, represents the rotation matrix of the pitch angle at rest around the axis, , represents the encoder rotation angle at rest, represents the rotation matrix of the encoder rotation angle at rest around the axis, , represents the roll angle in the current state, The rotation matrix of the roll angle around the axis in the current state, , represents the pitch angle in the current state, The rotation matrix of the pitch angle around the axis in the current state, , represents the rotation angle of the encoder in the current state, The rotation matrix of the rotation angle of the encoder around the axis in the current state.

[0012] Furthermore, in S4, the formula for calculating the first yaw angle based on the rotation matrix corresponding to the first yaw angle is: , where represents the first yaw angle, represents the value corresponding to the second row and first column in the rotation matrix corresponding to the first yaw angle, represents the value corresponding to the first row and first column in the rotation matrix corresponding to the first yaw angle, represents the angle of the coordinate ( , ) on the

[0013] Furthermore, in S5, the formula for correcting the first yaw angle using the second yaw angle in the attitude angle of the IMU is: , where represents the corrected yaw angle, represents the first yaw angle, represents the second yaw angle, represents the weight parameter, and its value range is from 0 to 1.

[0014] Advantages of the present invention: The present invention provides a method for correcting the yaw angle based on an encoder and an IMU. In a stationary state, the IMU is used for static alignment calibration, and the acceleration is obtained. Based on the acceleration, the pitch angle and roll angle in the stationary state are calculated, and the rotation angle of the encoder in the stationary state is read to obtain the rotation angle of the encoder and the attitude angle of the IMU in the current state. The attitude angle of the IMU includes the roll angle, pitch angle, and second yaw angle in the current state. The rotation matrix corresponding to the first yaw angle is calculated based on the rotation matrices corresponding to the pitch angle in the stationary state, the roll angle in the stationary state, the rotation angle of the encoder in the stationary state, the rotation angle of the encoder in the current state, the roll angle in the current state, and the pitch angle in the current state. The first yaw angle is calculated based on the rotation matrix corresponding to the first yaw angle, and the first yaw angle is corrected using the second yaw angle in the attitude angle of the IMU to obtain the corrected yaw angle, solving the problem that an accurate yaw angle cannot be obtained indoors or in the absence of GNSS. Description of the Drawings

[0015] Figure 1 is a schematic flowchart of a method for correcting the yaw angle based on an encoder and an IMU provided by the present invention. Detailed Embodiments

[0016] In view of the problem that an accurate yaw angle cannot be obtained indoors or in the absence of GNSS, the present invention provides a method for correcting the 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. The method is as Figure 1 shown and includes the following steps: S1. In a stationary state, the IMU is used for static alignment calibration, and the acceleration is obtained. Based on the acceleration, the pitch angle and roll angle in the stationary state are calculated, and the rotation angle of the encoder in the stationary state is read.

[0017] Specifically, the formula for calculating the pitch angle in the stationary state based on the acceleration is: , where represents the pitch angle, represents the acceleration, represents the component of the acceleration on the X-axis. The formula for calculating the roll angle in the stationary state based on the acceleration is: , where represents the roll angle, represents the acceleration, represents the component of the acceleration on the axis, represents the component of the acceleration on the Z-axis.

[0018] S2. Obtain the rotation angle of the encoder and the attitude angles of the IMU in the current state. The attitude angles of the IMU include the roll angle, pitch angle, and second yaw angle in the current state.

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

[0020] S3. Calculate the rotation matrix corresponding to the first yaw angle based on the rotation matrices corresponding to the pitch angle at rest, roll angle at rest, encoder rotation angle at rest, encoder rotation angle in the current state, roll angle in the current state, and pitch angle in the current state, respectively.

[0021] Specifically, the formula used to calculate the rotation matrix corresponding to the first yaw angle is: , where represents the first yaw angle, represents the rotation matrix corresponding to the first yaw angle, , represents the roll angle at rest, represents the rotation matrix of the pitch angle at rest about the axis, , represents the pitch angle at rest, represents the rotation matrix of the pitch angle at rest about the axis, , represents the encoder rotation angle at rest, represents the rotation matrix of the encoder rotation angle at rest about the axis, , represents the roll angle in the current state, represents the rotation matrix of the roll angle in the current state about the axis, , represents the pitch angle in the current state, represents the rotation matrix of the pitch angle in the current state about the axis, , represents the encoder rotation angle in the current state, represents the rotation matrix of the encoder rotation angle in the current state about the axis.

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

[0023] Specifically, the formula used to calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle is: , where represents the first yaw angle, represents the value corresponding to the first element in the second row of the rotation matrix corresponding to the first yaw angle, represents the value corresponding to the first element in the first row of the rotation matrix corresponding to the first yaw angle, represents the angle of the coordinate ([[]]END]] , ) on the

[0024] S5. Use the second yaw angle in the attitude angles of the IMU to correct the first yaw angle to obtain the corrected yaw angle.

[0025] Specifically, the formula for correcting the first yaw angle using the second yaw angle in the attitude angles of the IMU is: , where represents the corrected yaw angle, represents the first yaw angle, represents the second yaw angle, represents the weight parameter, and its value range is from 0 to 1.

[0026] Solve accurate position and attitude information through the corrected yaw angle above, so that the system using the encoder to replace the GNSS to obtain the device attitude information can obtain accurate position and attitude information.

Claims

1. Method for correcting yaw angle based on encoder and IMU, characterized in that Applied 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 for static alignment calibration, obtain the acceleration, calculate the pitch angle and roll angle in the stationary state based on the acceleration, and read the encoder rotation angle in the stationary state; S2. Obtain the encoder rotation angle in the current state and the attitude angles of the IMU, where the attitude angles of the IMU include 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; S4. Calculate the first yaw angle based on the rotation matrix corresponding to the first yaw angle; S5. Use the second yaw angle in the attitude angles of the IMU to correct the first yaw angle to obtain the corrected yaw angle.

2. The method for correcting the yaw angle based on an encoder and an IMU according to claim 1, wherein In S1, the formula for calculating the pitch angle in the stationary state based on acceleration is: , where represents the pitch angle, represents the acceleration, represents the component of the acceleration on the X-axis.

3. The method for correcting the yaw angle based on an encoder and an IMU according to claim 1, wherein In S1, the formula for calculating the roll angle in the stationary state based on acceleration is: , where represents the roll angle, represents the acceleration, represents the component of the acceleration in the axis, represents the component of the acceleration in the Z axis.

4. The method for correcting the yaw angle based on an encoder and an IMU according to claim 1, wherein In S3, the formula for calculating the rotation matrix corresponding to the first yaw angle is as follows: , where represents the first yaw angle, represents the rotation matrix corresponding to the first yaw angle, , represents the roll angle in the stationary state, represents the rotation matrix of the pitch angle in the stationary state around the axis, , represents the pitch angle in the stationary state, represents the rotation matrix of the pitch angle in the stationary state around the axis, , represents the encoder rotation angle in the stationary state, represents the rotation matrix of the encoder rotation angle in the stationary state around the axis, , represents the roll angle in the current state, represents the rotation matrix of the roll angle in the current state around the axis, , represents the pitch angle in the current state, represents the rotation matrix of the pitch angle in the current state around the axis, , represents the encoder rotation angle in the current state, represents the rotation matrix of the encoder rotation angle in the current state around the axis.

5. The method for correcting the yaw angle based on an encoder and an IMU according to claim 1, wherein In S4, the formula for calculating the first yaw angle based on the rotation matrix corresponding to the first yaw angle is: , where represents the first yaw angle, represents the value corresponding to the first column of the second row in the rotation matrix corresponding to the first yaw angle, represents the value corresponding to the first column of the first row in the rotation matrix corresponding to the first yaw angle, represents the angle of the coordinate ([[]]END]] , ) on the 6. The method for correcting the yaw angle based on an encoder and an IMU according to claim 1, wherein In S5, the formula for correcting the first yaw angle using the second yaw angle in the attitude angles of the IMU is: , where represents the corrected yaw angle, represents the first yaw angle, represents the second yaw angle, represents the weight parameter, and its value range is from 0 to 1.

Images