Barometer, laser radar and optical flow sensor multi-source fusion unmanned aerial vehicle positioning method based on Kalman filtering

Abstract

The invention provides a barometer, laser radar and optical flow sensor multi-source fusion unmanned aerial vehicle positioning method based on Kalman filtering, and the method comprises the steps: fusing the measurement data of an inertial measurement unit, a barometer and a laser radar based on a Kalman filter, and obtaining the fusion height of an unmanned aerial vehicle; performing attitude compensation on the pixel displacement measured by the optical flow sensor based on the attitude angle measured by the inertial measurement unit, and performing scale conversion on the compensated pixel displacement according to the fusion height to obtain the horizontal speed of the unmanned aerial vehicle; comparing and fusing the horizontal speed with the speed estimated by the inertial navigation system, and correcting the horizontal position estimation of the inertial navigation system to obtain the positioning of the unmanned aerial vehicle in the environment without the global positioning system signal; the problem of scale misalignment caused by height change is solved, and the horizontal positioning precision is remarkably improved, so that long-time and high-precision horizontal position keeping is realized in a GPS-free environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) navigation and positioning technology, and in particular to a multi-source fusion method for UAV positioning based on Kalman filtering, using a barometer, lidar, and optical flow sensor. Background Technology

[0002] Currently, in environments where GPS signals are blocked, such as indoors, underground, or in urban canyons, drones cannot rely on GPS for navigation and must rely on onboard sensors for autonomous positioning. Common sensors used in such environments include inertial measurement units (IMUs), barometers, laser rangefinders (LiDAR), and optical flow sensors. However, these individual sensors all have inherent defects: barometers (such as SPL06) are susceptible to propeller downwash and ambient temperature, exhibiting high-frequency noise and long-term drift, making it difficult to achieve centimeter-level altitude fixation; lidar (such as VL53L1X) has high accuracy but limited range (usually <4 meters), and is affected by ground material reflectivity, which may cause data jumps or loss; optical flow sensors (such as PMW3901) can measure the speed of ground texture movement, but when the aircraft tilts (Roll / Pitch changes), "false displacement" will occur, and the ratio between its output pixel velocity and the actual physical velocity (scale factor) is highly dependent on the flight altitude. If the altitude is not accurately estimated, the optical flow data will be completely invalid; existing technologies usually simply switch thresholds or weighted average the data from each sensor, lacking dynamic estimation of sensor noise characteristics, and have not effectively solved the coupling interference problem of aircraft attitude on optical flow data, resulting in insufficient positioning accuracy and stability of UAVs in complex GPS-free environments; Therefore, in order to overcome the above-mentioned technical problems, the present invention provides a multi-source fusion method for UAV positioning based on Kalman filtering, which integrates barometer, lidar and optical flow sensor. Summary of the Invention

[0003] This invention provides a multi-source fusion method for UAV positioning based on Kalman filtering, which integrates barometer, lidar, and optical flow sensors. Through a hierarchical sensor fusion architecture, it dynamically fuses altitude information in the vertical channel using an improved Kalman filter, and performs scale correction and attitude compensation on the optical flow data in the horizontal channel using the fused high-precision altitude. Ultimately, this enables UAVs to achieve accurate altitude and horizontal positioning in environments without GPS.

[0004] A multi-source fusion method for UAV localization based on Kalman filtering, including barometer, lidar, and optical flow sensor, comprising: Step 1: Based on the Kalman filter, fuse the measurement data from the inertial measurement unit, barometer, and lidar to obtain the fused altitude of the UAV; Step 2: Based on the attitude angle measured by the inertial measurement unit, the pixel displacement measured by the optical flow sensor is attitude compensated, and the compensated pixel displacement is scaled according to the fusion height to obtain the horizontal velocity of the UAV. Step 3: Compare and fuse the horizontal velocity with the velocity estimated by the inertial navigation system, correct the horizontal position estimate of the inertial navigation system, and obtain the UAV's positioning in an environment without a global positioning system signal.

