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

Abstract

The present application relates to unmanned aerial vehicle positioning technology field, especially to a kind of amphibious unmanned aerial vehicle of water and air multi-sensor fusion positioning method, comprising the following steps: when amphibious unmanned aerial vehicle of water and air is underwater, underwater position of the amphibious unmanned aerial vehicle of water and air is estimated based on Doppler velocity meter and inertial measurement unit, and preliminary positioning information is obtained;When unmanned aerial vehicle is in the air, the air position of the amphibious unmanned aerial vehicle of water and air is estimated based on real-time dynamic measurement method, and observation information is obtained;When unmanned aerial vehicle enters underwater from the air, underwater working environment of the amphibious unmanned aerial vehicle of water and air is scanned based on mechanical scanning sonar, and underwater point cloud data is obtained;With the preliminary positioning information and the observation information as the node of factor graph, and according to the underwater point cloud data, the factor graph is solved, and the optimal position estimation information of the amphibious unmanned aerial vehicle of water and air is obtained.The present application improves the underwater positioning accuracy of amphibious unmanned aerial vehicle of water and air.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle positioning, and particularly relates to a multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle. BACKGROUND

[0002] In the context of today's rapid development of science and technology, robots play an increasingly important role in the fields of ocean exploration, resource development and environmental monitoring, especially in special working scenarios such as bridge detection and reservoir inspection, which require unmanned robots to perform cross-medium tasks. In this case, amphibious unmanned aerial vehicles show their important value due to their unique ability to operate in different environments.

[0003] Traditional unmanned aerial vehicle positioning methods are relatively mature, for example, outdoor unmanned aerial vehicles usually use GPS for absolute positioning, while indoor environments often use cameras or laser radar sensors for precise positioning. However, amphibious unmanned aerial vehicles face many challenges when performing underwater operations, especially the severe attenuation of GPS signals in underwater environments and the signal interruption problem at a certain depth.

[0004] Currently, long-time underwater positioning technology mainly relies on cooperation with water surface vessels or buoys to establish a local information network and use long / short baseline methods to simulate outdoor air GNSS principles to calculate the position of the robot underwater. However, these methods usually require specific pre-deployment of the environment and have low adaptability to the environment, making them unsuitable for lightweight amphibious unmanned aerial vehicles. Moreover, in the prior art, underwater and aerial positioning methods are usually developed independently and cannot be effectively combined. SUMMARY

[0005] The present application aims to solve the problem that existing amphibious unmanned aerial vehicles cannot effectively combine different position positioning information when positioning.

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

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

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

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

[0010] S4, taking 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 amphibious unmanned aerial vehicle.

[0011] Further, step S1 is specifically:

[0012] Based on the Doppler velocity log, velocity data of the amphibious unmanned aerial vehicle in a DVL coordinate system is obtained, and based on the inertial measurement unit, acceleration data and angular velocity data of the amphibious unmanned aerial vehicle in an IMU coordinate system are obtained. The velocity data, the acceleration data and the angular velocity data are processed by using an extended Kalman filter unit to obtain the preliminary positioning information.

[0013] Further, the inertial measurement unit calculates a posture matrix T according to the acceleration data and the angular velocity data obtained by the inertial measurement unit, and applies the posture matrix T to the step of obtaining velocity data by the Doppler velocity log.

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

[0015] Based on the mechanical scanning sonar, the underwater working environment of the amphibious unmanned aerial vehicle is scanned to obtain first sonar data in the form of a three-dimensional map;

[0016] The coordinate system in which the first sonar data is located is converted into a Cartesian coordinate system to obtain second sonar data;

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

[0018] Further, before the step of constructing the underwater point cloud data based on all coordinate points in the second sonar data, there is a step of constructing a K-D tree based on all coordinate points in the second sonar data, and filtering processing the coordinate points in the second sonar data according to the K-D tree. The step is specifically:

[0019] Setting a neighbor number threshold and a search radius for the coordinate points of the filtering processing;

[0020] According to the K-D tree, the coordinate points are sequentially queried, and the number of neighbor coordinate points within the search radius of each coordinate point is calculated. All coordinate points with a neighbor coordinate point number lower than the neighbor number threshold are removed.

[0021] Further, step S3 further includes:

