Height calculation method and system for water-air amphibious unmanned aerial vehicle based on multi-sensor fusion

Abstract

The application discloses a height calculation method and system for water-air amphibious unmanned aerial vehicle based on multi-sensor fusion, and the method comprises the following steps: selecting sensors according to the working range of the water-air amphibious unmanned aerial vehicle; performing space-time calibration on each sensor to obtain a calibration reference surface of the sensor; obtaining the measured height of each sensor, calibrating the measured height based on the calibration reference surface to obtain a relative height; determining a fusion scheme according to the working state of each sensor; performing data fusion processing on the relative height based on the fusion scheme to obtain an accurate output height. The system comprises a waterproof pressure sensor module, a sonar distance sensor module, a laser distance sensor module, an accelerometer sensor module, an MCU module and a flight control module. By using the application, the advantages of different sensors are complemented, the accurate height calculation of the water-air amphibious unmanned aerial vehicle can be realized, and the stable movement and transition in the water-air two domains can be met. The application can be widely applied in the technical field of unmanned aerial vehicles.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) technology, and in particular to a method and system for calculating the altitude of amphibious UAVs based on multi-sensor fusion. Background Technology

[0002] A drone is an unmanned aerial vehicle controlled by radio remote control equipment or its own program control device. Currently, with the development of technology, drone technology is becoming increasingly mature, and drones are widely used due to their high speed and flexible operation. An amphibious drone is a type of drone capable of flying in the air and navigating in water; when entering water from the air, it needs to perform a rotor tilt change at the water-air boundary. Altitude information, as a crucial flight parameter, is essential for the safe and effective flight of drones and the safety of ground operators and command personnel.

[0003] Current UAVs primarily use a fusion of barometer and accelerometer data for altitude calculation. Barometers offer good dynamic characteristics and high long-term accuracy, while accelerometers offer good static characteristics and high short-term accuracy. Combining the advantages of both provides accurate altitude information. Currently, most mainstream flight controllers use built-in barometers and accelerometers and lack waterproofing. Amphibious UAVs require their flight controllers to be housed in a sealed enclosure. The built-in barometers in this enclosure cannot detect changes in external pressure and therefore cannot obtain accurate altitude information. While some methods use external waterproof pressure sensors to replace built-in barometers, the relative accuracy of waterproof pressure sensors differs significantly between air and water domains. Furthermore, near the ground and above the water surface, the ground effect causes large data fluctuations, resulting in inaccurate altitude information. The operational areas of amphibious UAVs impose limitations on numerous sensors; therefore, conventional UAV altitude calculation methods cannot be applied to amphibious UAVs. Summary of the Invention

[0004] To address the aforementioned technical problems, the objective of this invention is to provide a method and system for calculating the altitude of amphibious unmanned aerial vehicles (UAVs) based on multi-sensor fusion. By leveraging the complementary advantages of different sensors, this method enables accurate altitude calculation for amphibious UAVs, ensuring stable movement and transitions in both water and air domains.

[0005] The first technical solution adopted in this invention is: a method for calculating the altitude of an amphibious unmanned aerial vehicle based on multi-sensor fusion, comprising the following steps:

[0006] Select sensors based on the working range of the amphibious UAV;

[0007] Spatiotemporal calibration of each sensor is performed to obtain the calibration reference surface of the sensor;

[0008] The measured height of each sensor is obtained, and the measured height is calibrated based on the calibration reference to obtain the relative height;

[0009] The fusion scheme is determined based on the operating status of each sensor;

[0010] The relative height is fused using a fusion scheme to obtain an accurate output height.

[0011] Furthermore, the step of selecting sensors based on the operating range of the amphibious unmanned aerial vehicle specifically includes:

[0012] The working range of amphibious drones is divided into sub-ranges: air, above ground, above water, at the water-air boundary, and below water.

[0013] Sensors are selected based on each branch range, with at least two sensors operating in each branch range.

[0014] This optimization process reduces the difficulty of sensor selection by dividing the branch range and ensures the compatibility of the selected sensors. It is only necessary to ensure that the branch has high accuracy. The fact that at least two sensors are working within the branch further ensures the accuracy of the output fusion height.

[0015] Furthermore, the sensors include a waterproof pressure sensor, a sonar ranging sensor, a laser ranging sensor, and an accelerometer sensor.

[0016] This optimization process, by selecting different types of sensors, improves the robustness of height measurement and avoids height calculation errors caused by the unavailability of a single type of sensor in special circumstances.

[0017] Furthermore, the step of performing spatiotemporal calibration on each sensor to obtain the sensor calibration reference surface specifically includes:

[0018] The waterproof pressure sensor uses the takeoff ground at the initial moment as the reference plane;

[0019] When no horizontal movement is performed, the laser rangefinder selects the takeoff ground at the initial moment as the reference plane;

[0020] When moving horizontally, the laser rangefinder selects the output height measured at the previous moment as the reference plane.

[0021] The sonar ranging sensor selects the output height measured at the previous moment as the reference plane;

[0022] The accelerometer sensor uses the ground at the initial moment of takeoff as the reference plane.

[0023] This optimization step allows for the selection of a reference surface with the smallest error, tailored to the specific characteristics of the sensor, to calibrate the data obtained by the sensor.

