CN114993313A - Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor - Google Patents

Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor Download PDF

Info

Publication number
CN114993313A
CN114993313A CN202210548232.5A CN202210548232A CN114993313A CN 114993313 A CN114993313 A CN 114993313A CN 202210548232 A CN202210548232 A CN 202210548232A CN 114993313 A CN114993313 A CN 114993313A
Authority
CN
China
Prior art keywords
usbl
coordinate system
data
enu
coordinate
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210548232.5A
Other languages
Chinese (zh)
Inventor
潘汉
袁煜
敬忠良
彭湃
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202210548232.5A priority Critical patent/CN114993313A/en
Publication of CN114993313A publication Critical patent/CN114993313A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses a trajectory resolving and registering method based on an autonomous underwater robot inertial navigation and an ultra-short baseline positioning sensor, wherein IMU data and USBL data are obtained through an autonomous underwater robot comprising an inertial navigation device and an ultra-short baseline positioning sensor, and USBL data are screened and processed; converting the IMU data point coordinates from a WGS84 coordinate system into an ENU coordinate system; drawing the tracks of the processed USBL data points and IMU data points in an ENU coordinate system; solving the deviation of the angle and the origin position between the ENU coordinate system and the USBL coordinate system; solving a transformation matrix between an ENU coordinate system and a USBL coordinate system, and registering the USBL data point and the IMU data point; and drawing the tracks of the registered IMU and USBL data in an ENU coordinate system, and superposing the drawn tracks and actual on-site satellite photos by using the satellite photos and the related GPS coordinate values. The method provided by the invention can provide accurate AUV motion trail for AUV control personnel and provide effective information for AUV task execution while verifying the accuracy of the proposed trail solution algorithm.

