Trajectory Calculation and Registration Method Based on Inertial Navigation and Ultra-Short Baseline Positioning Sensors of Autonomous Underwater Vehicles

Abstract

The present invention discloses a trajectory calculation and registration method based on an inertial navigation system and an ultra-short baseline positioning sensor of an autonomous underwater vehicle. Through the autonomous underwater vehicle including an inertial navigation device and an ultra-short baseline positioning sensor, IMU data and USBL data are acquired, and the USBL data is screened and processed; the coordinate points of the IMU data are converted from the WGS84 coordinate system to the ENU coordinate system; the trajectories of the processed USBL data points and IMU data points in the ENU coordinate system are plotted; the angle and the origin position deviation between the ENU coordinate system and the USBL coordinate system are solved; the transformation matrix between the ENU coordinate system and the USBL coordinate system is solved, and the USBL data points and the IMU data points are registered; the trajectories of the registered IMU and USBL data in the ENU coordinate system are plotted, and using satellite photos and relevant GPS coordinate values, the plotted trajectories are superimposed with the actual on-site satellite photos. The present invention verifies the accuracy of the proposed trajectory solving algorithm while providing accurate AUV movement trajectories for AUV control personnel and providing effective information for AUV mission execution.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater autonomous positioning and navigation, and particularly to a method for trajectory calculation and registration based on an inertial navigation and ultra-short baseline positioning sensor of an autonomous underwater vehicle. Background Art

[0002] As an emerging underwater exploration and development platform, AUV (Autonomous Underwater Vehicle) has been widely studied and applied in recent years. The trajectory measurement method of AUV can provide an accurate reference for its underwater navigation and positioning, so that AUV can perform specific tasks underwater and provide the actual movement trajectory of AUV for the operator.

[0003] Through literature retrieval of the existing technologies, it is found that limited by the low visibility, low light conditions underwater and the large electromagnetic wave attenuation rate, the currently commonly used AUV underwater navigation devices include IMU (Inertial Measurement Unit) and USBL (Ultra-short Baseline), etc. IMU usually includes three single-axis gyroscopes and three single-axis accelerometers. It measures the attitude and acceleration of the carrier platform through the gyroscope and the accelerometer respectively, and obtains the final position and attitude information based on the initial coordinates. IMU is carried on AUV and needs to be initially calibrated before use, and there are problems such as constant offset of the gyroscope and constant drift of the accelerometer, and its positioning accuracy decreases with the increase of working time. USBL consists of a transmitting transducer, a receiving array and a transponder. The transmitting transducer and the receiving array are located at reference positions such as ships or the shore, and the transponder is located on a moving carrier such as AUV. USBL emits an acoustic pulse through the transmitting transducer. The transponder on the AUV receives it and sends back a response acoustic pulse. The receiving array receives this signal and solves it to obtain the position information of the AUV where the transponder is located. The relative position of AUV can be obtained through the USBL system, but the array of USBL is prone to position and angle offsets, resulting in large positioning deviations. The relative position relationship between AUV and USBL in the experiment is as Figure 2 shown.

[0004] Therefore, the technical personnel in this field are committed to developing a method for trajectory calculation and registration based on an inertial navigation and ultra-short baseline positioning sensor of an autonomous underwater vehicle. Summary of the Invention

[0005] In view of the above-mentioned defects of the existing technologies, the technical problem to be solved by the present invention is how to solve the problem that the underwater navigation device USBL of AUV is prone to position and angle offsets, resulting in large positioning deviations, research and improve the trajectory solving system of the autonomous underwater vehicle, and provide an accurate AUV movement trajectory for AUV control personnel.

[0006] To achieve the above object, the present invention provides a method for trajectory calculation and registration based on an inertial navigation and ultra-short baseline positioning sensor of an autonomous underwater vehicle, including the following steps:

[0007] Step 1: Obtain IMU data and USBL data through an autonomous underwater vehicle including an inertial navigation device and an ultra-short baseline sensor, and screen and process the USBL data;

[0008] Step 2: Convert the coordinate points of the IMU data from the WGS84 (1984 World Geodetic System) coordinate system to the ENU coordinate system (Local East, North, Up Coordinates);

[0009] Step 3: Plot the trajectories of the processed USBL data points and IMU data points in the ENU coordinate system;

[0010] Step 4: Solve the angle and origin position deviation between the ENU coordinate system and the USBL coordinate system;

