Underwater unmanned vehicle formation cooperative navigation method based on geomagnetic direction finding assistance

Abstract

The application discloses a kind of underwater unmanned vehicle formation cooperative navigation methods based on geomagnetic direction finding auxiliary.The formation vehicle can measure the measured vehicle in any acoustic communication measurement equipment in formation by self-acoustic communication measurement equipment, and the distance information and azimuth information of the measured vehicle relative to the observation vehicle carrier coordinate system are obtained;The observation vehicle measures and solves the heading angle relative to the reference coordinate system by self-magnetometer, and the angle information of the line vector of the observation vehicle and the measured vehicle relative to the reference coordinate system can be calculated by combining the azimuth information of the measured vehicle relative to the observation vehicle carrier coordinate system, to assist in improving the overall autonomous navigation positioning accuracy of formation vehicle.The application can effectively correct the overall rotation divergence trend of navigation positioning error of formation vehicle based on inter-device communication ranging auxiliary around a certain point in space, so as to further improve the overall autonomous navigation positioning accuracy of underwater formation unmanned vehicle.

Description

Technical Field

[0001] This invention relates to autonomous navigation technology for underwater unmanned vehicles, specifically to a method for cooperative navigation of underwater vehicles in formation based on geomagnetic direction finding. Background Technology

[0002] The inherent weaknesses and vulnerabilities of satellite navigation signals limit their usability for underwater positioning and navigation. For over half a century, with increasing human investment in the utilization and development of the ocean, underwater vehicles have received widespread application and attention in many areas, such as large-scale underwater surveys, seabed mapping, deep-sea exploration, scientific sampling, underwater pipeline laying, tracking and maintenance, and military applications including battlefield surveillance, covert strikes, search and rescue operations. Autonomous navigation is crucial for the autonomous operation of underwater unmanned vehicles (UAVs). Due to the complexity of the underwater environment and limitations in information transmission methods and distances, underwater autonomous navigation is more challenging than aerial navigation. As one of the core and key technologies of underwater vehicles, autonomous navigation technology is also one of the most difficult technologies for researchers to solve. In traditional UAV swarm cooperative autonomous navigation schemes, networked UAVs interact and utilize information such as acoustic communication ranging and direction finding to correct the position, speed, attitude, and other navigation information of each UAV in the swarm in real time, thereby improving the overall navigation accuracy of the swarm. In this scheme, the unmanned aerial vehicles (UAVs) use pairwise communication ranging information to observe and correct the overall navigation and positioning information of the formation UAVs. Although this can curb the relative position error of the formation UAVs and improve the overall absolute positioning accuracy of the formation UAVs to a certain extent, it still cannot curb the trend of the overall absolute positioning error of the formation UAVs rotating and diverging around a certain point in space and the trend of overall drifting and diverging along a certain direction. Summary of the Invention

[0003] To address the challenges of complex underwater environments and the unavailability of radio communication navigation such as GNSS, traditional cooperative autonomous navigation methods for swarm unmanned aerial vehicles (UAVs) based on pairwise acoustic ranging cannot prevent the overall absolute positioning error of the swarm from rotating and diverging around a point in space, or from drifting and diverging in a certain direction. This invention provides a method for cooperative underwater vehicle swarm navigation based on geomagnetic direction finding. Building upon swarm navigation and positioning based on pairwise acoustic ranging, this method introduces the angle between the vector connecting any observer UAV and the observed UAV within the swarm and the reference coordinate system, calculated using geomagnetic direction finding, as auxiliary observation information for overall navigation and positioning. The introduction of geomagnetic direction finding information provides an absolute inertial spatial reference for the swarm UAVs. This method effectively eliminates the tendency for the overall absolute positioning error of the swarm UAVs to rotate and diverge around a point in space, thereby further improving the overall absolute navigation and positioning accuracy of the swarm UAVs.

[0004] A method for cooperative navigation of underwater unmanned vehicles (UAVs) in formation based on geomagnetic direction finding is proposed. Each UAV in the formation is equipped with an inertial measurement unit (IMU), depth gauge, Doppler current meter (DVL), magnetometer, and acoustic communication measurement equipment. An observation UAV in the formation can communicate and measure with any other UAV in the formation that is equipped with acoustic communication measurement equipment, obtaining the distance and azimuth information of the UAV under test relative to the observation UAV's carrier coordinate system. The observation UAV measures and calculates the heading angle relative to the reference coordinate system using its own magnetometer. Combined with the azimuth information of the UAV under test relative to the observation UAV's carrier coordinate system, the angle information of the line vector connecting the observation UAV and the UAV under test relative to the reference coordinate system can be calculated, which is used to help improve the overall autonomous navigation and positioning accuracy of the formation.

