Multi-sensor fusion positioning method of water-air amphibious unmanned aerial vehicle

Abstract

The invention relates to the technical field of unmanned aerial vehicle positioning, in particular to a multi-sensor fusion positioning method for a water-air amphibious unmanned aerial vehicle, which comprises the following steps: when the water-air amphibious unmanned aerial vehicle is underwater, estimating the underwater position of the water-air amphibious unmanned aerial vehicle based on a Doppler velocimeter and an inertial measurement unit to obtain preliminary positioning information; when the unmanned aerial vehicle is in the air, estimating the air position of the water-air amphibious unmanned aerial vehicle based on a real-time dynamic measurement method to obtain observation information; when the unmanned aerial vehicle enters underwater from the air, the underwater working environment of the water-air amphibious unmanned aerial vehicle is scanned based on a mechanical scanning sonar to obtain underwater point cloud data; and taking the initial positioning information and the observation information as nodes of a factor graph, and solving the factor graph according to the underwater point cloud data to obtain optimal position estimation information of the water-air amphibious unmanned aerial vehicle. According to the invention, the underwater positioning precision of the water-air amphibious unmanned aerial vehicle is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) positioning, and in particular to a multi-sensor fusion positioning method for an amphibious aerial vehicle (AAV). Background Art

[0002] Against the backdrop of today's rapidly advancing science and technology, robots are playing an increasingly important role in fields such as ocean exploration, resource development, and environmental monitoring. This is particularly true in specialized work scenarios, such as bridge inspection and reservoir inspection, where unmanned robots are required to perform cross-media tasks. In this context, amphibious drones, with their unique ability to operate in diverse environments, are demonstrating their significant value.

[0003] Traditional drone positioning methods are relatively mature. For example, outdoor drones typically use GPS for absolute positioning, while indoors, cameras or lidar sensors are often used for precise positioning. However, amphibious drones face numerous challenges when performing underwater operations, particularly the severe attenuation of GPS signals and signal interruption at certain depths.

[0004] Currently, long-term underwater positioning technologies primarily rely on collaboration with surface vessels or buoys, establishing a local information network and utilizing long / short baseline methods to simulate outdoor aerial GNSS principles to calculate the robot's underwater position. However, these methods typically require specific environments to be pre-configured and have limited adaptability, making them unsuitable for lightweight, amphibious drones. Furthermore, existing technologies often develop underwater and aerial positioning methods independently, preventing effective integration. Summary of the Invention

[0005] The present invention aims to solve the problem that existing amphibious unmanned aerial vehicles (UAVs) cannot effectively combine positioning information of different positions during positioning.

[0006] To solve the above technical problems, the present invention provides a multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle, comprising the following steps:

[0007] S1. When the amphibious drone is underwater, estimating the underwater position of the amphibious drone based on a Doppler velocimeter and an inertial measurement unit to obtain preliminary positioning information;

[0008] S2. When the amphibious drone is in the air, estimating the aerial position of the amphibious drone based on a real-time dynamic measurement method to obtain observation information;

[0009] S3. When the amphibious drone enters the water from the air, scanning the underwater working environment of the amphibious drone based on a mechanical scanning sonar to obtain underwater point cloud data;

[0010] S4. Using the preliminary positioning information and the observation information as nodes of a factor graph, and solving the factor graph according to the underwater point cloud data, to obtain optimal position estimation information of the water-air amphibious drone.

[0011] Furthermore, step S1 is specifically as follows:

[0012] Based on the Doppler velocimeter, the speed data of the water-air amphibious drone in the DVL coordinate system is obtained, and based on the inertial measurement unit, the acceleration data and angular velocity data of the water-air amphibious drone in the IMU coordinate system are obtained. The speed data, the acceleration data, and the angular velocity data are processed using an extended Kalman filter unit to obtain the preliminary positioning information.

[0013] Furthermore, the inertial measurement unit calculates a posture matrix T based on the acceleration data and the angular velocity data acquired, and applies the posture matrix T to the step of acquiring velocity data by the Doppler velocimeter.

[0014] Furthermore, step S3 includes the following sub-steps:

[0015] Scanning the underwater working environment of the amphibious drone based on the mechanical scanning sonar to obtain first sonar data in the form of a three-dimensional map;

[0016] Converting the coordinate system of the first sonar data into a Cartesian coordinate system to obtain second sonar data;