[0011] Step 5: Solve the transformation matrix between the ENU coordinate system and the USBL coordinate system, and register the USBL data points and IMU data points;

[0012] Step 6: Plot the trajectories of the registered IMU and USBL data in the ENU coordinate system, and overlay the plotted trajectories with the actual on-site satellite photos using satellite photos and relevant GPS (Global Positioning System) coordinate values.

[0013] Further, in Step 1, according to the prior information of the AUV vehicle in the actual experiment, process the abnormal data of the USBL data to ensure the reliability of the remaining USBL data.

[0014] Further, the specific operation process in Step 1 is as follows:

[0015] Step 1.1: In the X direction, process the data in the following manner:

[0016]

[0017] where x(i) is the X-axis coordinate of the i-th USBL data point, that is, the depth coordinate; d 0 is the maximum diving depth of the AUV during the experiment. According to d 0 mark the depth abnormal data points as error;

[0018] Step 1.2. Process the data in the Y direction in the following manner:

[0019]

[0020] where y(i) is the Y-axis coordinate of the i-th USBL data point; during the experiment, the AUV's movement range in the Y direction is within half of the width w of the experimental water area. Based on w 0 mark the abnormal data points in the Y direction as error; 0

[0021] Step 1.3. Process the data in the Z direction in the following manner:

[0022]

[0023] where z(i) is the Z-axis coordinate of the i-th USBL data point; during the experiment, the AUV's movement range in the Z direction is within the length l of the experimental water area. Based on l 0 mark the abnormal data points in the Z direction as error; 0

[0024] Step 1.4. Process the data between adjacent timestamp data points in the following manner:

[0025]

[0026] where v max is the maximum movement speed of the AUV during the experiment. Based on v max mark the abnormal data points in terms of speed as error;

[0027] Step 1.5. Eliminate the USBL data points marked as error according to the above Steps 1.1 to 1.4.

[0028] Furthermore, Step 1 also includes: deleting the data points where the XYZ coordinates are all 0, which represents that the ultra-short baseline sensor did not send or receive valid data properly during this period.

[0029] Furthermore, Step 2 specifically includes:

[0030] Step 2.1. Convert the IMU data point coordinates from the WGS84 coordinate system to the ECEF coordinate system (Earth-Centered, Earth-Fixed rectangular coordinate system);

[0031] Step 2.2. Convert the IMU data points in the ECEF coordinate system to the ENU coordinate system.

[0032] ​​Further, step 2.1 specifically includes: The origin of the ECEF coordinate system is the centroid of the Earth. The X-axis extends through the intersection of the Prime Meridian (0-degree longitude line) and the Equator (0-degree latitude line). The Z-axis extends through the North Pole. The Y-axis follows the right-hand coordinate system and passes through the Equator and 90-degree longitude. The calculation steps for converting (lon, lat, alt) in the WGS84 coordinate system to the point (X, Y, Z) in the ECEF coordinate system are as follows:

[0033]

[0034] where e is the ellipsoidal eccentricity and τ is the radius of curvature of the reference ellipsoid;

[0035]

[0036] where a is the equatorial radius and b is the polar axis radius.

[0037] Further, step 2.2 specifically includes: The origin of the coordinates of the user is P 0 =(x 0 , y 0 , z 0 ). The calculation point is P=(x, y, z). The position (e, n, u) in the ENU coordinate system with point P 0 as the origin of coordinates. The WGS84 coordinate point of P 0 is LLA 0 =(lon 0 , lat 0 , lat 0 ). The calculation steps are as follows:

[0038]

[0039]

[0040] where S is the coordinate transformation matrix:

[0041]

[0042] Further, in step 3, using the Y-axis and Z-axis data of the USBL obtained in step 1 and the coordinates of the IMU data points obtained in step 2 in the ENU coordinate system, the unregistered trajectories of the two in the ENU coordinate system are plotted.

[0043] Further, step 4 specifically includes:

[0044] Step 4.1: Select a data point A with stable changes, and calculate the coordinates of point A in the ENU coordinate system as A ENU , and the coordinates of data point A in the USBL coordinate system as A USBL ;

[0045] Step 4.2: Then, based on the longitude and latitude values of the USBL device coordinate point O, calculate the coordinates of point O in the ENU coordinate system, O ENU (O E ,O N ), and the coordinates of point O in the USBL coordinate system, O USBL (0, 0);