[0005] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering of barometer, lidar, and optical flow sensor data includes step 1, which involves fusing measurement data from the inertial measurement unit, barometer, and lidar based on Kalman filtering to obtain the fused altitude of the UAV, including: Obtain the state vector of the Kalman filter, wherein the state vector includes at least the altitude and vertical velocity of the UAV; Based on the uniform acceleration motion model, and by updating the state vector over time according to the vertical acceleration measured by the inertial measurement unit, the prior estimate of the state vector and the corresponding prior error covariance matrix are obtained. Determine whether the lidar data is in a valid measurement state, and dynamically adjust the value of the observation noise covariance matrix of the Kalman filter based on the determination result; The Kalman gain is calculated based on the prior error covariance matrix and the adjusted observation noise covariance matrix. The prior estimate of the state vector based on the Kalman gain is fused and updated with the altitude observations from the barometer or lidar to obtain the posterior estimate of the state vector including the fused altitude.

[0006] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering, using a barometer, lidar, and optical flow sensor, includes determining whether the lidar data is in a valid measurement state, comprising: Obtain the current measurement height and data quality flag bit output by the lidar; Determine whether the current measured height is less than a preset height threshold, and whether the data quality flag indicates normal operation; When the current measurement height is less than the preset height threshold and the data quality flag indicates normal operation, the lidar is determined to be in a valid measurement state. Otherwise, the lidar is determined not to be in an effective measurement state; Based on the judgment results, a dynamic adjustment strategy is generated for the values ​​of the observation noise covariance matrix of the Kalman filter.

[0007] Preferably, a multi-source fusion UAV positioning method based on Kalman filtering, incorporating barometers, lidar, and optical flow sensors, generates a dynamic adjustment strategy for the value of the Kalman filter observation noise covariance matrix based on the determination results, including: When the lidar is in an effective measurement state, the observation noise covariance matrix of the Kalman filter is adjusted to the first value; When the lidar is not in an effective measurement state, the observation noise covariance matrix is ​​adjusted to the second value; The second value is greater than the first value.

[0008] Preferably, in a multi-source fusion method for UAV positioning based on Kalman filtering, including barometer, lidar, and optical flow sensor, step 2 involves attitude compensation based on the attitude angle measured by the inertial measurement unit for the pixel displacement measured by the optical flow sensor, including: The raw pixel displacement measured by the optical flow sensor is obtained, and the raw pixel displacement is low-pass filtered to obtain the filtered pixel displacement. Obtain the current pitch angle of the UAV as measured by the inertial measurement unit; Based on the current pitch angle and preset focal length related parameters, calculate the pixel displacement compensation amount caused by the change in UAV attitude; The pixel displacement compensation amount is removed from the filtered pixel displacement to obtain the pose-compensated net pixel displacement.

[0009] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering, using a barometer, lidar, and optical flow sensor, includes step 2, where the compensated pixel displacement is scaled according to the fusion height to obtain the UAV's horizontal velocity, comprising: Based on the fusion height, the scale factor of the optical flow sensor is calculated, wherein the scale factor is directly proportional to the fusion height; Based on the scale factor, the pose-compensated pixel displacement is converted into the corresponding physical displacement; The horizontal velocity of the UAV is calculated based on the physical displacement and the corresponding displacement time.

[0010] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering, comprising a barometer, lidar, and optical flow sensor, calculates the scale factor of the optical flow sensor based on the fused altitude, including: Obtain the focal length parameters associated with the optical flow sensor; Based on the focal length parameter and the fusion height, the actual physical distance corresponding to the unit pixel displacement is calculated to obtain the scale factor.

[0011] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering, using a barometer, lidar, and optical flow sensor, includes step 3, which compares and fuses the horizontal velocity with the velocity estimated by the inertial navigation system, corrects the horizontal position estimate of the inertial navigation system, and obtains the UAV's positioning in an environment without a Global Positioning System (GPS) signal. This includes: Obtain the velocity and position estimates calculated by the inertial navigation system through integration; Calculate the speed error between the horizontal speed and the speed estimate; Based on the velocity error and by correcting the velocity and position estimates of the inertial navigation system according to complementary filtering logic, a corrected horizontal position estimate is obtained. The corrected horizontal position estimate is output, and the UAV is controlled in a fixed position based on the horizontal position estimate.