Description

Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor
Technical Field
The invention relates to the technical field of underwater autonomous positioning navigation, in particular to a track resolving and registering method based on autonomous underwater robot inertial navigation and an ultra-short baseline positioning sensor.
Background
AUV (Autonomous Underwater robot) is widely researched and applied in recent years as a new Underwater exploration and development platform. The trajectory measurement method of the AUV can provide accurate reference for underwater navigation positioning of the AUV, so that the AUV can perform specific tasks underwater, and an actual motion trajectory of the AUV is provided for an operator.
Through the literature search of the prior art, limited by low visibility under water, low lighting conditions and large electromagnetic wave attenuation rate, currently commonly used AUV underwater navigation devices include an IMU (Inertial Measurement Unit), an USBL (ultra short Baseline positioning system), and the like. The IMU generally includes three single-axis gyroscopes and three single-axis accelerometers, which measure the attitude and acceleration of the carrier platform through the gyroscopes and accelerometers, respectively, and obtain final position and attitude information based on initial coordinates. The IMU is mounted on the AUV, requires initial calibration before use, and has problems such as gyro constant offset and accelerometer constant drift, and the positioning accuracy thereof decreases as the operating time increases. The USBL is composed of a transmitting transducer, a receiving array and a transponder, the transmitting transducer and the receiving array are located at reference positions such as a ship or a shore, and the transponder is located on a moving carrier such as an AUV. The USBL transmits acoustic pulses through the transmitting transducer, the responder positioned on the AUV transmits back response acoustic pulses after receiving the response acoustic pulses, the receiving array receives the signals, and the AUV position information of the responder is obtained by decoding the signals. The relative position of the AUV can be obtained by the USBL system, but the USBL matrix is prone to position and angle offset, which results in large positioning deviation. The relative position relationship between AUV and USBL in the experiment is shown in FIG. 2.
Therefore, those skilled in the art are dedicated to develop a trajectory solution and registration method based on the autonomous underwater robot inertial navigation and the ultra-short baseline positioning sensor.
Disclosure of Invention
In view of the above defects in the prior art, the technical problem to be solved by the invention is how to solve the problem that the position and angle offset of the AUV underwater navigation device USBL is easy to generate and a large positioning deviation is generated, and the research and development and the improvement of the autonomous underwater robot trajectory solving system provide an accurate AUV motion trajectory for AUV control personnel.
In order to achieve the aim, the invention provides a track resolving and registering method based on autonomous underwater robot inertial navigation and an ultra-short baseline positioning sensor, which comprises the following steps:
step 1, obtaining IMU data and USBL data through an autonomous underwater robot comprising inertial navigation equipment and an ultra-short baseline sensor, and screening and processing the USBL data;
step 2, converting the IMU data point Coordinates from a WGS84(1984 World Geodetic System 1984) coordinate System into an ENU coordinate System (station center coordinate System, Local East, North, Up Coordinates);
step 3, drawing the tracks of the processed USBL data points and IMU data points in an ENU coordinate system;
step 4, solving the deviation of the angle between the ENU coordinate system and the USBL coordinate system and the position of the origin;
step 5, solving a transformation matrix between the ENU coordinate system and the USBL coordinate system, and registering the USBL data points and the IMU data points;
and 6, drawing the track of the registered IMU and USBL data in an ENU coordinate System, and superposing the drawn track and an actual on-site satellite photo by using the coordinate values of the satellite photo and a related GPS (Global Positioning System).
Further, in step 1, abnormal data of the USBL data are processed according to prior information of the AUV aircraft in practical experiments, and reliability of the remaining USBL data is ensured.
Further, the specific operation process in step 1 is as follows:
step 1.1, in the X direction, processing data in the following mode:
Figure BDA0003650391430000021
wherein X (i) is the X-axis coordinate, i.e., depth coordinate, of the ith USBL data point; d is a radical of 0 Maximum depth of submersion of AUV during the course of the experiment, according to d 0 Labeling the depth anomaly data points as error;
step 1.2, processing the data in the Y direction in the following way:
Figure BDA0003650391430000022
wherein Y (i) is the Y-axis coordinate of the ith USBL data point; in the experiment process of the AUV, the motion range in the Y direction is the width w of the experiment water area 0 Within/2, according to w 0 Marking abnormal data points in the Y direction as error;
step 1.3, processing the data in the Z direction in the following way:
Figure BDA0003650391430000023
wherein Z (i) is the Z-axis coordinate of the ith USBL data point; in the experiment process of the AUV, the movement range in the Z direction is the length l of the experiment water area 0 In accordance with l 0 Marking abnormal data points in the Z direction as error;
step 1.4, processing the data between adjacent time stamp data points in the following way:
Figure BDA0003650391430000024
wherein v is max Maximum speed of movement of AUV during the experiment according to v max Marking the speed abnormal data points as error;
step 1.5, according to the above steps 1.1 to 1.4, USBL data points labeled error are rejected.
Further, step 1 further comprises: data in which XYZ coordinates in the data points are all 0 values are deleted, which represents that valid data is not normally transmitted or received by the ultra-short baseline sensor during this period.
Further, step 2 specifically includes:
step 2.1, transferring the IMU data point coordinates from a WGS84 coordinate system to an ECEF coordinate system (Earth-Earth Fixed rectangular coordinate system, Earth-Centered, Earth-Fixed);
and 2.2, transferring the IMU data points in the ECEF coordinate system to an ENU coordinate system.
Further, 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 point of the meridian (0-degree longitude) and the equator (0-degree latitude), the Z axis extends through the north pole, and the Y axis follows the right-hand coordinate system and passes through the equator and the 90-degree longitude; the calculation step of converting (lon, lat, alt) in the WGS84 coordinate system into a point (X, Y, Z) in the ECEF coordinate system comprises the following steps:
Figure BDA0003650391430000031
wherein e is the eccentricity of the ellipsoid, and tau is the curvature radius of the reference ellipsoid;
Figure BDA0003650391430000032
where a is the equatorial radius and b is the polar radius.
Further, step 2.2 specifically includes: the origin of coordinates where the user is located is P 0 ＝(x 0 ,y 0 ,z 0 ) The calculation point is P ═ x, y, z, at the point P 0 ENU coordinate System position (e, n, u), P, as the origin of coordinates 0 The WGS84 coordinate point is LLA 0 ＝(lon 0 ,lat 0 ,lat 0 ) The calculation steps are as follows:
Figure BDA0003650391430000033
Figure BDA0003650391430000034
wherein S is a coordinate transformation matrix:
Figure BDA0003650391430000035
further, in step 3, a trajectory which is not registered in the ENU coordinate system is drawn by using the Y-axis and Z-axis data of the USBL obtained in step 1 and the coordinates of the IMU data point obtained in step 2 in the ENU coordinate system.
Further, step 4 specifically includes:
step 4.1, selecting a data point A with stable change, and calculating the coordinate A of the point A in the ENU coordinate system ENU Coordinate A of data point A in USBL coordinate system USBL ；
Step 4.2, calculating the coordinate O of the point O in the ENU coordinate system according to the longitude and latitude value of the USBL equipment coordinate point O ENU (O E ,O N ) And the coordinates O of the point O in the USBL coordinate system USBL (0,0)；
Step 4.3, obtaining the slope and the angle alpha of the straight line OA in the ENU coordinate system through the four coordinates, obtaining the slope and the angle beta of the straight line OA in the USBL coordinate system, and then obtaining the deviation value gamma of the angle between the ENU coordinate system and the USBL coordinate system and the origin position: γ ═ α - β.
Further, step 5 specifically includes:
step 5.1, rotation transformation: rotating the USBL coordinate coefficients by gamma angle according to the point clockwise:
Figure BDA0003650391430000041
wherein x is USBL (i) And y USBL (i) Respectively represent 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 coordinate system after the USBL data point i is subjected to rotation transformation.
Step 5.2, translation transformation: translating the data coordinate obtained in the step 5.1 to obtain a USBL data point at ECoordinates (x) in NU coordinate system 2 (i),y 2 (i))：
Figure BDA0003650391430000042
Step 5.3, solving a transformation matrix: according to the step 5.1 and the step 5.2, obtaining a rotation matrix R and a translation matrix T, and obtaining an augmentation transformation matrix C:
the rotation matrix is:
Figure BDA0003650391430000043
the translation matrix is:
Figure BDA0003650391430000044
augmented transformation matrix:
Figure BDA0003650391430000045
by the following formula:
Figure BDA0003650391430000051
the coordinate (x) of the USBL data point in the ENU coordinate system can be obtained 2 ,y 2 )。
Compared with the prior art, the scheme of the invention has the following beneficial effects: the invention realizes the research, development and improvement of the autonomous underwater robot track solving system, and eliminates USBL abnormal data through a data screening algorithm; based on the geometric relation between the coordinate systems and the data distribution condition, solving to obtain a transformation matrix between the coordinate systems, and providing effective information for registration of USBL and IMU data; the satellite picture and the existing GPS information are used for displaying the solving track in a superposition mode, the accuracy of the proposed track solving algorithm is verified, meanwhile, an accurate AUV motion track and a more visual track monitoring interface are provided for AUV control personnel, and effective information is provided for AUV task execution.
The conception, the specific structure and the technical effects of the present invention will be further described with reference to the accompanying drawings to fully understand the objects, the features and the effects of the present invention.
Drawings
FIG. 1 is a system architecture and flow diagram of a preferred embodiment of the present invention;
fig. 2 is a schematic diagram illustrating a relative position relationship between the constructed AUV system and the USBL receiving array according to a preferred embodiment of the present invention;
FIG. 3 is a schematic diagram of the USBL coordinate system in accordance with a preferred embodiment of the present invention;
FIG. 4 is a schematic diagram of the ECEF, ENU and WGS84 coordinate systems of a preferred embodiment of the invention;
FIG. 5 is a trace image based on IMU data points in the ENU coordinate system according to a preferred embodiment of the present invention;
FIG. 6 is a trajectory image based on IMU data points and filtered USBL data points in an ENU coordinate system according to a preferred embodiment of the present invention;
FIG. 7 is a schematic diagram of geometrical relationship between the ENU coordinate system and the USBL coordinate system and data point selection according to a preferred embodiment of the present invention;
FIG. 8 is a trace display result after registration of IMUs and USBL data according to a preferred embodiment of the present invention;
FIG. 9 is a diagram illustrating the display of the trajectory before registration based on IMU and USBL data in accordance with a preferred embodiment of the present invention;
fig. 10 is a display result of the superposition of the trajectory and the live satellite photograph after the registration of the IMU and the USBL data according to a preferred embodiment of the present invention.
Detailed Description
The technical contents of the preferred embodiments of the present invention will be made clear and easily understood by referring to the drawings attached to the specification. The present invention may be embodied in many different forms of embodiments and the scope of the invention is not limited to the embodiments set forth herein.
In the drawings, elements that are structurally identical are represented by like reference numerals, and elements that are structurally or functionally similar in each instance are represented by like reference numerals. The size and thickness of each component shown in the drawings are arbitrarily illustrated, and the present invention is not limited to the size and thickness of each component. The thickness of the components may be exaggerated where appropriate in the figures to improve clarity.
As shown in fig. 1, the trajectory calculation and registration method based on the autonomous underwater vehicle inertial navigation and the ultra-short baseline positioning sensor provided in this embodiment is implemented based on IMU and USBL data obtained by an autonomous underwater vehicle navigation experiment, and includes the following steps:
step 1, obtaining IMU data and USBL data through AUV including inertial navigation equipment and an ultra-short baseline sensor, and screening and processing the USBL data.
Fig. 2 shows the structural design and the external field experiment result of an AUV of this embodiment. The USBL coordinate system is shown in fig. 3 for each axis direction. In the actual experiment process, due to the fact that the ultra-short baseline sensor is interfered by the environment, fixed and limited by the working characteristics of the ultra-short baseline sensor, the USBL data obtained by the AUV is unstable and has many abnormal values. According to the navigation range, the maximum navigation speed, the working state and other prior information of the AUV aircraft in an actual experiment, relevant abnormal data in the USBL data are marked as error, and the abnormal data are deleted at the later stage, so that the reliability of the rest USBL data is ensured. The specific operation process is as follows:
step 1.1, in the X direction, processing data in the following mode:
Figure BDA0003650391430000061
where X (i) is the X-axis coordinate, i.e., depth coordinate, of the ith USBL data point. In the experimental process of AUV, the maximum diving depth is d 0 The depth anomaly data point can therefore be labeled as error according to this a priori information.
Step 1.2, processing the data in the Y direction in the following way:
Figure BDA0003650391430000062
where Y (i) is the Y-axis coordinate of the ith USBL data point. In the experiment process of the AUV, the motion range in the Y direction is the width w of the experiment water area 0 Within/2, abnormal data points in the Y direction can therefore be labeled as error according to this a priori information.
Step 1.3, processing the data in the Z direction in the following way:
Figure BDA0003650391430000063
where Z (i) is the Z-axis coordinate of the ith USBL data point. In the experiment process of the AUV, the movement range in the Z direction is the length l of the experiment water area 0 Therefore, the abnormal data point in the Z direction can be labeled as error according to the prior information.
Step 1.4, processing the data between adjacent time stamp data points in the following way:
Figure BDA0003650391430000071
in the experimental process, the maximum movement speed of the AUV is v max And the motion state of the object changes continuously, and the data change of the close time stamps is generally monotonous and has small change, so that the speed abnormal data point can be marked as error according to the prior information.
Step 1.5, according to the steps 1.1 to 1.4, eliminating the USBL data points showing error, and deleting the data of which the XYZ coordinates are all 0 values in the data points, wherein the data represents that the ultra-short baseline sensor does not normally send or receive effective data in the period.
And 2, converting the data point coordinates obtained by the IMU from a WGS84 coordinate system to an ENU coordinate system.
Because the IMU data initialization is derived from GPS calibration, the data values given after the IMU are also latitude and longitude values and are in a WGS84 geodetic coordinate system. To facilitate the trajectory mapping, the data points obtained by the IMU need to be converted into coordinates in an northeast heaven (ENU) coordinate system with the first effective point measured in the AUV navigation experiment as the origin. The ENU coordinate system is also called a station center coordinate system, and takes the position of the user as the origin of coordinates, the E axis of the coordinate system points to the east side, the N axis points to the north side, and the U axis points to the zenith. The relative relationship between the coordinate systems of ECEF, ENU and WGS84 involved in this step is schematically shown in fig. 4. The step 2 is mainly divided into the following two steps:
step 2.1, converting the WGS84 coordinate system into an ECEF coordinate system:
the origin of the ECEF coordinate system is the earth's centroid, the X-axis extends through the intersection of the meridian (0 degrees longitude) and the equator (0 degrees latitude), and the Z-axis extends through the north pole. The Y-axis follows the right-hand coordinate system, passing through the equator and 90 degrees longitude. The calculation step of converting (lon, lat, alt) in the WGS84 coordinate system into a point (X, Y, Z) in the ECEF coordinate system comprises the following steps:
Figure BDA0003650391430000072
wherein e is the eccentricity of the ellipsoid, and τ is the curvature radius of the reference ellipsoid.
Figure BDA0003650391430000073
Very flat rate due to WGS84 coordinate system
Figure BDA0003650391430000074
The relationship between eccentricity e and very-flat ratio f is:
e 2 ＝f(2-f)
thus, the radius of curvature τ of the reference ellipsoid can also be written as
Figure BDA0003650391430000081
Step 2.2, transferring the ECEF coordinate system to the ENU coordinate system:
let the origin of coordinates P of the user 0 ＝(x 0 ,y 0 ,z 0 ) The calculation point P is (x, y, z). At the point P 0 ENU coordinate system position (e, n, u) as the origin of coordinates. Here, the data, P, using the WGS84 coordinate system is required 0 The WGS84 coordinate point is LLA 0 ＝(lon 0 ,lat 0 ,lat 0 ). The correlation calculation steps are as follows:
Figure BDA0003650391430000082
Figure BDA0003650391430000083
wherein the coordinate transformation matrix S:
Figure BDA0003650391430000084
after all the IMU data points are converted into the ENU coordinates, the trajectory recorded by the IMU is obtained by plotting, and fig. 5 shows the trajectory of the IMU data points in the ENU coordinate system.
And 3, drawing the tracks of the processed USBL data points and IMU data points in an ENU coordinate system.
And 3, drawing a track of the USBL data point obtained by the processing in the step 1 and the coordinate of the IMU data point obtained in the step 2 in the ENU coordinate system without registration in the ENU coordinate system. The USBL coordinate system takes the point of a transmitting transducer and a receiving base array of the ultra-short baseline sensor as an origin, the direction defined by the ultra-short baseline sensor as a coordinate system, the Y axis of the USBL coordinate system is closer to the E axis of the ENU coordinate system, the Z axis of the USBL coordinate system is closer to the N axis of the ENU coordinate system, and in the actual experiment process, the AUV diving depth change is small, so that the USBL coordinate system is simplified, and only a two-dimensional track is considered. And drawing the track of the USBL under the ENU coordinate system by using only the Y-axis data and the Z-axis data of the USBL.
The unregistered IMU and USBL trajectories obtained in step 3 are shown in fig. 6. As can be intuitively observed from fig. 6, due to the angular difference and the origin position difference between the USBL coordinate system and the ENU coordinate system, the trajectories of the USBL coordinate system and the ENU coordinate system are different by an angle and a position difference, so that the IMU data and the USBL data need to be registered.
And 4, solving the deviation of the angle between the ENU coordinate system and the USBL coordinate system and the position of the origin.
As shown in FIG. 7, there is an angular difference γ between the USBL coordinate system and the ENU coordinate system (E-N and Y-Z plane). Because the angle difference corresponding to the slope of the same straight line under different coordinate systems on the same plane is equal to the angle difference of the coordinate axes. To calculate the angular difference, a stable and relatively reliable data point A is selected, such as the 225 th time stamp data point, and the coordinate A of the data point A in the ENU coordinate system (E-N plane) is calculated ENU (-14.88295364, -35.75550466), coordinate A of point A in USBL coordinate system (Y-Z plane) 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 coordinate O of the point O in the ENU coordinate system is calculated ENU (O E ,O N ) (13.16433724,36.59235146), and the coordinate O of point O in the USBL coordinate system USBL (0,0). After obtaining the above four coordinates, the slope and angle α of the line OA in the ENU coordinate system, the slope and angle β of the line OA in the USBL coordinate system, and then the angle difference γ between the two coordinate systems can be obtained.
γ＝α-β＝56.63194470°-43.54190515°＝13.09003955°
And 5, solving a transformation matrix between the ENU coordinate system and the USBL coordinate system, and registering the USBL data point and the IMU data point.
And 5, solving the obtained angle difference value and the original point position deviation of the two coordinate systems according to the step 4 to obtain a transformation matrix between the two coordinate systems, and registering the data point tracks of the USBL and the IMU. The method comprises the following specific steps:
step 5.1, rotation transformation: rotating the USBL coordinate coefficients by gamma angle according to the point clockwise: obtaining the coordinate (x) under the USBL coordinate system 1 (i),y 1 (i))
Figure BDA0003650391430000091
Wherein x USBL (i) And y USBL (i) Respectively representing X-axis and Y-axis data of the USBL data point i in the USBL coordinate system.
Step 5.2, translation transformation: translating the data coordinate obtained in the step 5.1 to obtain the coordinate (x) of the USBL data point in the ENU coordinate system 2 (i),y 2 (i))
Figure BDA0003650391430000092
Step 5.3, transforming the matrix: and according to the step 5.1 and the step 5.2, obtaining a rotation matrix R and a translation matrix T, and obtaining an augmented coordinate system transformation matrix C.
Rotating the matrix:
Figure BDA0003650391430000093
translation matrix:
Figure BDA0003650391430000094
namely, the method comprises the following steps:
Figure BDA0003650391430000095
for convenience of expression, the above formula can be written as an augmented coordinate system transformation matrix:
Figure BDA0003650391430000096
namely, the method comprises the following steps:
Figure BDA0003650391430000101
the coordinates of the USBL data point in the ENU coordinate system can be obtained.
Fig. 8 shows a trajectory image of the present invention after registration based on IMU and USBL data, and fig. 9 shows a trajectory image before registration of IMU and USBL, for comparison.
And 6, drawing the track after the data points are registered, and superposing the track with the position corresponding to the satellite photo.
And 6, drawing according to the data obtained in the step 5 to obtain the registered IMU and USBL data tracks. And overlapping the drawn track and the actual on-site satellite photo by using the satellite photo and the related GPS coordinate value. Further proving the effectiveness of data processing and the authenticity of the track.
Fig. 10 shows the trajectory and on-site satellite photo overlay display result after registration based on IMU and USBL data.
According to the track calculating and registering method based on the autonomous underwater robot inertial navigation and the ultra-short baseline positioning sensor, aiming at the problems of the current ultra-short baseline sensor and the IMU equipment, the prior information in an actual experiment is utilized to effectively screen data, the connection between the IMU and the USBL data point is effectively established based on the geometric relation between coordinate systems, the data of the IMU and the USBL data point are registered, the corresponding track is drawn based on the registered data, and finally the satellite photo and the track in the experiment site are combined to be superposed. The method accurately calculates the navigation track of the AUV, effectively improves the position sensing performance of the AUV, and improves the track monitoring capability of AUV control personnel. According to the method, only a small amount of necessary prior information is utilized, after the ultra-short baseline sensor receives various interferences, the USBL normal data can be utilized to the maximum extent, and AUV trajectory solving and registering are completed in cooperation with data generated by the IMU.
The foregoing detailed description of the preferred embodiments of the invention has been presented. It should be understood that numerous modifications and variations could be devised by those skilled in the art in light of the present teachings without departing from the inventive concepts. Therefore, the technical solutions available to those skilled in the art through logic analysis, reasoning and limited experiments based on the prior art according to the concept of the present invention should be within the scope of protection defined by the claims.