[0046] Step 4.3: Obtain the slope and its angle α of the straight line OA in the ENU coordinate system, the slope and its angle β of the straight line OA in the USBL coordinate system through the above four coordinates, and then obtain the angle and origin position deviation value γ between the ENU coordinate system and the USBL coordinate system: γ = α - β.

[0047] Furthermore, step 5 specifically includes:

[0048] Step 5.1: Rotation transformation: Rotate the USBL coordinate system data points clockwise by γ angle:

[0049]

[0050] where x USBL (i) and y USBL (i) respectively represent the X-axis and Y-axis data of the USBL data point i in the USBL coordinate system, and (x 1 (i), y 1 (i)) is the coordinate of the USBL data point i in the USBL coordinate system after rotation transformation.

[0051] Step 5.2: Translation transformation: Translate the data coordinates obtained in the above step 5.1 to obtain the coordinates (x 2 (i), y 2 (i)) of the USBL data point in the ENU coordinate system:

[0052]

[0053] Step 5.3: Solve the transformation matrix: According to the above steps 5.1 and 5.2, obtain the rotation matrix R and the translation matrix T, and obtain the augmented transformation matrix C:

[0054] The rotation matrix is:

[0055]

[0056] The translation matrix is:

[0057]

[0058] The augmented transformation matrix:

[0059]

[0060] By the following formula:

[0061]

[0062] the coordinates (x 2 , y 2 ) of the USBL data points in the ENU coordinate system can be obtained.

[0063] Compared with the existing technical solutions, the beneficial effects brought by the solution of the present invention are as follows: The present invention realizes the research and improvement of the trajectory solution system of the autonomous underwater vehicle. Through the data screening algorithm, the abnormal USBL data is eliminated; based on the geometric relationship between coordinate systems and combined with the data distribution, the transformation matrix between coordinate systems is solved, providing effective information for the registration of USBL and IMU data; by using satellite images and existing GPS information, the solved trajectory is superimposed and displayed, while verifying the accuracy of the proposed trajectory solution algorithm, providing the accurate AUV motion trajectory and a more intuitive trajectory monitoring interface for the AUV control personnel, and providing effective information for the AUV to execute tasks.

[0064] The following will further illustrate the concept, specific structure and technical effects generated by the present invention with reference to the accompanying drawings, so as to fully understand the purpose, features and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 is the system structure and flowchart of a preferred embodiment of the present invention;

[0066] Figure 2 is a schematic diagram of the relative position relationship between the AUV system and the USBL receiving array constructed in a preferred embodiment of the present invention;

[0067] Figure 3 is a schematic diagram of the USBL coordinate system in a preferred embodiment of the present invention;

[0068] Figure 4 is a schematic diagram of the ECEF, ENU and WGS84 coordinate systems in a preferred embodiment of the present invention;

[0069] Figure 5 is a trajectory image of the IMU data points in the ENU coordinate system in a preferred embodiment of the present invention;

[0070] Figure 6 is a trajectory image of the IMU data points and the screened USBL data points in the ENU coordinate system in a preferred embodiment of the present invention;

[0071] Figure 7Schematic diagram of the geometric relationship and data point selection between the ENU coordinate system and the USBL coordinate system in a preferred embodiment of the present invention;

[0072] Figure 8 Trajectory display result after registration of IMU and USBL data in a preferred embodiment of the present invention;

[0073] Figure 9 Trajectory display result before registration of IMU and USBL data in a preferred embodiment of the present invention;

[0074] Figure 10 Overlay display result of the trajectory after registration of IMU and USBL data and the on-site satellite photo in a preferred embodiment of the present invention. Detailed implementation manners

[0075] The following introduces multiple preferred embodiments of the present invention with reference to the accompanying drawings of the specification to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0076] In the drawings, components with the same structure are denoted by the same numeral labels, and components with similar structures or functions are denoted by similar numeral labels. The size and thickness of each component shown in the drawings are arbitrarily shown, and the present invention does not limit the size and thickness of each component. To make the illustration clearer, the thickness of some components in the drawings is appropriately exaggerated.

[0077] As Figure 1 shown, the trajectory calculation and registration method based on the inertial navigation of an autonomous underwater vehicle and a short baseline positioning sensor provided in this embodiment is based on the IMU and USBL data obtained from the autonomous underwater vehicle navigation experiment, and the implementation steps are as follows:

[0078] Step 1, obtain IMU data and USBL data through an AUV including an inertial navigation device and a short baseline sensor, and screen and process the USBL data.

[0079] Figure 2 Structural design and field experiment results of an AUV in this embodiment. The directions of each axis of the USBL coordinate system are as Figure 3 shown. Due to the influence of environmental interference, fixing methods, and its own working characteristics on the short baseline sensor during the actual experiment, the USBL data obtained by the AUV is relatively unstable and contains many outliers. According to the prior information such as the navigation range, maximum navigation speed, and working state of the AUV vehicle in the actual experiment, the relevant abnormal data in the USBL data is marked as error, and these abnormal data will be deleted later to ensure the reliability of the remaining USBL data. The specific calculation process is as follows:

[0080] Step 1.1. In the X direction, process the data in the following manner:

[0081]

[0082] where x(i) is the X-axis coordinate, i.e., the depth coordinate, of the i-th USBL data point. During the experiment, the maximum diving depth of the AUV is d 0 , so the depth anomaly data points can be marked as error based on this prior information.

[0083] Step 1.2. In the Y direction, process the data in the following manner:

[0084]

[0085] where y(i) is the Y-axis coordinate of the i-th USBL data point. During the experiment, the movement range of the AUV in the Y direction is within half of the width w of the experimental water area 0 , so the anomaly data points in the Y direction can be marked as error based on this prior information.

[0086] Step 1.3. In the Z direction, process the data in the following manner:

[0087]

[0088] where z(i) is the Z-axis coordinate of the i-th USBL data point. During the experiment, the movement range of the AUV in the Z direction is within the length l of the experimental water area 0 , so the anomaly data points in the Z direction can be marked as error based on this prior information.

[0089] Step 1.4. Between adjacent timestamp data points, process the data in the following manner:

[0090]

[0091] During the experiment, the maximum movement speed of the AUV is v max , and the movement state of the object changes continuously. The data changes between adjacent timestamps are generally monotonic and have small variations. Therefore, the speed anomaly data points can be marked as error based on this prior information.

[0092] Step 1.5. According to the above steps 1.1 to 1.4, eliminate the USBL data points marked with error, and at the same time delete the data points where the XYZ coordinates are all 0. This data represents that the ultra-short baseline sensor did not send or receive valid data properly during this period.

[0093] Step 2: Convert the data point coordinates obtained by the IMU from the WGS84 coordinate system to the ENU coordinate system.

[0094] Since the initialization of the IMU data comes from GPS calibration, the data values given by the IMU later are also longitude and latitude values, and the coordinate system is the WGS84 geodetic coordinate system. To facilitate the plotting of the trajectory, it is necessary to convert the data points obtained by the IMU into coordinates in the East-North-Up (ENU) coordinate system with the first valid point measured in the AUV navigation experiment as the origin. The ENU coordinate system is also called the local tangent plane coordinate system, which takes the user's location as the coordinate origin, the E-axis of the coordinate system points to the east, the N-axis points to the north, and the U-axis points to the zenith. The schematic diagram of the relative relationship between the ECEF, ENU, and WGS84 coordinate systems involved in this step is as Figure 4 . Step 2 is mainly divided into the following two steps:

[0095] Step 2.1: Convert the WGS84 coordinate system to the ECEF coordinate system:

[0096] The origin of the ECEF coordinate system is the center of mass of the earth. The X-axis extends through the intersection of the prime meridian (0-degree longitude) and the equator (0-degree latitude), and the Z-axis extends through the North Pole. The Y-axis follows the right-hand coordinate system and passes through the equator and 90-degree longitude. The calculation steps for converting (lon, lat, alt) in the WGS84 coordinate system to the point (X, Y, Z) in the ECEF coordinate system are as follows:

[0097]

[0098] where e is the ellipsoidal eccentricity and τ is the radius of curvature of the reference ellipsoid.

[0099]

[0100] Since in the WGS84 coordinate system, the flattening The relationship between the eccentricity e and the flattening f is:

[0101] e 2 = f(2 - f)

[0102] Therefore, the radius of curvature τ of the reference ellipsoid can also be written as

[0103]

[0104] Step 2.2: Convert the ECEF coordinate system to the ENU coordinate system:

[0105] Let the origin of the user's coordinate be point P 0 =(x 0 , y 0 , z 0 ), and calculate the point P = (x, y, z). At point P 0The position (e, n, u) in the ENU coordinate system with the origin as the coordinate origin. Here, the data of the WGS84 coordinate system is required, and P 0 The WGS84 coordinate point of is LLA 0 =(lon 0 , lat 0 , lat 0 ). The relevant calculation steps are as follows:

[0106]

[0107]

[0108] Among them, the coordinate transformation matrix S:

[0109]

[0110] After converting all IMU data points into coordinates in the ENU, plotting gives the trajectory recorded by the IMU, Figure 5 which shows the trajectory of the IMU data points in the ENU coordinate system.

[0111] Step 3, plot the trajectories of the processed USBL data points and the IMU data points in the ENU coordinate system.

[0112] Step 3 uses the USBL data points obtained by processing in Step 1 and the coordinates of the IMU data points obtained in Step 2 in the ENU coordinate system to plot their unregistered trajectories in the ENU coordinate system. The USBL coordinate system takes the point where the transmitting transducer and the receiving array of the ultra-short baseline sensor are located as the origin, and the direction defined by the ultra-short baseline sensor as the coordinate system. The Y-axis of the USBL coordinate system is relatively close to the E-axis of the ENU coordinate system, and the Z-axis of the USBL coordinate system is relatively close to the N-axis of the ENU coordinate system. And in the actual experiment process, since the diving depth of the AUV changes very little, it is simplified, and only two-dimensional trajectories are considered. Only the Y-axis and Z-axis data of the USBL are used to plot its trajectory in the ENU coordinate system.

[0113] The unregistered IMU and USBL trajectories obtained in Step 3 are as Figure 6 shown. It can be intuitively observed from Figure 6 that due to the angular difference and origin position difference between the USBL coordinate system and the ENU coordinate system, there is an angular difference and a position difference between the two trajectories. Therefore, it is necessary to register the IMU data and the USBL data.

[0114] Step 4, solve the angular and origin position deviations between the ENU coordinate system and the USBL coordinate system.

[0115] As Figure 7As shown in the figure, there is an angular difference γ between the USBL coordinate system and the ENU coordinate system (E-N and Y-Z planes). Since the angular difference corresponding to the slope of the same straight line in different coordinate systems of the same plane is equal to the angular difference of the coordinate axes. To calculate this angular difference, a data point A with stable change and relatively reliable is selected, such as the 225th timestamp data point, and the coordinates of point A in the ENU coordinate system (E-N plane) are calculated as A ENU (-14.88295364, -35.75550466), and the coordinates of point A in the USBL coordinate system (Y-Z plane) are A USBL (-38.0, -57.7); then, according to the longitude and latitude values of the coordinate point O of the ultra-short baseline sensor, the coordinates of point O in the ENU coordinate system are calculated as O ENU (O E , O N ) = (13.16433724, 36.59235146), and the coordinates of point O in the USBL coordinate system are O USBL (0, 0). After obtaining the above four coordinates, the slope and its angle α of the straight line OA in the ENU coordinate system, the slope and its angle β of the straight line OA in the USBL coordinate system can be obtained, and then the angular difference γ between the two coordinate systems can be obtained.

[0116] γ = α - β = 56.63194470° - 43.54190515° = 13.09003955°

[0117] Step 5: Solve the transformation matrix between the ENU coordinate system and the USBL coordinate system, and register the USBL data points and the IMU data points.

[0118] According to the angular difference and origin position deviation between the two coordinate systems obtained in Step 4, Step 5 obtains the transformation matrix between the two coordinate systems, and registers the trajectory of the data points of USBL and IMU. The specific steps are as follows:

[0119] Step 5.1: Rotation transformation: Rotate the USBL coordinate system data points clockwise by an angle γ: Obtain the coordinates (x 1 (i), y 1 (i)) in the USBL coordinate system

[0120]

[0121] where x USBL (i) and y USBL (i) represent the X-axis and Y-axis data of the USBL data point i in the USBL coordinate system respectively.

[0122] Step 5.2, Translation Transformation: Translate the data coordinates obtained in the above Step 5.1 to obtain the coordinates (x 2 (i), y 2 (i))

[0123]

[0124] Step 5.3, Transformation Matrix: According to the above Steps 5.1 and 5.2, obtain the rotation matrix R and the translation matrix T, and obtain the augmented coordinate system transformation matrix C.