[0024] Furthermore, the step of acquiring the measured height of each sensor, calibrating the measured height based on a calibration reference surface, and obtaining the relative height specifically includes:

[0025] The waterproof pressure sensor obtains the first relative height based on the difference between the measured height at the time of measurement and the height of the reference plane;

[0026] Without horizontal movement, the laser rangefinder obtains the second relative height based on the difference between the measured height at the time of measurement and the height of the reference plane;

[0027] When moving horizontally, the laser rangefinder adds the reference plane height to the difference between the measured height at the measurement moment and the measured height at the previous moment to obtain the third relative height;

[0028] The sonar ranging sensor adds the reference plane height to the difference between the measured height at the current measurement moment and the measured height at the previous moment to obtain the fourth relative height;

[0029] The accelerometer sensor obtains the relative acceleration in the Z-axis direction based on the difference between the Z-axis acceleration at the measurement moment and the Z-axis acceleration at the initial moment.

[0030] This optimized step calibrates the measured altitude based on the reference plane altitude, ensuring that the reference point for the relative altitude obtained by each sensor is consistent, which is the ground at the initial takeoff moment.

[0031] Furthermore, the fusion scheme specifically includes:

[0032] When the amphibious drone is in the air, the waterproof pressure sensor and the accelerometer sensor are in working condition, and the relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused and processed.

[0033] When the amphibious drone is above the ground, the waterproof pressure sensor, accelerometer sensor and laser rangefinder sensor are in working condition, and the relative height of the waterproof pressure sensor, the relative acceleration of the accelerometer sensor and the relative height of the laser rangefinder sensor are fused.

[0034] When the amphibious drone is above the water surface, the waterproof pressure sensor, accelerometer sensor and sonar ranging sensor are in working condition, and the relative height of the waterproof pressure sensor, the relative acceleration of the accelerometer sensor and the relative height of the sonar ranging sensor are fused.

[0035] When the amphibious drone is at the water-air boundary and has not entered the water, the sonar ranging sensor and the accelerometer sensor are in working condition, and the relative acceleration of the accelerometer sensor and the relative height of the sonar ranging sensor are fused.

[0036] When the amphibious drone is at the water-air boundary and has entered the water, the waterproof pressure sensor and the accelerometer sensor are in working condition, and the relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused.

[0037] When the amphibious drone is below the water surface, the waterproof pressure sensor and the sonar ranging sensor are in working order, and the relative height of the waterproof pressure sensor and the relative height of the sonar ranging sensor are fused together.

[0038] This optimization process establishes a comprehensive fusion scheme, preventing the introduction of invalid interference data during the fusion process.

[0039] Furthermore, the fusion process specifically includes:

[0040] When the amphibious drone is in the air, the relative altitude of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused by Kalman filtering to obtain an accurate output altitude.

[0041] When the amphibious drone is above the ground, the relative height of the waterproof pressure sensor and the relative height of the laser rangefinder sensor are fused by weighted average method to obtain the first fused height. The first fused height is then fused with the relative acceleration of the accelerometer sensor by Kalman filtering to obtain the accurate output height.

[0042] When the amphibious UAV is above the water surface, the relative height of the waterproof pressure sensor and the relative height of the sonar ranging sensor are fused by weighted average method to obtain a second fused height. The second fused height is then fused with the relative acceleration of the accelerometer sensor by Kalman filtering to obtain a precise output height.

[0043] When the amphibious UAV is at the water-air boundary, Kalman filtering is used to fuse the relative altitude from the sonar ranging sensor and the relative acceleration from the accelerometer to obtain a precise output altitude.

[0044] When the amphibious drone is at the water-air boundary, Kalman filtering is used to fuse the relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor to obtain an accurate output height.

[0045] When the amphibious UAV is below the water surface, a weighted average method is used to fuse the relative height of the waterproof pressure sensor and the relative height of the sonar ranging sensor to obtain a third fused height. The third fused height is then fused with the relative acceleration of the accelerometer sensor using Kalman filtering to obtain a precise output height.

[0046] Through this optimization step, a relatively high degree of fusion is achieved based on a combined fusion method of weighted average and Kalman filter fusion.

[0047] The second technical solution adopted in this invention is: an amphibious unmanned aerial vehicle altitude calculation system based on multi-sensor fusion, comprising:

[0048] Waterproof pressure sensor module: used to obtain the absolute height of the waterproof pressure sensor;

[0049] Sonar ranging sensor module: used to obtain the measured height of the sonar ranging sensor;

[0050] Laser rangefinder sensor module: used to obtain the measurement height of the laser rangefinder sensor;

[0051] Accelerometer sensor module: Used to acquire the measured acceleration from the accelerometer sensor;

[0052] MCU module: Used for high-precision calibration and data fusion of various modules;

[0053] Flight control module: Used for flight control of amphibious drones.

[0054] The beneficial effects of the method and system of this invention are as follows: This invention selects appropriate sensors within the working range of the amphibious UAV to obtain the measured altitude of each sensor, calibrates the measured altitude based on the calibration reference surface to obtain the relative altitude; it performs data fusion processing on the relative altitude based on a fusion scheme and a composite fusion method based on weighted average and Kalman filtering fusion methods to obtain an accurate output altitude; this invention integrates sensors such as waterproof pressure gauges, sonar ranging, laser ranging, and accelerometers, and by utilizing the complementary advantages of different sensors, it achieves accurate altitude calculation for the amphibious UAV, satisfying stable movement and transition in both water and air domains. Attached Figure Description