[0012] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering, incorporating a barometer, lidar, and optical flow sensor, is based on a uniform acceleration motion model. The method updates the state vector over time according to the vertical acceleration measured by the inertial measurement unit, obtaining a prior estimate of the state vector and the corresponding prior error covariance matrix, including: Get the current state vector ,in, Indicates the current time The corresponding state vector; Indicates the current time The height value; Show the current time The corresponding vertical velocity; Indicates transpose; Based on the uniformly accelerated motion model and the inertial measurement unit at the current moment... Corresponding vertical acceleration The current state vector is updated over time using the following formula; ; in, Indicates the current time The predicted height; Indicates the previous time before the current time. The corresponding height value; Indicates the previous time before the current time. The corresponding vertical velocity; Indicates the previous moment of the current moment. The time interval up to the current moment; Indicates the current time The corresponding predicted vertical velocity value; based on and Obtain the prior estimate of the state vector: = ; Calculate the prior error covariance matrix corresponding to the prior estimate using the following formula. ; ; in, Let represent the state transition matrix, and ; This represents the process noise covariance matrix.

[0013] In this embodiment, the process noise covariance matrix is ​​used to reflect the accelerometer noise.

[0014] Preferably, a multi-source fusion method for UAV positioning based on Kalman filtering, using a barometer, lidar, and optical flow sensor, further includes altitude hold control after determining the vertical velocity and fused altitude. Specifically: The fusion height and the vertical speed are input to a cascaded PID controller; Obtain the desired height, and calculate the desired vertical speed based on the height error between the desired height and the fused height using the outer loop of the cascaded PID controller; The inner loop of the cascaded PID controller calculates and outputs the throttle control quantity based on the speed error between the desired vertical speed and the vertical speed, and realizes constant altitude control based on the throttle control quantity.

[0015] Compared with the prior art, the beneficial effects of the present invention are as follows: By dynamically adjusting the Kalman filter observation noise covariance, a smooth transition between lidar and barometer data was achieved, effectively suppressing the "step effect" and oscillations in altitude control. Secondly, the innovative use of the fused high-precision altitude real-time correction optical flow scale factor solved the scale inaccuracy problem caused by altitude changes, significantly improving horizontal positioning accuracy. Simultaneously, the optical flow attitude compensation mechanism based on a physical model efficiently eliminated spurious displacements introduced by body tilt, making it suitable for embedded real-time processing. Finally, through the complementary fusion of optical flow velocity and INS velocity, the accumulated INS integral error was corrected, thus achieving long-term, high-precision horizontal position maintenance even in GPS-free environments.

[0016] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in this application.

[0017] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0018] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart of a UAV positioning method based on the multi-source fusion of barometer, lidar and optical flow sensor using Kalman filtering, according to an embodiment of the present invention. Figure 2 This is a flowchart of step 1 in a multi-source fusion UAV positioning method based on Kalman filtering, using a barometer, lidar, and optical flow sensor, according to an embodiment of the present invention. Figure 3 This is a flowchart of step 3 in a multi-source fusion UAV positioning method based on Kalman filtering, using a barometer, lidar, and optical flow sensor, according to an embodiment of the present invention. Detailed Implementation

[0019] The preferred embodiments of the present invention will be described below with reference to the accompanying drawings. It should be understood that the preferred embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0020] Example 1: This example provides a multi-source fusion method for UAV positioning based on Kalman filtering, using barometers, lidar, and optical flow sensors. Figure 1 As shown, it includes: Step 1: Based on the Kalman filter, fuse the measurement data from the inertial measurement unit, barometer, and lidar to obtain the fused altitude of the UAV; The UAV's accurate fused altitude is obtained by fusing measurement data from the inertial measurement unit (IMU), barometer, and lidar using a Kalman filter (KF). Specifically, this step involves designing a Kalman filter that incorporates state variables such as altitude and vertical velocity, and dynamically adjusting its observation noise covariance matrix to smoothly handle the switching between lidar and barometer data, thereby outputting stable and reliable altitude and vertical velocity estimates.

[0021] Step 2: Based on the attitude angle measured by the inertial measurement unit, the pixel displacement measured by the optical flow sensor is attitude compensated, and the compensated pixel displacement is scaled according to the fusion height to obtain the horizontal velocity of the UAV. First, attitude compensation is performed on the raw pixel displacements measured by the optical flow sensor based on attitude angles (such as pitch and roll angles) measured by the IMU to eliminate spurious displacement measurements caused by the tilt of the UAV. Then, using the fused altitude obtained in step 1, the scale factor of the optical flow sensor is calculated in real time, and the attitude-compensated pixel displacements are converted into horizontal velocities in the physical world. This dynamic calculation of the scale factor ensures that the velocity conversion of the optical flow remains accurate even as the flight altitude changes.