[0017] The underwater point cloud data is constructed based on all coordinate points in the second sonar data.

[0018] Furthermore, before the step of constructing the underwater point cloud data based on all the coordinate points in the second sonar data, the method further includes the step of constructing a KD tree based on all the coordinate points in the second sonar data, and filtering the coordinate points in the second sonar data according to the KD tree. This step is specifically as follows:

[0019] Set the threshold for the number of neighbors and the search radius of the coordinate points to be filtered;

[0020] The coordinate points are queried in sequence according to the KD tree, and the number of neighbor coordinate points of each coordinate point within the search radius is calculated, and all coordinate points whose number of neighbor coordinate points is lower than the neighbor number threshold are removed.

[0021] Furthermore, step S3 further includes:

[0022] Mapping the underwater point cloud data into three-dimensional voxels, and calculating the mean and covariance matrix of points in each preset voxel area in the three-dimensional voxels;

[0023] A preset lower limit value of the cluster voxel size is set, and based on the mean and covariance matrix of the points in the preset voxel area, it is determined whether the distance between the adjacent preset voxel areas is greater than the preset lower limit value. If so, point cloud clustering is performed on different preset voxel areas to obtain underwater point cloud clustering data.

[0024] Furthermore, in step S3, when scanning the underwater working environment of the amphibious drone based on the mechanical scanning sonar, a preset coordinate position change threshold is set, and after each scan is completed, the current coordinate position change of the amphibious drone is calculated, and the underwater point cloud clustering data of the current frame in which the coordinate position change is greater than the position change threshold is used as key frame data;

[0025] At the same time, based on a preset threshold algorithm, the underwater point cloud clustering data of the current frame is compared with the previous frame. If the output of the preset threshold algorithm is greater than a preset change threshold, the underwater point cloud data of the current frame is used as the key frame data.

[0026] Furthermore, step S4 includes the following sub-steps:

[0027] Define the residual of the preliminary positioning information as r DVL / IMU , the residual of the observation information is r RTK , the closed-loop residual between the underwater point cloud clustering data of the current frame and the previous frame is in Indicates the coordinate point of the current frame's underwater point cloud cluster mean in the corresponding three-dimensional voxel The coordinates after transformation by the posture matrix T, ‖·‖ represents the Euclidean distance;

[0028] Constructing the factor graph using the preliminary positioning information as a relationship factor between consecutive different coordinate points, using the observation information as a relationship factor between a coordinate point and a value of the observation information, and using the closed-loop residual as a relationship factor between coordinate points of a current frame and a previous frame;

[0029] The residual r of the preliminary positioning information DVL / IMU , the residual r of the observation information RTK , the closed-loop residual r sonar Construct a preset optimization objective function, iteratively solve the factor graph, and obtain the optimal position estimation information. The preset optimization objective function satisfies:

[0030] r total =min(λ a ·r DVL / IMU +λ b ·rRTK +λ c ·r sonar )

[0031] Among them, r total is the preset optimization value, λ a ,λ b ,λ c are the residual r of the preliminary positioning information respectively DVL / IMU , the residual of the observation information is r RTK , the closed-loop residual r sonar The weight of the preset optimization objective function is to solve the preset optimization value r total Minimum.

[0032] Furthermore, in step S4, the preset optimization objective function is calculated according to different preset voxel regions.

[0033] Furthermore, the preset threshold algorithm is a non-destructive detection algorithm.

[0034] The beneficial effect achieved by the present invention lies in proposing a multi-sensor fusion positioning method for amphibious drones that can combine underwater positioning and aerial positioning. This method does not require external ships or buoys to obtain data. It relies on a variety of sensors that the amphibious drones themselves can carry, combines speed and attitude information with different positioning accuracy and stability, and combines the point cloud data comparison and registration method to achieve underwater closed-loop observation. Through this method, position changes can be accurately identified in underwater environments, thereby improving the underwater positioning accuracy of amphibious drones. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 This is a schematic diagram of the steps of the multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle provided by an embodiment of the present invention;

[0036] Figure 2 The figure is a schematic diagram of the data processing flow of the Doppler velocimeter and the inertial measurement unit provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0037] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0038] Please refer to Figure 1 , Figure 1 1 is a schematic flow chart of the steps of a multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle provided by an embodiment of the present invention. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle includes the following steps:

[0039] S1. When the amphibious drone is underwater, estimating the underwater position of the amphibious drone based on a Doppler velocimeter and an inertial measurement unit to obtain preliminary positioning information;

[0040] S2. When the amphibious drone is in the air, estimating the aerial position of the amphibious drone based on a real-time dynamic measurement method to obtain observation information;

[0041] S3. When the amphibious drone enters the water from the air, scanning the underwater working environment of the amphibious drone based on a mechanical scanning sonar to obtain underwater point cloud data;

[0042] S4. Using the preliminary positioning information and the observation information as nodes of a factor graph, and solving the factor graph according to the underwater point cloud data, to obtain optimal position estimation information of the water-air amphibious drone.

[0043] For easier understanding, please refer to Figure 2 FIG. 1 is a schematic diagram of a Doppler velocimeter and an inertial measurement unit data processing flow chart. In the embodiment of the present invention, step S1 is specifically as follows:

[0044] Based on the Doppler velocimeter, the speed data (b-system speed) of the water-air amphibious drone in the DVL coordinate system is obtained, and based on the inertial measurement unit, the acceleration data and angular velocity data of the water-air amphibious drone in the IMU coordinate system are obtained. The speed data, the acceleration data, and the angular velocity data are processed using an extended Kalman filter unit to obtain the preliminary positioning information.

[0045] The inertial measurement unit calculates a posture matrix T based on the acceleration data and the angular velocity data it obtains, and applies the posture matrix T to the step of obtaining velocity data from the Doppler velocimeter. During implementation, due to the higher operating frequency of the inertial measurement unit, the inertial measurement unit integral value is used for updating in a short period of time, and the updated posture matrix is ​​then applied to the velocity data of the Doppler velocimeter to obtain the velocity value in the n-frame.

[0046] In step S2, combined with the operational characteristics of the amphibious drone switching between underwater and air, when the water pressure gauge on the amphibious drone detects that the drone has surfaced from underwater and entered air operation, the real-time dynamic measurement method (RTK) is used to obtain accurate position information data, which is conducive to real-time correction of drift errors when subsequent data are combined.

[0047] Step S3 includes the following sub-steps:

[0048] Scanning the underwater working environment of the amphibious drone based on the mechanical scanning sonar to obtain first sonar data in the form of a three-dimensional map;

[0049] Converting the coordinate system of the first sonar data into a Cartesian coordinate system to obtain second sonar data;

[0050] The underwater point cloud data is constructed based on all coordinate points in the second sonar data.

[0051] The first sonar data obtained by the mechanical scanning sonar includes reflection intensity, angle information, timestamp, etc. The polar coordinate system where the sonar data is located is converted into Cartesian coordinates (x, y, z) as follows:

[0052] x=P X + r·cos(Θ);

[0053] y=P Y + r·sin(Θ);

[0054] z=P Z ;

[0055] Among them, P X 、P Y 、P Z They are respectively the current coordinate values ​​of the amphibious drone relative to the world coordinates.

[0056] Since some isolated and scattered noise points may appear due to echo interference, environmental noise or other unstable factors, it is necessary to filter the generated point cloud data. Before the step of constructing the underwater point cloud data based on all the coordinate points in the second sonar data, the step of: constructing a KD tree based on all the coordinate points in the second sonar data, and filtering the coordinate points in the second sonar data according to the KD tree is further included. The specific steps are:

[0057] Set the threshold for the number of neighbors and the search radius of the coordinate points to be filtered;

[0058] The coordinate points are queried in sequence according to the KD tree, and the number of neighbor coordinate points of each coordinate point within the search radius is calculated, and all coordinate points whose number of neighbor coordinate points is lower than the neighbor number threshold are removed.

[0059] Step S3 further includes:

[0060] Mapping the underwater point cloud data into three-dimensional voxels, and calculating the mean and covariance matrix of points in each preset voxel area in the three-dimensional voxels;

[0061] A preset lower limit value of the cluster voxel size is set, and based on the mean and covariance matrix of the points in the preset voxel area, it is determined whether the distance between the adjacent preset voxel areas is greater than the preset lower limit value. If so, point cloud clustering is performed on different preset voxel areas to obtain underwater point cloud clustering data.

[0062] In step S3, when scanning the underwater working environment of the amphibious drone using the mechanical scanning sonar, due to the slow sonar scanning speed, key frames should not be selected frequently, otherwise redundant information between key frames will increase. A preset coordinate position change threshold is set, and after each scan is completed, the current coordinate position change of the amphibious drone is calculated, and the underwater point cloud cluster data of the current frame whose coordinate position change is greater than the position change threshold is used as the key frame data;

[0063] At the same time, the underwater point cloud cluster data of the current frame is compared with the previous frame based on a preset threshold algorithm. If the output of the preset threshold algorithm is greater than a preset change threshold, the underwater point cloud data of the current frame is used as the key frame data. The preset threshold algorithm is a non-destructive testing algorithm (NDT).

[0064] Step S4 includes the following sub-steps:

[0065] Define the residual of the preliminary positioning information as r DVL / IMU , the residual of the observation information is r RTK , the closed-loop residual between the underwater point cloud clustering data of the current frame and the previous frame is in Indicates the coordinate point of the current frame's underwater point cloud cluster mean data in the corresponding three-dimensional voxel The coordinates after transformation by the posture matrix T, ‖·‖ represents the Euclidean distance;

[0066] Specifically, in the embodiment of the present invention:

[0067]

[0068] v ^ and Θ ^ are the fused velocity and attitude estimates output by the extended Kalman filter unit in step S1;

[0069] r RTK =PP RTK ;

[0070] P RTK is the position data obtained from RTK;

[0071] Constructing the factor graph using the preliminary positioning information as a relationship factor between consecutive different coordinate points, using the observation information as a relationship factor between a coordinate point and a value of the observation information, and using the closed-loop residual as a relationship factor between coordinate points of a current frame and a previous frame;

[0072] The residual r of the preliminary positioning information DVL / IMU , the residual r of the observation information RTK , the closed-loop residual r sonar Construct a preset optimization objective function, iteratively solve the factor graph, and obtain the optimal position estimation information. The preset optimization objective function satisfies:

[0073] r total =min(λ a ·r DVL / IMU +λ b ·r RTK +λ c ·r sonar )

[0074] Among them, r total is the preset optimization value, λ a ,λ b ,λ c are the residual r of the preliminary positioning information respectively DVL / IMU , the residual r of the observation information RTK , the closed-loop residual r sonar The weight of the preset optimization objective function is to solve the preset optimization value r total Minimum.

[0075] In step S4, the preset optimization objective function is calculated according to different preset voxel regions, and the calculation and matching time of point cloud data between different frames can be greatly reduced through cluster matching.

[0076] The beneficial effect achieved by the present invention lies in proposing a multi-sensor fusion positioning method for amphibious drones that can combine underwater positioning and aerial positioning. This method does not require external ships or buoys to obtain data. It relies on a variety of sensors that the amphibious drones themselves can carry, combines speed and attitude information with different positioning accuracy and stability, and combines the point cloud data comparison and registration method to achieve underwater closed-loop observation. Through this method, position changes can be accurately identified in underwater environments, thereby improving the underwater positioning accuracy of amphibious drones.

[0077] Those skilled in the art will appreciate that all or part of the processes in the above-described method embodiments can be implemented by instructing the relevant hardware through a computer program. The program can be stored in a computer-readable storage medium, and when executed, the program can include the processes in the above-described method embodiments. The storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0078] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0079] Through the description of the above embodiments, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus the necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better embodiment. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in each embodiment of the present invention.

[0080] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only a preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms and equivalent changes without departing from the scope of protection of the purpose of the present invention and the claims, which are all within the protection of the present invention.

Claims

1. A multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle, characterized in that: The following steps are involved: S1. When the water-air amphibious UAV is underwater, estimating the underwater position of the water-air amphibious UAV based on a Doppler velocimeter and an inertial measurement unit to obtain preliminary positioning information; S2. When the water-air amphibious UAV is in the air, estimating the air position of the water-air amphibious UAV based on a real-time dynamic measurement method to obtain observation information; S3, when the water-air amphibious unmanned aerial vehicle enters underwater from the air, scanning the underwater working environment of the water-air amphibious unmanned aerial vehicle based on a mechanical scanning sonar to obtain underwater point cloud data; S4. Using the preliminary positioning information and the observation information as nodes of a factor graph, and solving the factor graph according to the underwater point cloud data, to obtain the optimal position estimation information of the water-air amphibious UAV.

2. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 1 is characterized in that: Step S1 is specifically as follows: Based on the Doppler speed meter, the speed data of the water-air amphibious UAV in the DVL coordinate system is obtained. At the same time, based on the inertial measurement unit, the acceleration data and angular velocity data of the water-air amphibious UAV in the IMU coordinate system are obtained. The speed data, the acceleration data, and the angular velocity data are processed using an extended Kalman filter unit to obtain the preliminary positioning information.

3. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 2 is characterized in that: The inertial measurement unit calculates a posture matrix T according to the acquired acceleration data and angular velocity data, and applies the posture matrix T to the step of acquiring velocity data by the Doppler velocimeter.

4. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 1 is characterized in that: Step S3 includes the following sub-steps: Scanning the underwater working environment of the water-air amphibious drone based on the mechanical scanning sonar to obtain first sonar data in the form of a three-dimensional map; Converting the coordinate system of the first sonar data into a Cartesian coordinate system to obtain second sonar data; The underwater point cloud data is constructed based on all coordinate points in the second sonar data.

5. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 4 is characterized in that: Before the step of constructing the underwater point cloud data based on all the coordinate points in the second sonar data, the method further includes the step of constructing a KD tree based on all the coordinate points in the second sonar data, and filtering the coordinate points in the second sonar data according to the KD tree. The specific step is: Set the threshold of the number of neighbors and the search radius of the coordinate points to be filtered; The coordinate points are queried in sequence according to the KD tree, and the number of neighbor coordinate points of each coordinate point within the search radius is calculated, and all coordinate points whose number of neighbor coordinate points is lower than the neighbor number threshold are removed.

6. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 3 is characterized in that: Step S3 also includes: Mapping the underwater point cloud data into three-dimensional voxels, and calculating the mean and covariance matrix of points in each preset voxel area in the three-dimensional voxels; A preset lower limit value of the cluster voxel size is set, and based on the mean and covariance matrix of the points in the preset voxel area, it is determined whether the distance to the adjacent preset voxel area is greater than the preset lower limit value. If so, point cloud clustering is performed on different preset voxel areas to obtain underwater point cloud clustering data.

7. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 6 is characterized in that: In step S3, when the underwater working environment of the water-air amphibious UAV is scanned based on the mechanical scanning sonar, a preset coordinate position change threshold is set, and after each scan is completed, the current coordinate position change of the water-air amphibious UAV is calculated, and the underwater point cloud clustering data of the current frame whose coordinate position change is greater than the position change threshold is used as key frame data; At the same time, based on a preset threshold algorithm, the underwater clustered point cloud data of the current frame is compared with the underwater clustered point cloud data of the previous frame. If the output of the preset threshold algorithm is greater than a preset change threshold, the underwater point cloud data of the current frame is used as the key frame data.

8. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 7 is characterized in that: Step S4 includes the following sub-steps: Define the residual of the preliminary positioning information as r DVL / IMU , the residual of the observation information is r RTK , the closed-loop residual between the underwater point cloud clustering data of the current frame and the previous frame is in Indicates the coordinate point of the underwater point cloud clustering mean of the current frame in the corresponding three-dimensional voxel The coordinates after transformation by the posture matrix T, ‖·‖ represents the Euclidean distance; The factor graph is constructed by using the preliminary positioning information as a relationship factor between consecutive different coordinate points, using the observation information as a relationship factor between a coordinate point and a value of the observation information, and using the closed-loop residual as a relationship factor between coordinate points of a current frame and a previous frame; The residual r of the preliminary positioning information DVL / IMU , the residual r of the observation information RTK , the closed-loop residual r sonar Construct a preset optimization objective function, iteratively solve the factor graph, and obtain the optimal position estimation information. The preset optimization objective function satisfies: r total =min(λ a ·r DVL / IMU +λ b ·r RTK +λ c ·r sonar ) Among them, r total is the preset optimization value, λ a , b , c are the residual r of the preliminary positioning information respectively DVL / IMU , the residual r of the observation information RTK , the closed-loop residual r sonar The iterative solution target of the preset optimization objective function is to make the preset optimization value r total Minimum.

9. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 8, characterized in that: In step S4, the preset optimization objective function is calculated according to different preset voxel regions.

10. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 7, characterized in that: The preset threshold algorithm is a non-destructive detection algorithm.

Images