[0005] The aforementioned method for collaborative navigation of underwater unmanned vehicles (UAVs) in formation, based on geomagnetic direction finding, uses the navigation parameters of each UAV in the formation, including position, velocity, and attitude, as the state to be estimated. The method employs a strapdown calculation process based on the navigation parameters measured by the IMUs of each UAV as the state prediction process for the Kalman Filter (KF) algorithm. The method uses the ranging information measured by acoustic communication between any two UAVs in the formation as the first set of observation information for KF. The method uses the angle between the line vector connecting the observed UAV and the measured UAV relative to the reference coordinate system, calculated by any UAV observing another UAV and combining it with its own magnetometer data, as the second set of observation information for KF. The method uses the depth data measured by the depth gauge of each UAV as the third set of observation information for KF. The method uses the velocity data in the vehicle coordinate system measured by the DVL of each UAV as the fourth set of observation information for KF. The overall navigation and positioning calculation of the UAV formation is completed through the KF algorithm.

[0006] The method for cooperative navigation of underwater unmanned vehicles based on geomagnetic direction finding assistance comprises the following steps:

[0007] Step 1: Each unmanned vehicle in the formation completes the integral recursive calculation of navigation parameters such as position, velocity, and attitude based on the measurement data of its own IMU. The depth data measured by the depth gauge of each vehicle is used to correct the error in the altitude direction of each vehicle. The velocity data measured by the DVL of each vehicle is used to correct the error in the velocity and attitude parameters of each vehicle. The corrected navigation parameter prediction data of each vehicle is sent to a certain observation vehicle. The observation vehicle that receives the navigation parameter prediction data of each vehicle is defined as the temporary central host.

[0008] Step 2: Each unmanned aerial vehicle (UAV) in the formation completes the communication and ranging between each other through acoustic measurement, and sends the ranging information between each pair of UAVs in the formation to the temporary central host. The temporary central host completes the overall navigation and positioning error correction of the UAVs in the formation based on the ranging observation information between the UAVs.

[0009] Step 3: An observation vehicle in the formation completes the orientation measurement of another tested vehicle through acoustic communication measurement, and obtains the orientation information of the tested vehicle relative to the carrier coordinate system of the observation vehicle. At the same time, combined with the measurement and calculation of the heading angle relative to the reference coordinate system by the magnetometer on the observation vehicle, the angle information of the line vector connecting the observation vehicle and the tested vehicle relative to the reference coordinate system is calculated. This angle information is sent to the temporary central host, and the temporary central host completes the overall navigation and positioning error correction of the formation UAVs based on geomagnetic orientation-assisted navigation.

[0010] The method for cooperative navigation of underwater unmanned vehicles based on geomagnetic orientation finding is described below. The algorithm model for the angle between the line vector connecting the observation vehicle and the measured vehicle relative to the north direction of the reference coordinate system, calculated using geomagnetic orientation finding, is as follows:

[0011]

[0012] Where, α ij x is the angle between the line vector connecting the observed UAV i and the measured UAV j within the formation and the reference coordinate system in the north direction, with clockwise being positive. i ,y i To observe the horizontal coordinates of the unmanned aerial vehicle, x j ,y j The horizontal coordinates of the unmanned aerial vehicle being tested are shown.

[0013] The beneficial effects and advantages of this invention are as follows:

[0014] The underwater unmanned vehicle (UAV) formation cooperative navigation method based on geomagnetic direction finding can effectively eliminate the tendency of the overall absolute positioning error of the formation UAV to rotate and diverge around a certain point in space, thereby further improving the overall absolute navigation and positioning accuracy of the formation UAV. Attached Figure Description

[0015] Figure 1 A schematic diagram of the vector angle between the observation vehicle and the measured vehicle for geomagnetic direction finding auxiliary calculation.

[0016] Figure 2 This is a schematic diagram of a collaborative navigation method for underwater vehicles based on geomagnetic direction finding. Detailed Implementation

[0017] To make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the figures and specific embodiments.