[0055] Figure 1 This is a flowchart of the steps of the multi-sensor fusion-based altitude calculation method for amphibious unmanned aerial vehicles (UAVs) of the present invention.

[0056] Figure 2 This is a system block diagram of the amphibious unmanned aerial vehicle altitude calculation system based on multi-sensor fusion of the present invention;

[0057] Figure 3This is a schematic diagram illustrating the working range of the multi-sensor fusion-based altitude calculation method for amphibious unmanned aerial vehicles (UAVs) of the present invention.

[0058] Figure 4 This is a schematic diagram of sensor installation for the multi-sensor fusion-based altitude calculation method for amphibious unmanned aerial vehicles (UAVs) according to the present invention.

[0059] Figure 5 This is a data fusion strategy diagram of the multi-sensor fusion-based amphibious unmanned aerial vehicle altitude calculation method of the present invention;

[0060] Figure 6 This is a Kalman filter fusion flowchart of the multi-sensor fusion-based altitude calculation method for amphibious unmanned aerial vehicles in this invention;

[0061] Figure 7 This is a data fusion flowchart of the multi-sensor fusion-based altitude calculation method for amphibious unmanned aerial vehicles (UAVs) according to the present invention;

[0062] Figure 8 This is a block diagram of a specific embodiment of the multi-sensor fusion-based amphibious unmanned aerial vehicle altitude calculation system of the present invention. Detailed Implementation

[0063] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only for ease of explanation and do not limit the order of the steps. The execution order of each step in the embodiments can be adapted according to the understanding of those skilled in the art.

[0064] Reference Figure 1 This invention provides a method for calculating the altitude of an amphibious unmanned aerial vehicle (UAV) based on multi-sensor fusion, the method comprising the following steps:

[0065] S1. Select appropriate sensors based on the working range of the amphibious UAV;

[0066] Reference Figure 3In a specific embodiment of the present invention, the amphibious unmanned aerial vehicle (UAV) operates within a range of 0–500 m above water and 0–10 m underwater. The sensors used in this specific embodiment include: a high-precision waterproof pressure sensor M (MS5803-01BA) with an operating range of 10–1300 hPa (absolute altitude 44216–-2.87 m); and a high-precision waterproof pressure sensor W (WF5803F-02Bar) with an operating range of 1… 0~2000hPa (absolute altitude 44216~-9.87m); Sonar ranging sensor S, using P30 ranging sonar, underwater working range is 0.5~50m from the bottom, above water working range is 0.2~1.2m from the surface; Laser ranging sensor L, using VL53-400 laser ranging, working range is 0.04~4m from the ground; Accelerometer sensor A, using BMI088 inertial measurement unit, working range is ±24g.

[0067] Based on the actual working conditions of the selected sensors, the working range of the amphibious UAV is divided into 0-500m above water and 0-10m underwater. Specifically, the working range of the amphibious UAV is further divided into sub-ranges such as air, above ground, above water, and below water. Specifically, air corresponds to a working range of 500m-1.2m above water and 500m-4m above ground; above ground corresponds to 4m-0m above ground; above water corresponds to 0.2m-1.2m above water; the water-air boundary corresponds to 0m-0.2m above water and 0m-0.2m below water; and below water corresponds to 0.2m-10m below water. In this specific embodiment, it is assumed that the absolute height of the ground is greater than the absolute height of the water surface.

[0068] Reference Figure 4In this invention, the waterproof pressure sensor is installed on the tripod of the amphibious UAV, and the sonar ranging sensor is installed higher than the waterproof pressure sensor. The vertical distance between the two is not less than the dead zone of the sonar ranging sensor on the water. The dead zone of the P30 ranging sonar on the water is 0-0.2m from the water surface. Therefore, the vertical distance L1 is set to 0.2m. When the amphibious UAV enters the water, the waterproof pressure sensor data will change abruptly. This design ensures that when the waterproof pressure sensor data changes abruptly, the sonar ranging sensor remains within its operating range. After the waterproof pressure sensor enters the water, the data recovers. The system stabilizes, but at this time the sonar ranging sensor is within the dead zone. The nodes where the waterproof pressure sensor data changes abruptly are offset from the dead zone of the sonar ranging sensor. The vertical distance between the installation position of the laser ranging sensor and the landing gear of the amphibious drone is not less than the dead zone of the laser ranging sensor on the ground. The dead zone of the laser ranging sensor is 0 to 0.04m from the ground. Therefore, the vertical distance L2 is set to 0.04m. This design ensures that the drone is in working condition before takeoff, rather than after takeoff when it exceeds the dead zone. It also ensures that the amphibious drone will not enter the dead zone when it is working normally.

[0069] S2. Perform spatiotemporal calibration on each sensor to obtain the calibration reference surface of the sensor;

[0070] The data acquired by the relatively high-precision waterproof pressure sensor M in the air and the relatively high-precision waterproof pressure sensor W in the water are pressure values, which vary little with terrain factors. The ground at the initial takeoff moment is selected as the reference plane.