[0125] Rotation Matrix:

[0126]

[0127] Translation Matrix:

[0128]

[0129] That is:

[0130]

[0131] For convenient expression, the above formula can be written as the augmented coordinate system transformation matrix:

[0132]

[0133] That is:

[0134]

[0135] Thus, the coordinates of the USBL data points in the ENU coordinate system can be obtained.

[0136] Figure 8 shows the trajectory image of the present invention after the registration of IMU and USBL data. For comparison, Figure 9 shows the trajectory image before the registration of IMU and USBL.

[0137] Step 6, Plot the trajectory after the registration of the data points and superimpose it on the corresponding position of the satellite photo.

[0138] According to the data obtained in Step 5, Step 6 plots the trajectories of the registered IMU and USBL data. Using the satellite photo and the relevant GPS coordinate values, the plotted trajectory is superimposed on the actual on-site satellite photo. This further proves the effectiveness of the data processing and the authenticity of the trajectory.

[0139] Figure 10 shows the superposition display result of the trajectory of the present invention based on the registration of IMU and USBL data and the on-site satellite photo.

[0140] The trajectory calculation and registration method based on the inertial navigation of an autonomous underwater vehicle and an ultra-short baseline positioning sensor provided in this embodiment aims at the problems existing in the current ultra-short baseline sensor and IMU device respectively. By using the prior information in actual experiments, the data is effectively screened. Based on the geometric relationship between coordinate systems, the connection between the IMU and USBL data points is effectively established, and the data of the two is registered. The corresponding trajectory is drawn based on the registered data, and finally, the satellite photo at the experimental site is superimposed with the trajectory. This method accurately calculates the navigation trajectory of the AUV, effectively improves the AUV position perception performance, and improves the trajectory monitoring ability of the AUV control personnel. This method only uses a small amount of necessary prior information, and can make the most of the normal USBL data after various interferences are received by the ultra-short baseline sensor, and cooperate with the data generated by the IMU to complete the AUV trajectory solution and registration.

[0141] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations according to the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field based on the concept of the present invention through logical analysis, reasoning or limited experiments on the basis of the prior art should be within the protection scope determined by the claims.

Claims

1. A trajectory calculation and registration method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor, It is characterized in that The following steps are involved: Step 1: Obtain IMU data and USBL data through an autonomous underwater robot including an inertial navigation device and an ultra-short baseline sensor, filter and process the USBL data, and process the abnormal data of the USBL data according to the prior information of the AUV vehicle in the actual experiment to ensure the reliability of the remaining USBL data. The specific operation process is as follows: Step 1.1: In the X direction, process the data in the following way: where x(i) is the X-axis coordinate of the i-th USBL data point, i.e., the depth coordinate; d 0 is the maximum diving depth of the AUV during the experiment. According to d 0 mark the depth anomaly data points as error Step 1.2: In the Y direction, process the data in the following way: Among them, y(i) is the Y-axis coordinate of the i-th USBL data point; during the experiment of the AUV, the movement range in the Y direction is within half of the width w of the experimental water area. According to w 0 / 2, mark the abnormal data points in the Y direction as error, 0 ​ Step 1.3: In the Z direction, process the data in the following way: where z(i) is the Z-axis coordinate of the i-th USBL data point; during the experiment, the AUV moves within the length l of the experimental water area in the Z direction 0 within, according to l 0 mark the abnormal data points in the Z direction as error Step 1.4: Between adjacent timestamp data points, process the data in the following way: Among them, v max is the maximum movement speed of the AUV during the experiment. According to v max mark the speed anomaly data points as error, Step 1.5, according to the above steps 1.1 to 1.4, remove the USBL data points marked as error; Step 2: Convert the coordinates of the IMU data points from the WGS84 coordinate system to the ENU coordinate system; Step 3: Draw the trajectories of the processed USBL data points and IMU data points in the ENU coordinate system; Step 4, solve the angle and origin position deviation between the ENU coordinate system and the USBL coordinate system; Step 5, solve the transformation matrix between the ENU coordinate system and the USBL coordinate system, and align the USBL data points with the IMU data points; Step 6: Draw the trajectory of the aligned IMU and USBL data in the ENU coordinate system, and use satellite photos and related GPS coordinate values ​​to overlay the drawn trajectory with the actual on-site satellite photos.