[0022] The underwater point cloud data is mapped into a three-dimensional voxel, and the mean value and the covariance matrix of the points in each preset voxel region in the three-dimensional voxel are calculated.

[0023] Setting a preset lower limit value of the cluster voxel size, judging whether the distance between adjacent preset voxel regions is greater than the preset lower limit value based on the mean and covariance matrix of the points of the preset voxel region, if yes, performing point cloud clustering on different preset voxel regions to obtain underwater point cloud clustering data.

[0024] Further, in step S3, when the mechanical scanning sonar scans the underwater working environment of the amphibious unmanned aerial vehicle, a preset coordinate position change threshold is set, and after each scanning is completed, the coordinate position change amount of the current amphibious unmanned aerial vehicle is calculated, and the underwater point cloud clustering data of the current frame whose coordinate position change amount is greater than the position change threshold is taken 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 and the previous frame are compared, and 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 taken as the key frame data.

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

[0027] The residual of the preliminary positioning information is defined as r DVL / IMU , the residual of the observation information is r RTK , and the loop closure residual between the underwater point cloud clustering data of the current frame and the previous frame is Wherein represents the coordinate of the underwater point cloud clustering mean of the current frame in the corresponding three-dimensional voxel converted by the attitude matrix T, and ‖·‖ represents the Euclidean distance;

[0028] The factor graph is constructed by taking the preliminary positioning information as the relationship factor of consecutive different coordinate points, taking the observation information as the relationship factor of the coordinate point and the value of the observation information, and taking the loop closure residual as the relationship factor of the coordinate points of the current frame and the previous frame.

[0029] The residual r DVL / IMU of the preliminary positioning information, the residual r RTK of the observation information, and the loop closure residual r sonar are used to construct a preset optimization objective function, the factor graph is iteratively solved to obtain the optimal position estimation information, and the preset optimization objective function satisfies:

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

[0031] wherein, r total is a preset optimization value, λ a , λ b , λ c are weights of the residual r DVL / IMU of the preliminary positioning information, the residual r RTK of the observation information, and the closed loop residual r sonar respectively, and an iterative solving target of the preset optimization objective function is to make the preset optimization value r total minimum.

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

[0033] Further, the preset threshold algorithm is a nondestructive testing algorithm.

[0034] The present application has the advantages that a multi-sensor fusion positioning method of an amphibious unmanned aerial vehicle combining underwater positioning and aerial positioning is provided, the method does not need to obtain data from an external ship or buoy, relies on various sensors that can be carried by the amphibious unmanned aerial vehicle itself, combines speed and attitude information with different positioning accuracies and stabilities, and realizes underwater closed loop observation by combining point cloud data comparison and registration, so that the position change can be accurately identified in an underwater environment, and the underwater positioning accuracy of the amphibious unmanned aerial vehicle is improved. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 is a step flowchart of the multi-sensor fusion positioning method of the amphibious unmanned aerial vehicle provided by the embodiment of the present application.

[0036] Figure 2 is a data processing flowchart of a Doppler speedometer and an inertial measurement unit. DETAILED DESCRIPTION

[0037] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application is further described in detail below in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application.

[0038] Please refer to Figure 1 , Figure 1 is a step flowchart of the multi-sensor fusion positioning method of the amphibious unmanned aerial vehicle provided by the embodiment of the present application, and the multi-sensor fusion positioning method of the amphibious unmanned aerial vehicle comprises the following steps:

[0039] S1, estimating the underwater position of the amphibious UAV based on a Doppler velocity log and an inertial measurement unit when the amphibious UAV is underwater, to obtain preliminary positioning information;

[0040] S2, estimating the aerial position of the amphibious UAV based on a real-time kinematic measurement method when the amphibious UAV is in the air, to obtain observation information;

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

[0042] S4, taking 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 amphibious UAV.

[0043] Specifically, for ease of understanding, please refer to the Doppler velocity log and inertial measurement unit data processing flowchart shown in Figure 2 The step S1 in the embodiment of the application is specifically:

[0044] Based on the Doppler velocity log, velocity data of the amphibious UAV in the DVL coordinate system is obtained, and based on the inertial measurement unit, acceleration data and angular velocity data of the amphibious UAV in the IMU coordinate system are obtained. The velocity 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 an attitude matrix T according to the acceleration data and the angular velocity data obtained by the inertial measurement unit, and applies the attitude matrix T to the step of obtaining velocity data by the Doppler velocity log. In the implementation process, the running frequency of the inertial measurement unit is higher, and the integral value of the inertial measurement unit is updated in a short time, and then the updated attitude matrix is applied to the velocity data of the Doppler velocity log to obtain the velocity value in the n system.

[0046] In step S2, in combination with the operation characteristics of the amphibious UAV switching between underwater and aerial, when the water pressure gauge carried on the amphibious UAV detects that the UAV floats out of the water and enters aerial operation, the real-time kinematic measurement method (RTK) is used to obtain accurate position information data, which is beneficial to real-time correction of drift error when subsequent data are combined.

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

[0048] The mechanical scanning sonar scans the underwater working environment of the amphibious unmanned aerial vehicle to obtain first sonar data in the form of a three-dimensional map;

[0049] The first sonar data is converted into a Cartesian coordinate system to obtain second sonar data;

[0050] The second sonar data is used to construct the underwater point cloud data.

[0051] The first sonar data obtained by the mechanical scanning sonar includes reflection intensity, angle information, and a timestamp. The polar coordinate system in which the sonar data is located is converted into a Cartesian coordinate (x, y, z) in the following manner:

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

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

[0054] z=P Z ;

[0055] wherein P X , P Y , and P Z are coordinate values of the amphibious unmanned aerial vehicle relative to the world coordinate.

[0056] Some isolated and scattered noise points may appear due to echo interference, environmental noise, or other unstable factors, and therefore the generated point cloud data needs to be filtered. Before the step of constructing the underwater point cloud data based on all coordinate points in the second sonar data, the method further includes the steps of constructing a K-D tree based on all coordinate points in the second sonar data and filtering the coordinate points in the second sonar data according to the K-D tree, which specifically includes the following steps:

[0057] setting a neighbor number threshold and a search radius for the filtered coordinate points;

[0058] querying the coordinate points according to the K-D tree one by one, calculating the number of neighbor coordinate points within the search radius for each coordinate point, and removing all coordinate points with a neighbor number lower than the neighbor number threshold.

[0059] Step S3 further includes:

[0060] mapping the underwater point cloud data to a three-dimensional voxel and calculating the mean value and covariance matrix of points in each preset voxel region in the three-dimensional voxel;

[0061] A preset lower limit value of a cluster voxel size is set, whether the distance between adjacent preset voxel regions is greater than the preset lower limit value is determined based on the mean value and the covariance matrix of the points of the preset voxel region, and if yes, point cloud clustering is performed on different preset voxel regions to obtain underwater point cloud clustering data.

[0062] In step S3, when the underwater working environment of the amphibious unmanned aerial vehicle is scanned based on the mechanical scanning sonar, the key frames are not suitable to be selected frequently because the scanning speed of the sonar is slow, otherwise the redundant information between the key frames will be increased. A preset coordinate position change threshold is set, after each scanning is completed, the coordinate position change amount of the amphibious unmanned aerial vehicle is calculated, and the underwater point cloud clustering data of the current frame whose coordinate position change amount is greater than the position change threshold is taken as the key frame data.

[0063] Meanwhile, the underwater point cloud clustering data of the current frame and the previous frame are compared 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 taken 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] The residual error of the preliminary positioning information is defined as r DVL / IMU , the residual error of the observation information is defined as r RTK , and the loop closure residual error between the underwater point cloud clustering data of the current frame and the previous frame is defined as Wherein represents the coordinate point of the underwater point cloud clustering mean value data of the current frame in the corresponding three-dimensional voxel The coordinates converted by the attitude matrix T, and ‖·‖ represents the Euclidean distance;

[0066] Specifically, in the embodiment of the application:

[0067]

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

[0069] r RTK = P-P RTK ;

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

[0071] constructing the factor graph by taking the preliminary positioning information as a relationship factor of continuous different coordinate points, taking the observation information as a relationship factor of coordinate points and values of the observation information, and taking the closed loop residual error as a relationship factor of coordinate points of a current frame and a previous frame;

[0072] a residual error r DVL / IMU of the preliminary positioning information,a residual error r RTK of the observation information, a closed loop residual error r sonar

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