[0071] The data obtained by the laser rangefinder L is the height value relative to the terrain. Changes in the ground terrain will cause abrupt changes in the data obtained by the L sensor. During the flight control self-test of the amphibious UAV, the initial takeoff ground is selected as the reference plane. When the amphibious UAV is not moving horizontally, the initial takeoff ground is selected as the reference plane, but this situation is difficult to occur in actual application scenarios. During the horizontal movement of the amphibious UAV on the ground, the data obtained by the sensor will change abruptly due to the change in terrain. Therefore, it is necessary to re-establish the reference plane to calibrate the relative height obtained by the sensor. Selecting the output height measured at the previous moment as the reference plane can improve the height deviation caused by terrain factors.

[0072] The data acquired by accelerometer sensor A is the acceleration during the flight of the amphibious UAV, which is only affected by the flight state of the amphibious UAV, and the ground at the initial moment of takeoff is selected as the reference plane.

[0073] The data obtained by the sonar ranging sensor S is the height value and confidence level relative to the terrain, which is greatly affected by the terrain. Therefore, the output height measured at the previous moment is selected as the reference plane.

[0074] S3. Obtain the measurement height of each sensor, calibrate the measurement height based on the calibration reference, and obtain the relative height;

[0075] Reference Figure 5 The data acquired by the relatively high-precision waterproof pressure sensor M in the air and the relatively high-precision waterproof pressure sensor W in the water are pressure values. These pressure values ​​need to be converted to obtain the corresponding absolute height values. The conversion formula is as follows:

[0076] While in the air:

[0077]

[0078] When in water:

[0079]

[0080] Where p is the pressure value, p 水 h is the water pressure value, and h is the absolute height value. 水 This represents the absolute height of the water surface.

[0081] Kalman filtering was applied to the absolute height values ​​converted from those of waterproof pressure sensor M and waterproof pressure sensor W, respectively.

[0082] Using the initial takeoff ground as the reference plane, the waterproof pressure sensor M and waterproof pressure sensor W are calibrated during the flight control self-test of the amphibious UAV. The calibration formula is as follows:

[0083] H m，k =h m，k -h m，0

[0084] H w，k =h w，k -h w，0

[0085] Among them, h m，0 and h w，0 The absolute heights h of the waterproof pressure sensor M and the waterproof pressure sensor W at the initial moment are respectively. m，k and h w，k H represents the absolute height of waterproof pressure sensor M and waterproof pressure sensor W at time k, respectively. m，k and H w，k These represent the relative heights of waterproof pressure sensor M and waterproof pressure sensor W at time k.

[0086] The data obtained by the laser rangefinder L is the altitude value relative to the terrain. Taking the initial takeoff ground as the reference plane, the laser rangefinder L is calibrated during the flight control self-test of the amphibious UAV. The calibration formula is as follows:

[0087]

[0088] Among them, h l，0 The relative height of the laser rangefinder L at the initial moment. To measure h l，0 Flight control tilt angle, h l，k Let L be the measured height of the laser rangefinder at time k. To measure h l，k Flight control tilt angle, H l，k Let L be the relative height of the laser rangefinder sensor after calibration at time k.

[0089] During the horizontal movement of an amphibious unmanned aerial vehicle (UAV) on the ground, the data obtained by the laser rangefinder L undergoes a sudden change. The mathematical representation of the basis for this judgment is as follows:

[0090] |Δh l，k |>θ l

[0091] |H l，k -H out，k |>β l

[0092] Where, Δh l，k Let θ be the relative height difference between the laser rangefinder L at time k and time k-1. l β is the threshold for data mutation in the laser rangefinder L. l For H l，k With H out，k The deviation threshold, H out，k Let k be the output altitude of the amphibious drone.

[0093] The laser rangefinder L is recalibrated using the following formula:

[0094]

[0095] Among them, h l,k Let L be the measured height of the laser rangefinder at time k. To measure h l,k Flight control tilt angle, H l,k Let h be the relative height of the laser rangefinder sensor L after calibration at time k. l,k-1 Let L be the measured height of the laser rangefinder at time k-1. To measure h l,k-1 Flight control tilt angle, Hout,k-1 The output altitude of the amphibious drone at time k-1 is the output altitude of the drone.

[0096] The sonar ranging sensor S obtains data in terms of relative terrain height and confidence level. It is calibrated when the confidence level of sonar ranging sensor S is greater than 90%, using the following calibration formula:

[0097] When above the water surface:

[0098]

[0099] Among them, h s，k Let k be the measured height of the sonar ranging sensor S at time k. To measure h s，k Flight control tilt angle, H out，k-1 Output the height H at time k-1. s，k Let S be the relative height of the sonar ranging sensor S after calibration at time k.

[0100] At this moment, the relative height of the water surface with respect to the ground at the initial takeoff point is:

[0101] H 水 =L1-(h s，k -H out，k )

[0102] When below the water surface:

[0103]

[0104] Among them, h s，k-1 h represents the output altitude of the amphibious drone at time k-1. s，k Let k be the measured height of the sonar ranging sensor S at time k. To measure h s，k Flight control tilt angle, H s，k Let S be the relative height of the sonar ranging sensor S after calibration at time k.