[0022] Step 3: Compare and fuse the horizontal velocity with the velocity estimated by the inertial navigation system, correct the horizontal position estimate of the inertial navigation system, and obtain the UAV's positioning in an environment without a global positioning system signal.

[0023] The horizontal optical flow velocity obtained in step 2 is compared with the velocity estimated by the inertial navigation system (INS) through acceleration integration to calculate the velocity error. Based on the velocity error, the velocity and position estimates of the INS are corrected through complementary filtering and other logic, thereby obtaining an accurate and drift-free horizontal position estimate of the UAV in an environment without a global positioning system signal, which is ultimately used for the UAV's fixed-point hovering control.

[0024] The working principle of the above technical solution is as follows: through a layered sensor fusion architecture, the improved Kalman filter is used to dynamically fuse altitude information in the vertical channel, and the high-precision fused altitude is used to perform scale correction and attitude compensation on the optical flow data in the horizontal channel, so as to achieve accurate altitude and horizontal positioning of the UAV in the absence of GPS.

[0025] The beneficial effects of the above technical solution are as follows: First, by dynamically adjusting the observation noise covariance of the Kalman filter, a smooth transition between lidar and barometer data is achieved, effectively suppressing the "step effect" and oscillations in altitude control. Second, the innovative use of the fused high-precision altitude real-time correction optical flow scale factor solves the scale inaccuracy problem caused by altitude changes, significantly improving horizontal positioning accuracy. Simultaneously, the optical flow attitude compensation mechanism based on the physical model efficiently eliminates spurious displacements introduced by body tilt, making it suitable for embedded real-time processing. Finally, through the complementary fusion of optical flow velocity and INS velocity, the accumulated INS integral error is corrected, thereby achieving long-term, high-precision horizontal position maintenance even in GPS-free environments.

[0026] Example 2: Based on Example 1, this example provides a multi-source fusion method for UAV positioning using barometers, lidar, and optical flow sensors based on Kalman filtering, such as... Figure 2 As shown, in step 1, the fused altitude of the UAV is obtained based on the measurement data from the inertial measurement unit, barometer, and lidar, using a Kalman filter. This includes: S101: Obtain the state vector of the Kalman filter, wherein the state vector includes at least the altitude and vertical velocity of the UAV; S102: Based on the uniform acceleration motion model, and according to the vertical acceleration measured by the inertial measurement unit, the state vector is updated over time to obtain the prior estimate of the state vector and the corresponding prior error covariance matrix. S103: Determine whether the lidar data is in a valid measurement state, and dynamically adjust the value of the observation noise covariance matrix of the Kalman filter according to the determination result; S104: Calculate the Kalman gain based on the prior error covariance matrix and the adjusted observation noise covariance matrix; S105: The prior estimate of the state vector based on the Kalman gain is fused and updated with the altitude observations from the barometer or lidar to obtain a posterior estimate of the state vector including the fused altitude.

[0027] In this embodiment, based on a uniform acceleration motion model, and by updating the state vector over time according to the vertical acceleration measured by the inertial measurement unit, a prior estimate of the state vector and the corresponding prior error covariance matrix are obtained, including: Get the current state vector ,in, Indicates the current time The corresponding state vector; Indicates the current time The height value; Show the current time The corresponding vertical velocity; Indicates transpose; Based on the uniformly accelerated motion model and the inertial measurement unit at the current moment... Corresponding vertical acceleration The current state vector is updated over time using the following formula; ; in, Indicates the current time The predicted height; Indicates the previous time before the current time. The corresponding height value; Indicates the previous time before the current time. The corresponding vertical velocity; Indicates the previous moment of the current moment. The time interval up to the current moment; Indicates the current time The corresponding predicted vertical velocity value; based on and Obtain the prior estimate of the state vector: = ; Calculate the prior error covariance matrix corresponding to the prior estimate using the following formula. ; ; in, Let represent the state transition matrix, and ; This represents the process noise covariance matrix.

[0028] The process noise covariance matrix described above is used to reflect accelerometer noise.

[0029] In this embodiment, the Kalman gain calculation is based on the prior covariance matrix. Calculate the Kalman gain using the adjusted observation noise covariance matrix R. ;