[0018] A method for cooperative navigation of underwater unmanned vehicles (UAVs) in formation based on geomagnetic direction finding is proposed. Each UAV in the formation is equipped with an inertial measurement unit (IMU), depth gauge, Doppler current meter (DVL), magnetometer, and acoustic communication measurement equipment. An observation UAV within the formation can use its built-in acoustic communication measurement equipment to perform communication measurements with any other UAV in the formation equipped with acoustic communication measurement equipment, obtaining the distance and azimuth information of the UAV relative to the observation UAV's coordinate system. The observation UAV uses its built-in magnetometer to measure and calculate the heading angle relative to the reference coordinate system. Combined with the azimuth information of the UAV relative to the observation UAV's coordinate system, the angle between the line vector connecting the observation UAV and the UAV relative to the reference coordinate system can be calculated, which helps improve the overall autonomous navigation and positioning accuracy of the formation. Figure 1 As shown, α1+α2 is a schematic diagram of the vector angle between the observation vehicle and the tested vehicle in the geomagnetic direction finding auxiliary solution, where α1 is the geomagnetic direction finding of the observation vehicle and α2 is the direction finding angle of the tested vehicle under the observation vehicle's carrying system.

[0019] The basic principle of this method is based on the Kalman Filter (KF) algorithm framework. It uses the navigation parameters of each unmanned aerial vehicle (UAV) in the formation, including position, velocity, and attitude, as the state to be estimated. The strapdown calculation process based on the navigation parameters measured by the IMUs of each UAV serves as the state prediction process for the KF algorithm. The ranging information between any two UAVs in the formation, measured via acoustic communication, serves as the first set of observation information for KF. The angle between the line vector connecting the observed UAV and the measured UAV relative to the reference coordinate system, measured by any UAV observing another UAV and combining it with data from its own magnetometer, serves as the second set of observation information for KF. The depth data measured by the depth gauge of each UAV serves as the third set of observation information for KF. The velocity data in the vehicle's coordinate system measured by the DVL of each UAV serves as the fourth set of observation information for KF. The overall navigation and positioning calculation of the UAV formation is then completed using the KF algorithm. The principle block diagram of this method is shown below. Figure 2 As shown, the algorithm involves the following formulas:

[0020] 1) The strapdown solution and KF state equation based on the data from the inertial navigation equipment of the formation UAV are as follows:

[0021]

[0022]

[0023]

[0024]

[0025]

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] Where, φ Ei ,φ Ni ,φ Ui Let δv be the three-axis attitude angle error of the i-th unmanned aerial vehicle. Ei ,δv Ni ,δv Ui Let δL be the three-axis velocity error of the i-th unmanned aerial vehicle. i ,δλ i ,δh i Let ε be the position error of the i-th unmanned aerial vehicle. xi ,ε yi ,ε zi For the i-th unmanned aerial vehicle gyroscope, the three-axis zero bias is ε Ei ,ε Ni ,ε Ui Let the three-axis zero-bias components of the i-th unmanned aerial vehicle gyroscope in the navigation coordinate system be represented. For the i-th unmanned aerial vehicle accelerometer, the three axes are zero bias. Let ω be the component of the three-axis zero bias of the i-th unmanned aerial vehicle accelerometer in the navigation coordinate system. Ni ,ω Ui Let f be the northward and celestial components of the Earth's rotation angular rate of the i-th unmanned aerial vehicle. Ei ,f Ni ,f Ui Let R be the component of the i-th unmanned aerial vehicle accelerometer data in the navigation coordinate system. Mhi ,R Nhi Let be the local radius of curvature containing altitude information for the i-th unmanned aerial vehicle.

[0033] 2) The KF observation equation based on pairwise acoustic ranging data from the formation of unmanned aerial vehicles is as follows:

[0034] Assume the distance measurement between UAV i and UAV j in the formation is:

[0035]

[0036] The distance measurement between the vehicles, based on the inertial navigation predictions of UAVs i and j, is as follows:

[0037]

[0038] The observation equation for the distance measurement error between vehicles is:

[0039]

[0040] in,

[0041]

[0042]

[0043] Where, x Ii ,y Ii ,z Ii Let r be the position estimate of the i-th unmanned vehicle based on inertial navigation recursion. Iij Numerically, ρ can be taken as... Iij D ij This is the transformation matrix between Earth coordinate system and latitude and longitude coordinates.