[0105] Accelerometer sensor A acquires acceleration data. Using the initial takeoff ground as the reference plane, accelerometer sensor A undergoes zero-bias calibration during the flight control self-test of the amphibious UAV. The calibration formula is as follows:

[0106] A z，k =a z，k -a z，0

[0107] Among them, a z，0 Let a be the initial acceleration of accelerometer sensor A in the Z-axis direction. z，k Let A be the measured acceleration of accelerometer sensor A in the Z-axis direction at time k. z，kLet k be the relative acceleration of accelerometer sensor A after calibration in the Z-axis direction at time k.

[0108] S4. Determine the fusion scheme based on the working status of each sensor;

[0109] When the amphibious UAV is at a height of 500m to 1.2m above water and 500m to 4m on land, the waterproof pressure sensor M, waterproof pressure sensor W, and accelerometer sensor A are in working condition, while the laser rangefinder L and sonar rangefinder S are outside their working range. The relative height of the waterproof pressure sensor M, the relative height of the waterproof pressure sensor W, and the relative acceleration of the accelerometer sensor A are fused together.

[0110] When the amphibious drone is at a height of 4m to 0m on the ground, the waterproof pressure sensor M, waterproof pressure sensor W, accelerometer sensor A and laser rangefinder L are in working condition, while the sonar rangefinder S is outside its working range. The relative height of the waterproof pressure sensor M, the relative height of the waterproof pressure sensor W, the relative acceleration of the accelerometer sensor A and the relative height of the laser rangefinder L are fused.

[0111] When the amphibious drone is 0.2m to 1.2m above the water, the waterproof pressure sensor M, waterproof pressure sensor W, accelerometer sensor A, and sonar ranging sensor L are in working condition. The laser ranging sensor L fails because the amphibious drone moves from the ground to the water surface. The relative height of the waterproof pressure sensor M, the relative height of the waterproof pressure sensor W, the relative acceleration of the accelerometer sensor A, and the relative height of the sonar ranging sensor L are fused.

[0112] When the amphibious UAV is at the water-air boundary, between 0m and 0.2m above the water, the accelerometer sensor A and the sonar ranging sensor S are in working condition. The data from the waterproof pressure sensor fluctuates greatly and cannot be used for data fusion. The sonar ranging sensor S is within its working range, and the relative height of the sonar relative to the ranging sensor and the relative acceleration of the accelerometer sensor are fused.

[0113] When the amphibious UAV is at the water-air boundary, at a depth of 0m to 0.2m, the accelerometer sensor A and the waterproof pressure sensor are in working condition. The entry of the waterproof pressure sensor into the water surface is determined by the sudden change in the data of the waterproof pressure sensor. The data of the waterproof pressure sensor is stable in the water and can be used for data fusion. The sonar ranging sensor S has not entered the water surface and is within the dead zone. The relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused.

[0114] When the amphibious UAV is underwater at a depth of 0.2m to 2.87m, the waterproof pressure sensor M, waterproof pressure sensor W, accelerometer sensor A, and sonar ranging sensor S are in working condition. The laser ranging sensor L fails due to the refraction and scattering of most of the laser light in the water. The relative heights of the waterproof pressure sensor M, the waterproof pressure sensor W, the accelerometer sensor A, and the sonar ranging sensor S are fused together.

[0115] When the amphibious drone is 2.87m-10m below the water surface, the waterproof pressure sensor W, accelerometer sensor A, and sonar ranging sensor S are in working condition, while the waterproof pressure sensor M and laser ranging sensor L are outside their working range. The relative height of the waterproof pressure sensor W, the relative acceleration of the accelerometer sensor A, and the relative height of the sonar ranging sensor S are fused.

[0116] S5. Based on the fusion scheme, the relative height is processed by data fusion to obtain an accurate output height.

[0117] When the amphibious UAV is at a height of 500m to 1.2m above water and 500m to 4m on land, a weighted average method is used to fuse the relative heights of the waterproof pressure sensor M and the waterproof pressure sensor W to obtain the fused relative heights of the two sensors. The fusion formula is as follows:

[0118]

[0119] Among them, H fusion，k H represents the fusion height at time k. mw，k H represents the fusion height of waterproof pressure sensor M and waterproof pressure sensor W at time k. m，k H represents the relative height of the waterproof pressure sensor M at time k. w，k S represents the relative height of the waterproof pressure sensor W at time k. m 2 H represents m，k The variance, S w 2 H represents w，k The variance.

[0120] Next, a Kalman filter is used to fuse the relative height of the waterproof pressure sensor M and the waterproof pressure sensor W with the relative acceleration of the accelerometer sensor A to obtain an accurate output height.

[0121] When the amphibious UAV is positioned between 4m and 0m above ground, a weighted average method is used to fuse the relative heights of the waterproof pressure sensor M and the waterproof pressure sensor W to obtain the fused relative height of the two sensors. Then, a weighted average method is used to fuse the fused relative heights of the two sensors and the relative height of the laser rangefinder L to obtain the fused heights of the three sensors. The fusion formula is as follows:

[0122]

[0123] Among them, H mwl，k H represents the fusion height of the waterproof pressure sensor M, waterproof pressure sensor W, and laser rangefinder L at time k. l，k H represents the relative height of the laser rangefinder L at time k. mw，k This represents the fused relative height of the waterproof pressure sensor M and the waterproof pressure sensor W at time k. The maximum tilt angle set in the flight controller. Indicates the measured H l，k The tilt angle of the flight control at that time.

[0124] Next, a Kalman filter is used to fuse the fusion height of the waterproof pressure sensor M, the waterproof pressure sensor W, and the laser rangefinder sensor with the relative acceleration of the accelerometer sensor A to obtain a precise output height.

[0125] When the amphibious UAV is positioned 0.2m to 1.2m above the water, a weighted average method is used to fuse the relative heights of the waterproof pressure sensor M and the waterproof pressure sensor W to obtain the fused relative height of the two sensors. Then, a weighted average method is used to fuse the fused relative heights of the two sensors and the relative height of the sonar ranging sensor S to obtain the fused heights of the three sensors. The fusion formula is as follows:

[0126]

[0127] Among them, H mws，k H represents the fused height of the waterproof pressure sensor M, waterproof pressure sensor W, and sonar ranging sensor S at time k. mw，k This represents the fused relative height of the waterproof pressure sensor M and the waterproof pressure sensor W at time k. H represents s，k variance H represents mw，k The variance, η kThis represents the confidence level of sensor S at time k.

[0128] Next, a Kalman filter is used to fuse the fused heights of the waterproof pressure sensor M, the waterproof pressure sensor W, and the sonar ranging sensor S with the relative acceleration of the accelerometer sensor A to obtain a precise output height.

[0129] When an amphibious UAV is at the critical point between water and air, with a depth of 0m to 0.2m above the water, the data from the waterproof pressure sensor fluctuates greatly and cannot be used for data fusion. At this time, the sonar ranging sensor S is within its operating range, and the data confidence level is greater than 90%, making it suitable for data fusion. The fusion formula is as follows:

[0130] H fusion，k =h s，k +H 水

[0131] Among them, h s，k Let H be the relative height of the sonar ranging sensor S at time k. 水 This represents the relative height between the water surface and the takeoff ground at the initial moment.

[0132] Next, a Kalman filter is used to fuse the fused height of the sonar ranging sensor S and the relative acceleration of the accelerometer sensor A to obtain a precise output height.

[0133] When an amphibious UAV is at the water-air boundary (0m-0.2m underwater), a sudden change in the water pressure sensor data indicates that the UAV has entered the water surface. The water pressure sensor data is stable underwater and can be used for data fusion. At this point, the sonar ranging sensor S is not in the water surface and is within the dead zone; its data confidence level is below 90%, and it cannot be used for data fusion. The fusion formula is as follows:

[0134]

[0135] Among them, H mw，k H represents the fused relative height of waterproof pressure sensor M and waterproof pressure sensor W at time k. m，k H represents the relative height of the waterproof pressure sensor M at time k. w，k S represents the relative height of the waterproof pressure sensor W at time k. m 2 H represents m，k The variance, S w 2 H represents w，k The variance.

[0136] Next, a Kalman filter is used to fuse the relative height of the waterproof pressure sensor M and the waterproof pressure sensor W with the relative acceleration of the accelerometer sensor A to obtain an accurate output height.

[0137] When the amphibious UAV is submerged at a depth of 0.2m to 2.87m, a weighted average method is used to fuse the relative heights of the waterproof pressure sensor M and the waterproof pressure sensor W to obtain the fused relative height of the two sensors. Then, a weighted average method is used to fuse the fused relative heights of the two sensors and the relative height of the sonar ranging sensor S to obtain the fused heights of the three sensors. The fusion formula is as follows:

[0138]

[0139] Among them, H mws，k H represents the fused height of the waterproof pressure sensor M, waterproof pressure sensor W, and sonar ranging sensor S at time k. mw，k This represents the fused relative height of the waterproof pressure sensor M and the waterproof pressure sensor W at time k. H represents s，k variance H represents mw，k The variance, η k This represents the confidence level of sensor S at time k.

[0140] Next, a Kalman filter is used to fuse the fusion height of the waterproof pressure sensor M, the waterproof pressure sensor W, and the sonar ranging sensor S with the relative acceleration of the accelerometer sensor A to obtain a precise output height.

[0141] When the amphibious UAV is submerged at a depth of 2.87m to 10m, the relative heights of the waterproof pressure sensor W and the sonar ranging sensor S are fused using a weighted average method to obtain the fused height of the waterproof pressure sensor W and the sonar ranging sensor S. The fusion formula is as follows:

[0142]

[0143] Among them, H ws，k H represents the fused height of the waterproof pressure sensor W and the sonar ranging sensor S at time k. s，k H represents the relative height of the sonar ranging sensor S at time k. w，k This represents the relative height of the waterproof pressure sensor W at time k. H represents w，k variance H represents s，k The variance, η k This represents the confidence level of sensor S at time k.

[0144] Next, a Kalman filter is used to fuse the height data of the waterproof pressure sensor W and the sonar ranging sensor S with the relative acceleration data of the accelerometer sensor A to obtain a precise output height.

[0145] fusion height H fusion，k The relative acceleration A after calibration with acceleration sensor A z，k Perform data fusion.

[0146] The state equation for data fusion is as follows:

[0147] X(k) = AX(k-1) + BU(k) + W(k)

[0148] in, U(k)=A z，k W(k) represents the process noise, and dt represents the time interval.

[0149] The measurement equation for data fusion is as follows:

[0150] Z(k) = HX(k) + V(K)

[0151] Where Z(k)=H fusion，k H = [1 0], V(K) is the measurement noise.

[0152] refer to Figure 6 The Kalman filter fusion formula is as follows:

[0153] X(k|k-1)=AX(k-1|k-1)+BU(k)

[0154] P(k|k-1)=AP(k-1|k-1)A T +Q

[0155]

[0156] X(k|k)=X(k|k-1)+K(k)(Z(k)-HX(k|k-1))

[0157] P(k|k)=(IK(k)H)P(k|k-1)

[0158] in, K(k) is the Kalman gain, P(k|k) is the covariance, Q is the process noise, and R is the measurement noise.

[0159] The accurate output height H is obtained by Kalman filtering fusion. out，k =h(k|k).

[0160] Reference Figure 7The actual data fusion process in this specific embodiment of the invention is based on the working state of the sensors to determine whether data fusion should be performed. The above-described fusion scheme and method are for ease of understanding and are based on the assumption that the amphibious drone operates within its operating range. In the actual operation of the amphibious drone, since both the waterproof pressure sensor W and the accelerometer sensor A are in working condition within the drone's operating range, there is no need to detect their working states. First, the working state of the waterproof pressure sensor M is detected. Since the working range of the laser rangefinder L is included within the working range of the waterproof pressure sensor M, after detecting that the waterproof pressure sensor M is in working condition, it is calibrated and then fused with the waterproof pressure sensor W. Next, the working state of the laser rangefinder L is detected. If the laser rangefinder L is in working condition, it can be determined that the amphibious drone is above the ground. The ranging sensor L is calibrated and added to the data fusion queue. At this point, it is determined whether the amphibious UAV is underwater. This determination is based on the sudden changes in data obtained from the waterproof pressure sensor M, waterproof pressure sensor W, and sonar ranging sensor S. This phenomenon is used to determine the UAV's entry and exit from the water. If it is determined to be in the air, the relative acceleration from the accelerometer sensor A is fused to output a precise altitude. If the waterproof pressure sensor M is not in operation, the operating status of the sonar ranging sensor S is checked, as the operating ranges of these two sensors only partially overlap. If the sonar ranging sensor S is in operation, its altitude is calibrated, and data fusion is performed. If the sonar ranging sensor S is not in operation, the relative acceleration from the accelerometer sensor A is then fused. However, within the operating range of the amphibious UAV in this specific embodiment of the invention, both the sonar ranging sensor S and the waterproof pressure sensor M will not be inactive. When the waterproof pressure sensor M is in working condition, but the laser rangefinder L is not in working condition, the working condition of the sonar rangefinder S is detected instead. If the sonar rangefinder S is in working condition, it can be preliminarily determined that the amphibious UAV is currently 0.2m to 2.87m below the surface or 0.2m to 1.2m above the water. Then, the relative acceleration of the accelerometer sensor A is fused to output the accurate altitude.

[0161] like Figure 2 As shown, the amphibious unmanned aerial vehicle (UAV) altitude calculation system based on multi-sensor fusion includes:

[0162] Waterproof pressure sensor module: used to obtain the absolute height of the waterproof pressure sensor;

[0163] Sonar ranging sensor module: used to obtain the measured height of the sonar ranging sensor;

[0164] Laser rangefinder sensor module: used to obtain the measurement height of the laser rangefinder sensor;

[0165] Accelerometer sensor module: Used to acquire the measured acceleration from the accelerometer sensor;

[0166] MCU module: used for calibration and data fusion of various modules;

[0167] Flight control module: Used for flight control of amphibious drones.

[0168] Specific implementations of the system of the present invention, for example Figure 8 As shown, the waterproof pressure sensor module is divided into a high-precision waterproof pressure sensor module for air and a high-precision waterproof pressure sensor module for water. This ensures that a waterproof pressure sensor is in a high-precision state within the working range of the amphibious UAV. In addition, the accelerometer sensor module is installed in the flight control module. The high-precision waterproof pressure sensor modules for air, water, sonar ranging, and laser ranging are not located in the flight control module because the flight control is a sealed environment. The high-precision waterproof pressure sensor modules for air, water, sonar ranging, and laser ranging need to sense and detect the external environment to obtain sensor data, while the accelerometer sensor module can directly obtain acceleration data from the flight control.

[0169] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0170] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.

Claims