[0030] Here, H represents the observation matrix, and H=[1,0] represents the direct observation height.

[0031] In this embodiment, the prior estimate of the state vector based on the Kalman gain is fused and updated with altitude observations from a barometer or lidar to obtain a posterior estimate of the state vector including the fused altitude. Specifically, based on actual observations The prior estimate is corrected using altitude measurements from lidar or barometers to obtain... : If a barometer is used, it must be sampled before takeoff and the initial pressure deviation subtracted to ensure consistency with the lidar's zero point. Based on the above, a posterior state estimate including the fused altitude is obtained, and the error covariance matrix is ​​updated. ,in, This represents the identity matrix, used to prepare for the next iteration.

[0032] In this embodiment, real-time altitude typically refers to the raw measurement value from a single sensor (such as a lidar or barometer), which contains noise and may fluctuate. Fusion altitude specifically refers to the optimal estimate obtained after dynamically weighting and smoothing the data from the inertial measurement unit, barometer, and lidar using a Kalman filter algorithm. It is the core, stable state used within the system for control and further calculations. Desired altitude is the target altitude set by the user or provided by the upper-level planning module, serving as the input reference for the control system. The purpose of altitude hold control is to drive the UAV's fused altitude to track the desired altitude.

[0033] In this embodiment, in the vertical channel, the vertical velocity is an estimate corresponding to the fused height, directly output from the Kalman filter state vector. The desired vertical velocity is an intermediate command calculated by the outer loop (position loop) of the cascaded PID controller based on the height error, serving as the input target for the inner loop. The throttle control quantity is the final output command of the inner loop (velocity loop). In the horizontal channel, the horizontal velocity refers to the physical velocity calculated from the optical flow data after attitude compensation and scale transformation based on the fused height. The INS velocity estimate is the inertial navigation system's calculated velocity obtained through integration; the two are fused through complementary filtering to generate a corrected velocity used for positioning.

[0034] Example 3: Based on Example 2, this example provides a multi-source fusion method for UAV positioning using barometers, lidar, and optical flow sensors based on Kalman filtering. The method determines whether the lidar data is in a valid measurement state, including: Obtain the current measurement height and data quality flag bit output by the lidar; Determine whether the current measured height is less than a preset height threshold, and whether the data quality flag indicates normal operation; When the current measurement height is less than the preset height threshold and the data quality flag indicates normal operation, the lidar is determined to be in a valid measurement state. Otherwise, the lidar is determined not to be in an effective measurement state; Based on the judgment results, a dynamic adjustment strategy is generated for the values ​​of the observation noise covariance matrix of the Kalman filter.

[0035] In this embodiment, determining whether the lidar data is in a valid measurement state specifically includes: the system acquiring two key pieces of information output by the lidar (e.g., VL53L1X) in real time: the current measurement height value and the data quality flag. First, it determines whether the current measurement height is less than a preset height threshold, which is usually set according to the lidar's reliable range (e.g., 4 meters). Second, it determines whether the data quality flag inside the lidar indicates that the current measurement data is normal (e.g., the signal strength is sufficient, and there is no data jump due to ground material issues). Only when both of the above conditions are met simultaneously, i.e., "current measurement height < preset height threshold" and "data quality flag == normal", does the system determine that the lidar is in a valid measurement state. Otherwise (height exceeds the limit or data is abnormal), the lidar is determined not to be in a valid measurement state. This determination result is the direct basis for the subsequent dynamic adjustment of the Kalman filter parameters (Embodiment 4).

[0036] Example 4: Based on Example 3, this example provides a multi-source fusion UAV positioning method based on Kalman filtering, which integrates barometers, lidar, and optical flow sensors. It generates a dynamic adjustment strategy for the value of the Kalman filter observation noise covariance matrix based on the determination results, including: When the lidar is in an effective measurement state, the observation noise covariance matrix of the Kalman filter is adjusted to the first value; When the lidar is not in an effective measurement state, the observation noise covariance matrix is ​​adjusted to the second value; The second value is greater than the first value.