[0044] 3) The KF observation equation for the angle between the line vector connecting the observed vehicle and the tested vehicle and the reference frame, based on magnetic measurement-aided calculation, is as follows:

[0045] Assuming the magnetic heading angle of the observing vehicle i is α1, the acoustic heading angle of the tested vehicle under the observation vehicle's system is α2, and the measured angle between the vector of the line connecting the observing vehicle and the tested vehicle and the reference frame is:

[0046]

[0047] The angle between the line vectors connecting the observing and tested vehicles relative to the reference frame, estimated by their built-in inertial navigation systems, is:

[0048]

[0049] The equation for the error in the angle between the connecting vector and the reference frame is:

[0050]

[0051] in,

[0052]

[0053] 4) The KF algorithm process is as follows:

[0054] In the underwater multi-UAV swarm cooperative autonomous navigation scheme, distributed sequential Kalman filtering is adopted as the fusion filtering algorithm framework. Taking the single sequential filtering of i UAVs and j-th UAV as an example, the specific process is as follows:

[0055] First, the navigation state parameters are updated over time based on the data from their respective inertial measurement units:

[0056] The time update process for the i-th unmanned aerial vehicle is as follows:

[0057] X i,k+1 / k =Φ i,k+1 / k ·X i,k

[0058]

[0059] The time update process for the jth unmanned aerial vehicle is as follows:

[0060] X j,k+1 / k =Φ j,k+1 / k ·X j,k

[0061]

[0062] The sequential measurement update process corresponding to the observations of the i-th UAV and the j-th UAV is as follows:

[0063]

[0064] X ij,k+1 =X ij,k+1 / k +K ij,k+1 ·(Z ij,k+1 -H ij,k+1 ·X ij,k+1 / k )

[0065] P ij,k+1 =(IK ij,k+1 ·H ij,k+1 )·P ij,k+1 / k

[0066] in,

[0067] X ij,k+1 / k =[X i,k+1 / k ,X j,k+1 / k ] T

[0068]

[0069] Φ i,k+1 / k , Φ j,k+1 / kFor updating the state of unmanned aerial vehicles (UAVs) i and j, the system matrix is ​​Γ. i,k , Γ j,k Q is the noise driving matrix. i,k Q j,k Z is the noise matrix. ij,k+1 Correlate observations of UAV i and UAV j, H ij,k+1 For the measurement matrix that correlates the observations of UAV i and UAV j, R ij,k+1 This is the measurement noise matrix.

[0070] This method specifically includes the following steps:

[0071] Step 1: Each unmanned vehicle in the formation completes the integral recursive calculation of navigation parameters such as position, velocity, and attitude based on the measurement data of its own IMU. The depth data measured by the depth gauge of each vehicle is used to correct the error in the altitude direction of each vehicle. The velocity data measured by the DVL of each vehicle is used to correct the error in the velocity and attitude parameters of each vehicle. The corrected navigation parameter prediction data of each vehicle is sent to a certain observation vehicle. The observation vehicle that receives the navigation parameter prediction data of each vehicle is defined as the temporary central host.

[0072] Step 2: Each unmanned aerial vehicle (UAV) in the formation completes the communication and ranging between each other through acoustic measurement, and sends the ranging information between each pair of UAVs in the formation to the temporary central host. The temporary central host completes the overall navigation and positioning error correction of the UAVs in the formation based on the ranging observation information between the UAVs.

[0073] Step 3: An observation vehicle in the formation completes the orientation measurement of another tested vehicle through acoustic communication measurement, and obtains the orientation information of the tested vehicle relative to the carrier coordinate system of the observation vehicle. At the same time, combined with the measurement and calculation of the heading angle relative to the reference coordinate system by the magnetometer on the observation vehicle, the angle information of the line vector connecting the observation vehicle and the tested vehicle relative to the reference coordinate system is calculated. This angle information is sent to the temporary central host, and the temporary central host completes the overall navigation and positioning error correction of the formation UAVs based on geomagnetic orientation-assisted navigation.

[0074] This invention relates to a collaborative navigation method for underwater unmanned vehicles (UAVs) based on geomagnetic direction finding. This method can effectively eliminate the tendency of the overall absolute positioning error of the UAVs to rotate and diverge around a certain point in space, thereby further improving the overall absolute navigation and positioning accuracy of the UAVs.

[0075] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A method for cooperative navigation of underwater unmanned vehicles in formation based on geomagnetic direction finding, characterized by: Each unmanned vehicle (UAV) in the formation is equipped with an inertial measurement unit (IMU), depth gauge, Doppler current meter (DVL), magnetometer, and acoustic communication measurement equipment. One UAV in the formation is selected as the observation UAV, and the rest are the test UAVs. The observation UAV uses its built-in acoustic communication measurement equipment to conduct communication measurements with any test UAV in the formation, obtaining the distance and bearing information of the test UAV relative to the observation UAV's carrier coordinate system. The observation UAV uses its built-in magnetometer to measure and calculate the heading angle relative to the reference coordinate system. Combined with the bearing information of the test UAV relative to the observation UAV's carrier coordinate system, the observation UAV calculates the angle information of the line vector connecting the observation UAV and the test UAV relative to the reference coordinate system. This helps to improve the overall autonomous navigation and positioning accuracy of the formation UAV and can effectively correct the trend of the overall rotational divergence of the navigation and positioning error of the formation UAV around a certain point in space when relying solely on inter-vehicle communication ranging.

2. The underwater unmanned vehicle formation cooperative navigation method based on geomagnetic direction finding assistance as described in claim 1, characterized in that: The navigation parameters of each unmanned aerial vehicle (UAV) in the formation, including position, velocity, and attitude, are taken as the state to be estimated. The strapdown calculation process based on the navigation parameters measured by the IMU of each UAV is used as the state prediction process of the Kalman Filter (KF) algorithm. The ranging information of any two UAVs in the formation based on acoustic communication measurement is used as the first set of observation information of KF. The angle information between the line vector connecting the observed UAV and the tested UAV relative to the reference coordinate system, which is measured and calculated by any UAV in the formation observing another UAV and combined with the data of its own magnetometer, is used as the second set of observation information of KF. The depth data measured by the depth gauge of each UAV is used as the third set of observation information of KF. The velocity data in the vehicle coordinate system measured by the DVL of each UAV is used as the fourth set of observation information of KF. The overall navigation and positioning calculation of the UAVs in the formation is completed by the KF algorithm.

3. The underwater unmanned vehicle formation cooperative navigation method based on geomagnetic direction finding assistance as described in claim 1, characterized in that: step as follows: Step 1: Each unmanned vehicle in the formation completes the integral recursive calculation of navigation parameters, including position, velocity, and attitude, based on the measurement data of its own IMU. The depth data measured by the depth gauge of each vehicle is used to correct the error in the altitude direction of each vehicle. The velocity data measured by the DVL of each vehicle is used to correct the error in the velocity and attitude parameters of each vehicle. The corrected navigation parameter prediction data of each vehicle is sent to the observation vehicle, which is defined as a temporary central host. Step 2: Each unmanned aerial vehicle (UAV) in the formation completes the communication and ranging between each other through acoustic measurement, and sends the ranging information between each pair of UAVs in the formation to the temporary central host. The temporary central host completes the overall navigation and positioning error correction of the UAVs in the formation based on the ranging observation information between the UAVs. Step 3: The observation vehicle in the formation completes the orientation measurement of the other tested vehicle through acoustic communication measurement, and obtains the orientation information of the tested vehicle relative to the carrier coordinate system of the observation vehicle. At the same time, combined with the measurement and calculation of the heading angle relative to the reference coordinate system by the magnetometer on the observation vehicle, the angle information of the line vector connecting the observation vehicle and the tested vehicle relative to the reference coordinate system is calculated. This angle information is sent to the temporary central host, and the temporary central host completes the overall navigation and positioning error correction of the formation unmanned vehicles based on geomagnetic orientation-assisted navigation.

4. The underwater unmanned vehicle formation cooperative navigation method based on geomagnetic direction finding assistance as described in claim 1, characterized in that: The algorithm model for the angle between the line vector connecting the observation vehicle and the measured vehicle relative to the north direction of the reference coordinate system, based on geomagnetic direction finding-aided calculation, is as follows: ； in, For observation of unmanned aerial vehicles within the formation With the unmanned aerial vehicle under test The angle between the connecting vectors and the reference coordinate system in the north direction is positive for clockwise directions. To observe the horizontal coordinates of the unmanned aerial vehicle, The horizontal coordinates of the unmanned aerial vehicle being tested are shown.

Images