1. A method for calculating the altitude of an amphibious unmanned aerial vehicle based on multi-sensor fusion, characterized in that, Includes the following steps: Select sensors based on the working range of the amphibious UAV; Spatiotemporal calibration of each sensor is performed to obtain the calibration reference surface of the sensor; The measured height of each sensor is obtained, and the measured height is calibrated based on the calibration reference to obtain the relative height; The fusion scheme is determined based on the operating status of each sensor; Based on the fusion scheme, the relative height is fused to obtain an accurate output height; The step of selecting sensors based on the operating range of the amphibious unmanned aerial vehicle specifically includes: The working range of amphibious drones is divided into sub-ranges: air, above ground, above water, at the water-air boundary, and below water. Sensors are selected based on each branch range, with at least two sensors operating in each branch range; The sensors include a waterproof pressure sensor, a sonar ranging sensor, a laser ranging sensor, and an accelerometer sensor; The step of obtaining the measured height of each sensor, calibrating the measured height based on the calibration reference surface, and obtaining the relative height specifically includes: The waterproof pressure sensor obtains the first relative height based on the difference between the measured height at the time of measurement and the height of the reference plane; Without horizontal movement, the laser rangefinder obtains the second relative height based on the difference between the measured height at the time of measurement and the height of the reference plane; When moving horizontally, the laser rangefinder adds the reference plane height to the difference between the measured height at the measurement moment and the measured height at the previous moment to obtain the third relative height; The sonar ranging sensor adds the reference plane height to the difference between the measured height at the current measurement moment and the measured height at the previous moment to obtain the fourth relative height; The accelerometer sensor obtains the relative acceleration in the Z-axis direction based on the difference between the Z-axis acceleration at the measurement moment and the Z-axis acceleration at the initial moment; The fusion scheme is determined based on the operating status of each sensor, including: When the amphibious drone is in the air, the waterproof pressure sensor and the accelerometer sensor are in working condition, and the relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused and processed. When the amphibious drone is above the ground, the waterproof pressure sensor, accelerometer sensor and laser rangefinder sensor are in working condition, and the relative height of the waterproof pressure sensor, the relative acceleration of the accelerometer sensor and the relative height of the laser rangefinder sensor are fused. When the amphibious drone is above the water surface, the waterproof pressure sensor, accelerometer sensor and sonar ranging sensor are in working condition, and the relative height of the waterproof pressure sensor, the relative acceleration of the accelerometer sensor and the relative height of the sonar ranging sensor are fused. When the amphibious drone is at the water-air boundary and has not entered the water, the sonar ranging sensor and the accelerometer sensor are in working condition, and the relative acceleration of the accelerometer sensor and the relative height of the sonar ranging sensor are fused. When the amphibious drone is at the water-air boundary and has entered the water, the waterproof pressure sensor and the accelerometer sensor are in working condition, and the relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused. When the amphibious drone is below the water surface, the waterproof pressure sensor, accelerometer sensor, and sonar ranging sensor are in working order, and the relative height of the waterproof pressure sensor, the relative acceleration of the accelerometer sensor, and the relative height of the sonar ranging sensor are fused together.

2. The method for calculating the altitude of an amphibious unmanned aerial vehicle based on multi-sensor fusion according to claim 1, characterized in that, The step of performing spatiotemporal calibration on each sensor to obtain the calibration reference surface of the sensor specifically includes: The waterproof pressure sensor uses the takeoff ground at the initial moment as the reference plane; When no horizontal movement is performed, the laser rangefinder selects the takeoff ground at the initial moment as the reference plane; When moving horizontally, the laser rangefinder selects the output height measured at the previous moment as the reference plane. The sonar ranging sensor selects the output height measured at the previous moment as the reference plane; The accelerometer sensor uses the ground at the initial moment of takeoff as the reference plane.

3. The method for calculating the altitude of an amphibious unmanned aerial vehicle based on multi-sensor fusion according to claim 1, characterized in that, The fusion process specifically includes: When the amphibious drone is in the air, the relative altitude of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor are fused by Kalman filtering to obtain an accurate output altitude. When the amphibious drone is above the ground, the relative height of the waterproof pressure sensor and the relative height of the laser rangefinder sensor are fused by weighted average method to obtain the first fused height. The first fused height is then fused with the relative acceleration of the accelerometer sensor by Kalman filtering to obtain the accurate output height. When the amphibious UAV is above the water surface, the relative height of the waterproof pressure sensor and the relative height of the sonar ranging sensor are fused by weighted average method to obtain a second fused height. The second fused height is then fused with the relative acceleration of the accelerometer sensor by Kalman filtering to obtain a precise output height. When the amphibious UAV is at the water-air boundary, Kalman filtering is used to fuse the relative altitude from the sonar ranging sensor and the relative acceleration from the accelerometer to obtain a precise output altitude. When the amphibious drone is at the water-air boundary, Kalman filtering is used to fuse the relative height of the waterproof pressure sensor and the relative acceleration of the accelerometer sensor to obtain an accurate output height. When the amphibious UAV is below the water surface, a weighted average method is used to fuse the relative height of the waterproof pressure sensor and the relative height of the sonar ranging sensor to obtain a third fused height. The third fused height is then fused with the relative acceleration of the accelerometer sensor using Kalman filtering to obtain a precise output height.

4. A multi-sensor fusion-based altitude calculation system for amphibious unmanned aerial vehicles (UAVs), characterized in that: The method applied to any one of claims 1-3 includes: Waterproof pressure sensor module: used to obtain the absolute height of the waterproof pressure sensor; Sonar ranging sensor module: used to obtain the measured height of the sonar ranging sensor; Laser rangefinder sensor module: used to obtain the measurement height of the laser rangefinder sensor; Accelerometer sensor module: Used to acquire the measured acceleration from the accelerometer sensor; MCU module: Used for high-precision calibration and data fusion of various modules; Flight control module: Used for flight control of amphibious drones.

Images