Height correction method for IMU and laser range finder information fusion

Abstract

The invention provides an IMU and laser range finder information fusion height correction method comprising the following steps: starting initial alignment in a static state, and obtaining a horizontal attitude; inertial navigation calculation: calculating the speed, position and attitude of the system according to the horizontal attitude and IMU original data; the attitude at least comprises a horizontal attitude angle; a laser ranging value is converted into a height value according to a horizontal attitude angle, and then a plurality of height values are fitted into a straight line to obtain a predicted speed value and a predicted height value; and fusing the inertial navigation resolving state and the fitting straight line data through a Kalman filter to obtain a corrected height value. Fitting a plurality of laser ranging values to obtain a predicted height and a predicted speed, wherein the predicted height and the predicted speed are used as measured values and enter a Kalman filter; meanwhile, the uncertainty of the distance measurement value is calculated according to the predicted value, the filter error is reasonably distributed, and the method can ensure that the speed and the height of the height channel cannot be diverged along with the running time.

Description

Technical Field

[0001] This application relates to the field of aviation technology, specifically to a height correction method that fuses information from an IMU and a laser rangefinder. Background Technology

[0002] As the demand for positioning and altitude determination in equipment increases, integrated navigation based on multi-sensor information fusion has a promising future. Integrated navigation can leverage the complementary strengths of different sensors to improve navigation accuracy, offering broad application prospects. Real-time altitude positioning of flight equipment may rely on sensors such as IMUs, cameras, barometers, ultrasonic sensors, and laser rangefinders. These devices provide distance information for altitude channels and can also provide criteria for judging motion status. However, simply superimposing multiple sensors is insufficient to meet the requirements for high-precision, real-time, and low-cost correction. Therefore, it is necessary to consider fully fusing sensor information. A high-frequency IMU and a high-precision laser rangefinder can be effectively combined to achieve high-precision altitude positioning.

[0003] Inertial sensors, primarily IMUs, can measure high-frequency angular velocity and acceleration of the system and further calculate velocity, position, and attitude. However, due to error characteristics, the height channel diverges rapidly. Laser rangefinders, while providing lower-frequency ranging values, offer higher and more stable accuracy. Their ranging values ​​do not diverge over time, but they are susceptible to interference from environmental factors, leading to anomalies or introduced errors. For example, hitting a reflective surface might result in no echo and no ranging value, or hitting an obstacle might introduce an error related to the obstacle's height. Therefore, using laser ranging values ​​to correct inertial navigation data can effectively improve the system's positioning accuracy. Furthermore, both types of sensors are autonomous and will not fail to operate due to environmental limitations or external signal shielding, allowing for a wider range of applications.

[0004] Existing methods have limitations, especially in real-world scenarios, such as those described in the instruction manual. Figure 1 As shown, the ground is complex. It is not a flat surface and may contain obstacles. Therefore, there is an error between the distance measurement value obtained by the distance measuring machine and the absolute true value of the ground. Existing methods mostly rely on simple processing such as smoothing filtering, and then directly use the smoothed distance measurement value as the measurement value of the filter. This does not actually make full use of the distance measurement information. Summary of the Invention

[0005] In view of the above-mentioned defects or deficiencies in the prior art, it is desirable to provide a height correction method for the fusion of IMU and laser rangefinder information.

[0006] This application provides a height correction method for fusing information from an IMU and a laser rangefinder, including the following steps: S100 initiates initial alignment from a stationary state to acquire horizontal attitude; S200, Inertial Navigation Calculation: Based on the current state of the inertial navigation system and the raw IMU data, the velocity, position, and attitude of the system are calculated; the attitude includes at least the horizontal attitude angle. S300, first convert the laser ranging value into a height value according to the horizontal attitude angle, then fit a straight line to multiple height values ​​to obtain the predicted velocity value and the predicted height value; The S400 uses a Kalman filter to fuse the inertial navigation solution state and the fitted straight line data to obtain the corrected height value.

[0007] Furthermore, Step S100 also includes the following steps: S101, Inject initial position information; S102, caches a set number of raw IMU data; S103, calculate the horizontal attitude, i.e., the pitch angle and roll angle in the navigation coordinate system.

[0008] Furthermore, The steps between S200 and S300 also include: S210, when the device is in a stationary state, the state is maintained by the zero-speed correction algorithm; S220, when the device is in the startup state, determine whether the system has received the laser ranging value.

[0009] Furthermore, S230, the system receives the laser ranging value and executes step S300; S240, the system did not receive the laser ranging value, and proceeded to step S200.

[0010] Furthermore, Step S300 also includes the following steps: S301, if the cumulative time of a single laser reception or sliding window expires, first clear the sliding window, and then execute step S200; S302, if the single laser reception or the cumulative time of the sliding window has not expired, the laser ranging value is converted into a height value; S303, the height value and the corresponding reception time are cached together in the sliding window; S304, determine whether the sliding window has overflowed.

[0011] Furthermore, S305, the sliding window has not overflowed, proceed to step S200; S306, the sliding window overflows, and a straight line is fitted to the height value using the least squares method.

[0012] Furthermore, The predicted velocity value is expressed by the slope of the fitted straight line; The predicted height value is expressed by the intercept of the fitted straight line; The uncertainty of the predicted height can be calculated from the predicted height and the height obtained by laser ranging, which is the noise of the measured value in the Kalman filter.

[0013] Furthermore, Step S400 includes the following specific steps: S401, Establish the filter model. S402, the predicted height and predicted velocity values ​​are used as measurement values ​​and fused with the inertial navigation solution state through the filter model; S403 outputs the corrected height value.

[0014] The advantages and positive effects of this application are: This technical solution uses multiple laser ranging values ​​to fit a predicted height and velocity, which are then fed into a Kalman filter as measurement values. Simultaneously, it calculates the uncertainty of the ranging value based on the predicted value and reasonably allocates the filter error. This method ensures that the velocity and height of the height channel do not diverge over time. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of distance measurement in a real-world scenario within the background technology. Figure 2 This is a flowchart illustrating the height correction method for IMU and laser rangefinder information fusion provided in an embodiment of this application. Figure 3 This is a complete flowchart illustrating the height correction method for IMU and laser rangefinder information fusion provided in this application embodiment. Detailed Implementation

[0016] To enable those skilled in the art to better understand the technical solution of this application, the application will be described in detail below with reference to the accompanying drawings. The description in this section is only exemplary and explanatory, and should not be used to limit the scope of protection of this application.

[0017] This embodiment provides a height correction method for fusing information from an IMU and a laser rangefinder, including the following steps: S100 initiates initial alignment from a stationary state to acquire horizontal attitude; Specifically, it includes the following steps: S101, Inject initial position information; where the initial position information is mainly altitude information, such as the current runway altitude; S102, cache a set number of IMU raw data; for example, cache 50 sets of angular velocity and acceleration data collected by IMUs in a stationary state to reduce the impact of random errors; S103, calculate the horizontal attitude, i.e., the pitch angle and roll angle in the navigation coordinate system; wherein, caching a sufficient amount of raw IMU data before calculating the horizontal attitude can effectively reduce error interference and improve the stability and reliability of attitude estimation.

[0018] S200, Inertial Navigation Calculation: Based on the current state of the inertial navigation system and the raw IMU data, the velocity, position, and attitude of the system are calculated; the attitude includes at least the horizontal attitude angle. Furthermore, after completing the inertial navigation calculation, the system will determine the operating status of the device through the zero-velocity detector and execute the corresponding subsequent steps based on the determination result; S210, when the device is stationary, the state is maintained by the zero-speed correction algorithm; when the device is stationary, the zero-speed correction algorithm can use the deterministic constraint of zero speed to break the vicious cycle of pure inertial navigation error accumulating over time, solve the drift problem of pure inertial navigation system, and ensure that the state estimation when stationary is always accurate and reliable. S220, when the device is in the startup state, determine whether the system has received the laser ranging value; S230, the system receives the laser ranging value and executes step S300; S240, the system did not receive the laser ranging value, and proceeded to step S200.

[0019] S300, first convert the laser ranging value into a height value according to the horizontal attitude angle, then fit a straight line to multiple height values ​​to obtain the predicted velocity value and the predicted height value; Specifically, it includes the following steps: S301, if the cumulative time of a single laser reception or sliding window expires, first clear the sliding window, and then execute step S200; specifically, when the time Δt between the new value and the previous value exceeds the timing threshold, it means that the previous data is unreliable and the data in the buffer needs to be cleared. S302, if the single laser reception or sliding window accumulation time has not expired, the laser ranging value is converted into a height value; wherein, the calculation formula is as follows: (one) In the formula, h is the height after conversion of the laser ranging value; l is the laser ranging value; P is the pitch angle in the attitude angle; and R is the roll angle in the attitude angle. S303, the height value and the corresponding reception time are cached together in the sliding window; wherein, caching the height value and the corresponding reception time in the sliding window at the same time is to make the data have time correlation, thereby supporting more accurate data analysis and processing that is more in line with the actual scenario; S304, determine whether the sliding window has overflowed.

[0020] S305, the sliding window has not overflowed, proceed to step S200; S306, the sliding window overflows, and a straight line is fitted to the height value using the least squares method; where the horizontal axis is time and the vertical axis is height, and the calculation formula is as follows: (two) in, (three) (Four) In the formula, k is the predicted velocity; b is the predicted height.

[0021] Furthermore, the predicted velocity value is expressed by the slope of the fitted straight line; the predicted height value is expressed by the intercept of the fitted straight line; the uncertainty of the predicted value can be calculated based on the predicted height value and the height value obtained by laser ranging, which is the noise of the measured value in the Kalman filter.

[0022] S400 uses a Kalman filter to fuse the inertial navigation solution state and the fitted straight line data to obtain the corrected height value; Specifically, it includes the following steps: S401, Establish the filter model. S402, the predicted altitude and predicted velocity values ​​are used as measurement values ​​and fused with the inertial navigation system (INS) solution state through the filter model; wherein, the INS solution state is as follows: (five) in, This represents the attitude matrix from the carrier system (b-frame, laser line of sight) to the navigation system (n-frame). Indicates the speed of the aircraft. Indicates the aircraft's position. and These represent acceleration and gyroscope deviation, respectively. S403 outputs the corrected height value.

[0023] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. The above descriptions are only preferred embodiments of this application. It should be noted that due to the limitations of textual expression, while there are objectively infinite specific structures, those skilled in the art can make several improvements, modifications, or changes without departing from the principles of this invention, and can also combine the above technical features in an appropriate manner. These improvements, modifications, changes, or combinations, or the direct application of the inventive concept and technical solution to other situations without modification, should all be considered within the scope of protection of this application.

Claims

1. A height correction method for IMU and laser rangefinder information fusion, characterized in that, Includes the following steps: S100 initiates initial alignment from a stationary state to acquire horizontal attitude; S200, Inertial Navigation Calculation: Based on the current state of the inertial navigation system and the raw IMU data, the velocity, position, and attitude of the system are calculated; the attitude includes at least the horizontal attitude angle. S300, first convert the laser ranging value into a height value according to the horizontal attitude angle, then fit a straight line to multiple height values ​​to obtain the predicted velocity value and the predicted height value; The S400 uses a Kalman filter to fuse the inertial navigation solution state and the fitted straight line data to obtain the corrected height value.

2. The height correction method for IMU and laser rangefinder information fusion according to claim 1, characterized in that, Step S100 also includes the following steps: S101, Inject initial position information; S102, caches a set number of raw IMU data; S103, calculate the horizontal attitude, i.e., the pitch angle and roll angle in the navigation coordinate system.

3. The height correction method for IMU and laser rangefinder information fusion according to claim 1, characterized in that, The steps between S200 and S300 also include: S210, when the device is in a stationary state, the state is maintained by the zero-speed correction algorithm; S220, when the device is in the startup state, determine whether the system has received the laser ranging value.

4. The height correction method for IMU and laser rangefinder information fusion according to claim 3, characterized in that, S230, the system receives the laser ranging value and executes step S300; S240, the system did not receive the laser ranging value, and proceeded to step S200.

5. The height correction method for IMU and laser rangefinder information fusion according to claim 4, characterized in that, Step S300 also includes the following steps: S301, if the cumulative time of a single laser reception or sliding window expires, first clear the sliding window, and then execute step S200; S302, if the single laser reception or the cumulative time of the sliding window has not expired, the laser ranging value is converted into a height value; S303, the height value and the corresponding reception time are cached together in the sliding window; S304, determine whether the sliding window has overflowed.

6. The height correction method for IMU and laser rangefinder information fusion according to claim 5, characterized in that, S305, the sliding window has not overflowed, proceed to step S200; S306, the sliding window overflows, and a straight line is fitted to the height value using the least squares method.

7. The height correction method for IMU and laser rangefinder information fusion according to claim 6, characterized in that, The predicted velocity value is expressed by the slope of the fitted straight line; The predicted height value is expressed by the intercept of the fitted straight line; The uncertainty of the predicted height can be calculated from the predicted height and the height obtained by laser ranging, which is the noise of the measured value in the Kalman filter.

8. The height correction method for IMU and laser rangefinder information fusion according to claim 1, characterized in that, Step S400 includes the following specific steps: S401, Establish the filter model. S402, the predicted height and predicted velocity values ​​are used as measurement values ​​and fused with the inertial navigation solution state through the filter model; S403 outputs the corrected height value.

Images