Claims (10)

1. A track resolving and registering method based on autonomous underwater robot inertial navigation and an ultra-short baseline positioning sensor is characterized by comprising the following steps:
step 1, obtaining IMU data and USBL data through an autonomous underwater robot comprising inertial navigation equipment and an ultra-short baseline sensor, and screening and processing the USBL data;
step 2, converting the IMU data point coordinates from a WGS84 coordinate system into an ENU coordinate system;
step 3, drawing the tracks of the processed USBL data points and IMU data points in an ENU coordinate system;
step 4, solving the deviation between the angle between the ENU coordinate system and the USBL coordinate system and the position of the origin;
step 5, solving a transformation matrix between the ENU coordinate system and the USBL coordinate system, and registering the USBL data point and the IMU data point;
and 6, drawing the tracks of the registered IMU and USBL data in an ENU coordinate system, and superposing the drawn tracks and actual on-site satellite photos by using the satellite photos and the relevant GPS coordinate values.
2. The autonomous underwater vehicle inertial navigation and ultra-short baseline positioning sensor-based trajectory calculation and registration method according to claim 1, wherein in step 1, abnormal data of the USBL data is processed according to prior information of the AUV vehicle in an actual experiment, so as to ensure reliability of the remaining USBL data.
3. The method for resolving and registering the trajectory based on the autonomous underwater robot inertial navigation and the ultra-short baseline positioning sensor according to claim 2, wherein the specific operation process in the step 1 is as follows:
step 1.1, in the X direction, processing data in the following mode:
Figure FDA0003650391420000011
wherein X (i) is the X-axis coordinate, i.e., depth coordinate, of the ith USBL data point; d 0 Maximum depth of submersion of AUV during the course of the experiment, according to d 0 Labeling the depth anomaly data points as error;
step 1.2, processing the data in the Y direction in the following way:
Figure FDA0003650391420000012
wherein Y (i) is the Y-axis coordinate of the ith USBL data point; in the experiment process of the AUV, the motion range in the Y direction is the width w of the experiment water area 0 Within/2, according to w 0 Marking abnormal data points in the Y direction as error;
step 1.3, processing the data in the Z direction in the following way:
Figure FDA0003650391420000013
wherein Z (i) is the Z-axis coordinate of the ith USBL data point; in the experiment process of the AUV, the movement range in the Z direction is the length l of the experiment water area 0 In accordance with l 0 Marking abnormal data points in the Z direction as error;
step 1.4, processing the data between adjacent time stamp data points in the following way:
Figure FDA0003650391420000021
wherein v is max Maximum speed of movement of AUV during the experiment according to v max Marking the speed abnormal data point as error;
step 1.5, according to the above steps 1.1 to 1.4, USBL data points labeled error are rejected.
4. The method for resolving and registering the trajectory based on the autonomous underwater robot inertial navigation and the ultra-short baseline positioning sensor according to claim 3, wherein the step 1 further comprises: data for which the X, Y, Z coordinates in the data points are all 0 values are deleted, which represents that valid data was not normally sent or received by the ultra-short baseline sensor during this period.
5. The method for calculating and registering the trajectory based on the autonomous underwater vehicle inertial navigation and the ultra-short baseline positioning sensor according to claim 1, wherein the step 2 specifically comprises:
2.1, converting the IMU data point coordinate from a WGS84 coordinate system to an ECEF coordinate system;
and 2.2, transferring the IMU data points in the ECEF coordinate system to an ENU coordinate system.
6. The method for resolving and registering the trajectory based on the autonomous underwater robot inertial navigation and the ultra-short baseline positioning sensor according to claim 5, wherein the step 2.1 specifically comprises: the origin of the ECEF coordinate system is the center of mass of the earth, the X axis extends through the intersection point of the meridian (0-degree longitude) and the equator (0-degree latitude), the Z axis extends through the north pole, and the Y axis follows the right-hand coordinate system and passes through the equator and the 90-degree longitude; the calculation step of converting (lon, lat, alt) in the WGS84 coordinate system into a point (X, Y, Z) in the ECEF coordinate system comprises the following steps:
Figure FDA0003650391420000022
wherein e is the eccentricity of the ellipsoid, and tau is the curvature radius of the reference ellipsoid;
Figure FDA0003650391420000023
where a is the equatorial radius and b is the polar radius.
7. The trajectory calculation and registration method based on the autonomous underwater vehicle inertial navigation and the ultra-short baseline positioning sensor according to claim 5, wherein the step 2.2 specifically comprises: the origin of coordinates where the user is located is P 0 ＝(x 0 ,y 0 ,z 0 ) The calculation point is P ═ (x, y, z), at point P 0 ENU coordinate System position (e, n, u), P, as the origin of coordinates 0 The WGS84 coordinate point is LLA 0 ＝(lon 0 ,lat 0 ,lat 0 ) The calculation steps are as follows:
Figure FDA0003650391420000031
Figure FDA0003650391420000032
wherein S is a coordinate transformation matrix:
Figure FDA0003650391420000033
8. the method for resolving and registering the trajectory based on the autonomous underwater robot inertial navigation and the ultra-short baseline positioning sensor according to claim 1, wherein in the step 3, the trajectory which is not registered in the ENU coordinate system is drawn by using the Y-axis and Z-axis data of the USBL obtained in the step 1 and the coordinates of the IMU data points obtained in the step 2 in the ENU coordinate system.
9. The method for calculating and registering the trajectory based on the autonomous underwater vehicle inertial navigation and the ultra-short baseline positioning sensor according to claim 1, wherein the step 4 specifically comprises:
step 4.1, selecting a data point A with stable change, and calculating the coordinate A of the point A in the ENU coordinate system ENU Data point A at USBL coordinatesCoordinate under system A USBL ；
Step 4.2, calculating the coordinate O of the point O in the ENU coordinate system according to the longitude and latitude value of the coordinate point O of the ultra-short baseline sensor ENU (O E ,O N ) And the coordinates O of the point O in the USBL coordinate system USBL (0,0)；
Step 4.3, obtaining the slope and the angle alpha of the straight line OA in the ENU coordinate system through the four coordinates, obtaining the slope and the angle beta of the straight line OA in the USBL coordinate system, and then obtaining the deviation value gamma of the angle between the ENU coordinate system and the USBL coordinate system and the origin position: γ - β.
10. The method for calculating and registering the trajectory based on the autonomous underwater vehicle inertial navigation and the ultra-short baseline positioning sensor according to claim 1, wherein the step 5 specifically comprises:
step 5.1, rotation transformation: rotating the USBL coordinate coefficients by gamma angle according to the point clockwise:
Figure FDA0003650391420000034
wherein x is USBL (i) And y USBL (i) Respectively represent 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: translating the data coordinate obtained in the step 5.1 to obtain the coordinate (x) of the USBL data point in the ENU coordinate system 2 (i),y 2 (i))：
Figure FDA0003650391420000041
Step 5.3, solving a transformation matrix: according to the step 5.1 and the step 5.2, obtaining a rotation matrix R and a translation matrix T, and obtaining an augmentation transformation matrix C:
the rotation matrix is:
Figure FDA0003650391420000042
the translation matrix is:
Figure FDA0003650391420000043
augmented transformation matrix:
Figure FDA0003650391420000044
by the following formula:
Figure FDA0003650391420000045
the coordinate (x) of the USBL data point under the ENU coordinate system can be obtained 2 ,y 2 )。
CN202210548232.5A 2022-05-18 2022-05-18 Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor Pending CN114993313A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210548232.5A CN114993313A (en) 2022-05-18 2022-05-18 Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210548232.5A CN114993313A (en) 2022-05-18 2022-05-18 Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor

Publications (1)

Publication Number Publication Date
CN114993313A true CN114993313A (en) 2022-09-02

Family

ID=83027214

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210548232.5A Pending CN114993313A (en) 2022-05-18 2022-05-18 Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor

Country Status (1)

Country Link
CN (1) CN114993313A (en)

Similar Documents

Publication Publication Date Title
US9618344B2 (en) Digital map tracking apparatus and methods
CA2837179C (en) Determining spatial orientation information of a body from multiple electromagnetic signals
CN103697889B (en) A kind of unmanned plane independent navigation and localization method based on multi-model Distributed filtering
CN105160125B (en) A kind of simulating analysis of star sensor quaternary number
CN105549060A (en) Object positioning system based on position and attitude of airborne photoelectric gondola
EP3040680B1 (en) Magnetic anomaly tracking for an inertial navigation system
CN103335654B (en) A kind of autonomous navigation method of planetary power descending branch
CN109974713B (en) Navigation method and system based on surface feature group
Trigubovich et al. Complex technology of navigation and geodetic support of airborne electromagnetic surveys
US11977150B2 (en) Vehicle localization precision enhancement via multi-sensor fusion
CN112197765B (en) Method for realizing fine navigation of underwater robot
JP7214896B2 (en) underwater navigation beacon
CN114993313A (en) Trajectory resolving and registering method based on autonomous underwater robot inertial navigation and ultra-short baseline positioning sensor
Jia-Tong et al. On simultaneous AUV localization with single acoustic beacon using angles measurements
CN112649001B (en) Gesture and position resolving method for small unmanned aerial vehicle
Li et al. Unmanned aerial vehicle position estimation augmentation using optical flow sensor
CN112214028A (en) Underwater robot pose control method based on OpenMV
JP3012398B2 (en) Positioning method for multiple moving objects
JPH11325951A (en) Method and device for determining orbit with attitude sensor of space navigating object
Trojnacki et al. Determination of motion parameters with inertial measurement units–Part 1: mathematical formulation of the algorithm
Bourgeois Using range and range rate for relative navigation
CN105283733A (en) Apparatus for providing azimuth
Jaulin Mobile robotics: Kalman Filter
Geng et al. GPS/IMU/DMI integrated high-precision positioning approach
Lustosa et al. A novel imaging measurement model for vision and inertial navigation fusion with extended kalman filtering

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination