A heading angle calculation method based on front-end fusion

Abstract

The field of calculation method of robot heading angle of the present invention, specifically discloses a heading angle calculation method based on front-end fusion, including an inertial navigation module, a front-end fusion module and a positioning algorithm module, the inertial navigation module outputs the original IMU data, and the positioning algorithm module calculates and processes To output the pose data, the front-end fusion module includes a Kalman filter unit and a motion model calculation unit. The front-end fusion module receives the pose data and the original IMU data and processes them to obtain the fused IMU data including heading angle, angular velocity and linear acceleration, and outputs it to the positioning algorithm module. Its heading angle calculation method uses the pose data output by the positioning algorithm module as an observation to realize the method of heading angle incremental Kalman filtering; there is also a heading angle output method that uses a motion model to update, which can provide a relatively accurate positioning algorithm. Accurate heading angle estimation can timely correct the occasional position loss problem of the positioning algorithm during registration, and improve the reliability and stability of positioning.

Description

technical field [0001] The invention relates to the field of calculation methods for heading angles of robots. Background technique [0002] In the prior art, the robot usually obtains the laser data of the lidar and the IMU data of the inertial navigation module to perform positioning calculations, such as figure 1 As shown, the inertial navigation module is a module that is often used in robot navigation technology. It includes a gyroscope. The accelerometer can provide data such as angular velocity and linear acceleration for the positioning algorithm. It can also include a magnetometer. Using a magnetometer The inertial navigation module can provide heading angle information. After coordinate transformation, it can be directly used as a positioning algorithm to estimate the orientation of the pose. This method can provide a more accurate estimated pose for the positioning algorithm and reduce the number of iterations and calculations of the positioning algorithm. Howeve...

AI Technical Summary

Problems solved by technology

[0002] In the prior art, the robot usually obtains the laser data of the lidar and the IMU data of the inertial navigation module to perform positioning calculations, such as figure 1 As shown, the inertial navigation module is a module that is often used in robot navigation technology. It includes a gyroscope. The accelerometer can provide data such as angular velocity and linear acceleration for the positioning algorithm. It can also include a magnetometer. Using a magnetometer The inertial navigation module can provide heading angle information. After coordinate transformation, it can be directly used as a positioning algorithm to estimate the orientation of the pose. This method can provide a more accurate estimated pose for the positioning algorithm and reduce the number of iterations and calculations of the positioning algorithm. However, the use of the magnetometer will be disturbed by the scene, especially in the scene of serious interference, which will lead to inaccurate heading angle of the inertial navigation module; there are also inertial navigation modules without a magnetometer, and there is no data of the heading angle, so the angular velocity is used Integrate to get the angle increment, combined with the pose at the previous moment, calculate the estimated pose at the current moment through the motion model, but this method is prone to lack of accuracy due to the cumulative error of the angular velocity integration

[0003] To sum up, the existing technical solutions, either depend on whether the inertial navigation module supports heading angle output; or are prone to lack of accuracy due to the cumulative error of angular velocity integration, and cannot provide a more accurate estimated pose for the positioning algorithm, which may lead to serious positioning problems. lost

Images