2. The trajectory calculation and registration method based on the autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor according to claim 1, It is characterized in that The step 1 also includes: deleting data points whose X, Y, and Z coordinates are all 0, which indicates that the ultra-short baseline sensor did not normally send or receive valid data during this period.

3. The trajectory calculation and registration method based on the autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor according to claim 1, It is characterized in that The step 2 specifically includes: Step 2.1, convert the coordinates of the IMU data points from the WGS84 coordinate system to the ECEF coordinate system; Step 2.2: Convert the IMU data points in the ECEF coordinate system to the ENU coordinate system.

4. The trajectory calculation and registration method based on the autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor according to claim 3, It is characterized in that The step 2.1 specifically includes: the origin of the ECEF coordinate system is the center of mass of the earth, the X axis extends through the intersection of the prime meridian (0 degrees longitude) and the equator (0 degrees latitude), the Z axis extends through the North Pole, and the Y axis follows the right-hand coordinate system, passing through the equator and 90 degrees longitude; the calculation steps for converting (lon, lat, alt) in the WGS84 coordinate system to the point (X, Y, Z) in the ECEF coordinate system are: Where e is the eccentricity of the ellipsoid, and τ is the radius of curvature of the reference ellipsoid; Where a is the equatorial radius and b is the polar radius.

5. The trajectory calculation and registration method based on the inertial navigation of an autonomous underwater vehicle and an ultra-short baseline positioning sensor according to claim 3, characterized in that, The specific content of step 2.2 is as follows: The origin of coordinates of the user is P 0 =(x 0 ,y 0 ,z 0 ), the calculated point is P=(x, y, z). Calculate the position (e, n, u) in the ENU coordinate system with point P 0 as the origin of coordinates. The WGS84 coordinate point of P 0 is LLA 0 =(lon 0 ,lat 0 ,lat 0 ). The calculation steps are as follows: wherein, S is the coordinate transformation matrix:

6. The trajectory calculation and registration method based on the inertial navigation of an autonomous underwater vehicle and an ultra-short baseline positioning sensor according to claim 1, characterized in that, in step 3, the Y-axis and Z-axis data of the USBL obtained in step 1 and the coordinates of the IMU data points in the ENU coordinate system obtained in step 2 are used to plot the unregistered trajectories of the two in the ENU coordinate system.

7. The trajectory calculation and registration method based on the inertial navigation of an autonomous underwater vehicle and an ultra-short baseline positioning sensor according to claim 1, characterized in that, step 4 specifically includes: Step 4.1: Select a data point A with stable changes and calculate the coordinates of point A in the ENU coordinate system, A ENU , and the coordinates of data point A in the USBL coordinate system, A USBL ; Step 4.2: Then, based on the longitude and latitude values of the ultra-short baseline sensor coordinate point O, calculate the coordinates of point O in the ENU coordinate system, O ENU (O E ,O N ), and the coordinates of point O in the USBL coordinate system, O USBL (0, 0); Step 4.3: Obtain the slope and its angle α of the straight line OA in the ENU coordinate system and the slope and its angle β of the straight line OA in the USBL coordinate system through the above four coordinates, and then obtain the angle and the origin position deviation value γ between the ENU coordinate system and the USBL coordinate system: γ = α - β.

8. The trajectory calculation and registration method based on the inertial navigation of an autonomous underwater vehicle and an ultra-short baseline positioning sensor according to claim 1, characterized in that, step 5 specifically includes: Step 5.1: Rotation transformation: Rotate the USBL coordinate system data points clockwise by an angle γ: where x USBL (i) and y USBL (i) respectively represent the X-axis and Y-axis data of the USBL data point i in the USBL coordinate system. (x 1 (i), y 1 (i)) is the coordinate of the USBL data point i in the USBL coordinate system after rotation transformation: Step 5.2, translation transformation: Translate the data coordinates obtained in the above Step 5.1 to obtain the coordinates (x 2 (i), y 2 (i)) of the USBL data points in the ENU coordinate system: Step 5.3: Solve the transformation matrix: According to the above steps 5.1 and 5.2, obtain the rotation matrix R and the translation matrix T, and obtain the augmented transformation matrix C: The rotation matrix is: The translation matrix is: The augmented transformation matrix: Through the following formula: The coordinates (x 2 , y 2 ) of the USBL data points in the ENU coordinate system can be obtained.

Images