[0037] The working principle and beneficial effects of the above technical solution are as follows: Based on the judgment result, a dynamic adjustment strategy is generated for the value of the observation noise covariance matrix R of the Kalman filter. The observation noise covariance matrix R is a scalar or matrix, and its magnitude reflects the system's understanding of the current altitude observation value. The higher the R-value, the greater the trust level, and the greater the weight of the observation in the fusion process. The specific strategy is as follows: When the lidar is determined to be in an effective measurement state, indicating that the lidar altitude data is reliable and accurate, the R-matrix is ​​adjusted to its first value, a small number (e.g., 0.01), giving the lidar data a higher weight. When the lidar is determined to be not in an effective measurement state (e.g., exceeding its range), the system smoothly switches to relying on barometer data. In this case, the R-matrix is ​​adjusted to its second value, significantly greater than the first value (e.g., 0.5), indicating that the system has lower trust in barometer data, which is more susceptible to noise and drift, thus reducing its weight in the fusion process. This strategy of dynamically adjusting the R-value based on sensor status achieves a mathematically smooth transition from near-ground high-precision lidar measurement to high-altitude low-precision barometer measurement, effectively eliminating the "step effect" and oscillations in altitude control caused by traditional hard switching methods. The second value is greater than the first value.

[0038] Example 5: Based on Example 1, this example provides a multi-source fusion method for UAV positioning based on Kalman filtering, using a barometer, lidar, and optical flow sensor. In step 2, attitude compensation is performed on the pixel displacement measured by the optical flow sensor based on the attitude angle measured by the inertial measurement unit, including: The raw pixel displacement measured by the optical flow sensor is obtained, and the raw pixel displacement is low-pass filtered to obtain the filtered pixel displacement. Obtain the current pitch angle of the UAV as measured by the inertial measurement unit; Based on the current pitch angle and preset focal length related parameters, calculate the pixel displacement compensation amount caused by the change in UAV attitude; The pixel displacement compensation amount is removed from the filtered pixel displacement to obtain the pose-compensated net pixel displacement.

[0039] In this embodiment, the original pixel displacement is subjected to low-pass filtering, including: obtaining the original pixel displacement output by an optical flow sensor (such as PMW3901). , , representing the cumulative motion of pixels in the X (horizontal coordinate) and Y (vertical coordinate) directions, respectively; This represents the pixel index value; a first-order low-pass filter is applied to smooth the high-frequency noise inherent in the sensor. The filtering formula is: ; in, Indicates the pixel displacement after filtering; Indicates the current time; This represents the filter coefficient, with a value of 0.2. Indicates the previous moment; In this embodiment, attitude compensation is key to solving the "false displacement" of optical flow. When the drone tilts in place (e.g., a pitch change), the camera image shifts, which the optical flow sensor misinterprets as horizontal displacement. This invention utilizes the IMU attitude angle for reverse compensation: ;in, This represents a coefficient related to height and camera parameters (i.e., a preset focal length-related parameter). Indicates the filter coefficients; Indicates the current pitch angle of the drone; This indicates the result of reverse compensation; This represents the tangent function.

[0040] In this embodiment, the calculation of net pixel displacement includes: .

[0041] Example 6: Based on Example 1, this example provides a multi-source fusion method for UAV positioning using barometers, lidar, and optical flow sensors based on Kalman filtering. In step 2, the compensated pixel displacement is scaled according to the fusion height to obtain the horizontal velocity of the UAV, including: Based on the fusion height, the scale factor of the optical flow sensor is calculated, wherein the scale factor is directly proportional to the fusion height; Based on the scale factor, the pose-compensated pixel displacement is converted into the corresponding physical displacement; The horizontal velocity of the UAV is calculated based on the physical displacement and the corresponding displacement time.

[0042] In this embodiment, calculating the scale factor of the optical flow sensor based on the fusion height includes: obtaining the focal length parameter related to the optical flow sensor; calculating the actual physical distance corresponding to the unit pixel displacement based on the focal length parameter and the fusion height, and obtaining the scale factor.

[0043] The aforementioned scale factor (e.g., centimeters per impulse, CPI) is crucial for connecting the pixel domain and the physical domain. It is proportional to the flight altitude. The high-precision fused altitude obtained from the vertical channel Kalman filter is calculated in real-time: ;in, Indicates the scale factor; Indicates the aircraft's merging altitude; , , This represents the focal length parameter associated with the camera and optical flow sensor.