[0074] wherein r total is a preset optimization value, λ a , λ b , λ c are weights of the residual error r DVL / IMU of the preliminary positioning information, the residual error r RTK of the observation information, and the closed loop residual error r sonar , and an iteration solving target of the preset optimization target function is to make the preset optimization value r total minimum.

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

[0076] The method can combine underwater positioning and aerial positioning, and the multi-sensor fusion positioning method of the amphibious unmanned aerial vehicle has the advantages that data is not required from an external ship or buoy, different positioning accuracy and stability speed and attitude information are combined, underwater closed loop observation is realized by combining point cloud data comparison and registration, position changes can be accurately identified in an underwater environment, and underwater positioning accuracy of the amphibious unmanned aerial vehicle is improved.

[0077] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing relevant hardware through a computer program. The program can be stored in a computer readable storage medium. When the program is executed, the processes of the above-mentioned embodiment methods can be included. The storage medium can be a magnetic disc, an optical disc, a read-only memory (ROM) or a random access memory (RAM).

[0078] It should be noted that, in this document, the terms "comprising", "containing" or any other variant thereof are intended to cover non-exclusive inclusions, so that a process, method, article or device that includes a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such a process, method, article or device. Without more limitations, the element defined by the statement "comprising a" does not exclude the presence of other identical elements in the process, method, article or device that includes the element.

[0079] From the above description of the embodiments, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be realized by means of software and a necessary general hardware platform, and of course, they can also be realized by hardware, but in many cases, the former is a better embodiment. Based on this understanding, the technical solutions of the present application can be embodied in the form of a software product, which is stored in a storage medium (such as a ROM / RAM, a magnetic disc, an optical disc) and includes a plurality of instructions for making a terminal (which can be a mobile phone, a computer, a server, an air conditioner or a network device) execute the methods described in the various embodiments of the present application.

[0080] The embodiments of the present application are described above in combination with the accompanying drawings, and the disclosed are only the preferred embodiments of the present application, but the present application is not limited to the above-mentioned specific embodiments. The above-mentioned specific embodiments are only illustrative, not restrictive, and those skilled in the art can make many equivalent changes without departing from the purpose of the present application and the scope of the claims.

Claims

1. A multi-sensor fusion positioning method for amphibious unmanned aerial vehicles, characterized in that: The following steps are involved: 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; 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; 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; 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 amphibious drone; Wherein, step S1 is specifically as follows: Acquire velocity data of the amphibious unmanned aerial vehicle in a DVL coordinate system based on the Doppler velocimeter, and simultaneously acquire acceleration data and angular velocity data of the amphibious unmanned aerial vehicle in an IMU coordinate system based on the inertial measurement unit, and process the velocity data, the acceleration data, and the angular velocity data using an extended Kalman filter unit to obtain the preliminary positioning information; The inertial measurement unit calculates the attitude matrix based on the acceleration data and the angular velocity data it obtains , and the posture matrix Acting on the step of obtaining velocity data by the Doppler velocimeter; Step S3 includes the following sub-steps: 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; Converting the coordinate system of the first sonar data into a Cartesian coordinate system to obtain second sonar data; Constructing the underwater point cloud data based on all coordinate points in the second sonar data; Step S3 further 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; Setting a preset lower limit value for the cluster voxel size, determining whether the distance between adjacent preset voxel regions is greater than the preset lower limit value based on the mean and covariance matrix of the points in the preset voxel region, and if so, performing point cloud clustering on different preset voxel regions to obtain underwater point cloud clustering data; Step S4 includes the following sub-steps: The residual of the preliminary positioning information is defined as , the residual of the observation information is , 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 Through the posture matrix The converted coordinates are represents the Euclidean distance; 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; The residual of the preliminary positioning information , the residual of the observation information , the closed-loop residual Construct a preset optimization objective function, iteratively solve the factor graph, and obtain the optimal position estimation information. The preset optimization objective function satisfies: in, is the preset optimized value, are the residuals of the preliminary positioning information , the residual of the observation information , the closed-loop residual The iterative solution goal of the preset optimization objective function is to make the preset optimization value Minimum.

2. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 1 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 steps 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 steps are as follows: Set the threshold for 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.

3. The multi-sensor fusion positioning method for an amphibious unmanned aerial vehicle according to claim 1 is characterized in that: 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; 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.

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

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

Images