[0044] Example 7: Based on Example 1, this example provides a multi-source fusion method for UAV positioning using barometers, lidar, and optical flow sensors based on Kalman filtering, such as... Figure 3 As shown, in step 3, the horizontal velocity is compared and fused with the velocity estimated by the inertial navigation system to correct the horizontal position estimate of the inertial navigation system, thereby obtaining the UAV's positioning in an environment without a Global Positioning System (GPS) signal. This includes: S301: Obtain the velocity estimate and position estimate obtained by the inertial navigation system through integration calculation; S302: Calculate the speed error (i.e., comparison) between the horizontal speed and the speed estimate. S303: Based on the velocity error and according to complementary filtering logic, the velocity estimate and position estimate of the inertial navigation system are corrected to obtain the corrected horizontal position estimate; S304: Output the corrected horizontal position estimate and perform fixed-point control of the UAV based on the horizontal position estimate.

[0045] The working principle of the above technical solution is as follows: the speed error is calculated by comparing the horizontal speed with the speed estimate obtained by integration, and the speed and position estimates of the INS are corrected by complementary filtering logic, and finally used for fixed-point PID control.

[0046] Example 8: Based on Example 2, this example provides a multi-source fusion UAV positioning method based on Kalman filtering, using a barometer, lidar, and optical flow sensor. After determining the vertical velocity and fused altitude, it also includes altitude hold control, specifically: The fusion height and the vertical speed are input to a cascaded PID controller; Obtain the desired height, and calculate the desired vertical speed based on the height error between the desired height and the fused height using the outer loop of the cascaded PID controller; The inner loop of the cascaded PID controller calculates and outputs the throttle control quantity based on the speed error between the desired vertical speed and the vertical speed, and realizes constant altitude control based on the throttle control quantity.

[0047] The working principle of the above technical solution is as follows: after determining the vertical velocity and fusion height through Kalman filtering, it also includes a step of height control, specifically: inputting the fusion height and the vertical velocity into a cascaded PID controller. This controller includes an outer loop (position loop) and an inner loop (velocity loop).

[0048] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from the spirit and scope of this invention.

Claims

1. A multi-source fusion method for UAV positioning based on Kalman filtering, including barometers, lidar, and optical flow sensors, characterized in that... include: Step 1: Based on the Kalman filter, fuse the measurement data from the inertial measurement unit, barometer, and lidar to obtain the fused altitude of the UAV; Step 2: Based on the attitude angle measured by the inertial measurement unit, the pixel displacement measured by the optical flow sensor is attitude compensated, and the compensated pixel displacement is scaled according to the fusion height to obtain the horizontal velocity of the UAV. Step 3: Compare and fuse the horizontal velocity with the velocity estimated by the inertial navigation system, correct the horizontal position estimate of the inertial navigation system, and obtain the UAV's positioning in an environment without a global positioning system signal.

2. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 1, is characterized in that... In step 1, the fused altitude of the UAV is obtained based on the measurement data from the inertial measurement unit, barometer, and lidar, using a Kalman filter fusion method. This includes: Obtain the state vector of the Kalman filter, wherein the state vector includes at least the altitude and vertical velocity of the UAV; Based on the uniform acceleration motion model, and by updating the state vector over time according to the vertical acceleration measured by the inertial measurement unit, the prior estimate of the state vector and the corresponding prior error covariance matrix are obtained. Determine whether the lidar data is in a valid measurement state, and dynamically adjust the value of the observation noise covariance matrix of the Kalman filter based on the determination result; The Kalman gain is calculated based on the prior error covariance matrix and the adjusted observation noise covariance matrix. The prior estimate of the state vector based on the Kalman gain is fused and updated with the altitude observations from the barometer or lidar to obtain the posterior estimate of the state vector including the fused altitude.

3. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 2, is characterized in that... Determining whether the lidar data is in a valid measurement state includes: Obtain the current measurement height and data quality flag bit output by the lidar; Determine whether the current measured height is less than a preset height threshold, and whether the data quality flag indicates normal operation; When the current measurement height is less than the preset height threshold and the data quality flag indicates normal operation, the lidar is determined to be in a valid measurement state. Otherwise, the lidar is determined not to be in an effective measurement state; Based on the judgment results, a dynamic adjustment strategy is generated for the values ​​of the observation noise covariance matrix of the Kalman filter.

4. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 3, is characterized in that... Based on the judgment results, a dynamic adjustment strategy is generated for the values ​​of the Kalman filter observation noise covariance matrix, including: When the lidar is in an effective measurement state, the observation noise covariance matrix of the Kalman filter is adjusted to the first value; When the lidar is not in an effective measurement state, the observation noise covariance matrix is ​​adjusted to the second value; The second value is greater than the first value.

5. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 1, is characterized in that... In step 2, attitude compensation is performed on the pixel displacement measured by the optical flow sensor based on the attitude angle measured by the inertial measurement unit, including: The raw pixel displacement measured by the optical flow sensor is obtained, and the raw pixel displacement is low-pass filtered to obtain the filtered pixel displacement. Obtain the current pitch angle of the UAV as measured by the inertial measurement unit; Based on the current pitch angle and preset focal length related parameters, calculate the pixel displacement compensation amount caused by the change in UAV attitude; The pixel displacement compensation amount is removed from the filtered pixel displacement to obtain the pose-compensated net pixel displacement.

6. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 1, is characterized in that... In step 2, the compensated pixel displacement is scaled according to the fusion height to obtain the horizontal velocity of the drone, including: Based on the fusion height, the scale factor of the optical flow sensor is calculated, wherein the scale factor is directly proportional to the fusion height; Based on the scale factor, the pose-compensated pixel displacement is converted into the corresponding physical displacement; The horizontal velocity of the UAV is calculated based on the physical displacement and the corresponding displacement time.

7. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 6, is characterized in that... The scale factor of the optical flow sensor is calculated based on the fusion height, including: Obtain the focal length parameters associated with the optical flow sensor; Based on the focal length parameter and the fusion height, the actual physical distance corresponding to the unit pixel displacement is calculated to obtain the scale factor.

8. The UAV positioning method based on Kalman filtering, which integrates a barometer, lidar, and optical flow sensor, as described in claim 1, is characterized in that... In step 3, the horizontal velocity is compared and fused with the velocity estimated by the inertial navigation system to correct the horizontal position estimate of the inertial navigation system, thereby obtaining the UAV's positioning in an environment without a Global Positioning System (GPS) signal. This includes: Obtain the velocity and position estimates calculated by the inertial navigation system through integration; Calculate the speed error between the horizontal speed and the speed estimate; Based on the velocity error and by correcting the velocity and position estimates of the inertial navigation system according to complementary filtering logic, a corrected horizontal position estimate is obtained. The corrected horizontal position estimate is output, and the UAV is controlled in a fixed position based on the horizontal position estimate.

9. A multi-source fusion method for UAV positioning based on Kalman filtering, comprising barometer, lidar, and optical flow sensor, as described in claim 2, characterized in that... Based on a uniformly accelerated motion model, and by updating the state vector over time according to the vertical acceleration measured by the inertial measurement unit, a prior estimate of the state vector and the corresponding prior error covariance matrix are obtained, including: Get the current state vector ,in, Indicates the current time The corresponding state vector; Indicates the current time The height value; Show the current time The corresponding vertical velocity; Indicates transpose; Based on the uniformly accelerated motion model and the inertial measurement unit at the current moment... Corresponding vertical acceleration The current state vector is updated over time using the following formula; ； in, Indicates the current time The predicted height; Indicates the previous moment before the current moment. The corresponding height value; Indicates the previous time before the current time. The corresponding vertical velocity; Indicates the previous moment of the current moment. The time interval up to the current moment; Indicates the current time The corresponding predicted vertical velocity value; based on and Obtain the prior estimate of the state vector: = ; Calculate the prior error covariance matrix corresponding to the prior estimate using the following formula. ; ； in, Let represent the state transition matrix, and ; This represents the process noise covariance matrix.

10. A multi-source fusion method for UAV positioning based on Kalman filtering, comprising a barometer, lidar, and optical flow sensor, as described in claim 2, characterized in that... After determining the vertical velocity and fusion height, height control is also performed, specifically: The fusion height and the vertical speed are input to a cascaded PID controller; Obtain the desired height, and calculate the desired vertical speed based on the height error between the desired height and the fused height using the outer loop of the cascaded PID controller; The inner loop of the cascaded PID controller calculates and outputs the throttle control quantity based on the speed error between the desired vertical speed and the vertical speed, and realizes constant altitude control based on the throttle control quantity.

Images