Sonar-assisted underwater navigation positioning method and system

Abstract

The invention provides a sonar-assisted underwater navigation positioning method and system, and the method comprises the steps: carrying out dead reckoning according to an attitude angle outputted by an IMU and a speed outputted by a DVL, and obtaining a dead reckoning odometer factor; after the first frame is set as the key frame, if the time difference between the current frame and the previous key frame is greater than a time difference threshold value or the pose change of the current frame relative to the previous key frame is greater than a pose change threshold value, the current frame is set as the key frame; performing filtering, feature extraction and matching on the sonar image; performing pose extraction according to the matched feature points to obtain a sonar odometer factor; key frames in a preset range are searched for loopback detection, and a loopback constraint factor is obtained through matching; and performing back-end optimization on the to-be-optimized pose by using a factor graph optimization method. According to the method, dead reckoning results of the IMU and the DVL are adopted as odometer factors, and calculation of back-end optimization can be greatly simplified.

Description

Technical Field

[0001] The present invention relates to the field of underwater navigation and positioning technology, and in particular to a sonar-assisted underwater navigation and positioning method and system. Background Art

[0002] Underwater robots play an important role in many fields such as ocean observation, resource exploration and military operations, showing broad application potential. However, the lack of cost-effective and comprehensive navigation and positioning technology in underwater environments has become a major obstacle to the development of underwater robots. The navigation and positioning system of underwater robots needs to provide accurate positioning, speed and attitude information, which is crucial for them to perform their tasks.

[0003] Because electromagnetic waves attenuate rapidly when propagating in water, traditional GPS navigation systems cannot be used underwater. At present, underwater navigation mainly relies on an inertial navigation system consisting of an inertial measurement unit (IMU) and a Doppler velocimeter (DVL). However, this system has a cumulative error problem, and the error will gradually increase over time. In addition, if hydroacoustic positioning technology is used, such as ultra-short baseline (USBL), short baseline (SBL) or long baseline (LBL) sonar systems, it is necessary to pre-deploy acoustic beacons with known positions underwater or on the surface, which not only increases the complexity of the operation, but also limits its application in a wider range of fields.

[0004] Sonar uses the principle of underwater sound to form images. It is not affected by water quality and light, has a long imaging distance and strong penetration. If the underwater robot is performing long-term operations in a single area such as fine detection and reconnaissance, it can be equipped with a forward-looking imaging sonar for auxiliary navigation. The current problems with the sonar-assisted navigation system are mainly: 1. Sonar images have few features and much noise, making it difficult to extract appropriate features for matching. 2. The sonar-assisted navigation system mainly uses the posture information obtained by matching the previous and next frames to participate in navigation, but the posture information between the previous and next frames has large noise, and the global error cannot be eliminated.

[0005] Patent application document CN111880184A discloses a method and system for locating a submarine target using a ship-borne side-scan sonar, comprising the following steps: obtaining the position information of a sonar source; calculating a posture correction coordinate transformation matrix T; determining the position information of the submarine target relative to the sonar source, and determining the relative position of the submarine target relative to the sonar source; using the posture correction coordinate transformation matrix T to correct the relative position information of the submarine target, and obtaining the precise position information of the submarine target in the geodetic coordinate system; the sonar image display module receives the precise position information and converts it into image information, and outputs a complete submarine target image. However, this patent cannot completely solve the existing technical problems, nor can it meet the needs of the present invention. Summary of the invention

[0006] In view of the defects in the prior art, an object of the present invention is to provide a sonar-assisted underwater navigation and positioning method and system.

[0007] The sonar-assisted underwater navigation positioning method provided by the present invention comprises:

[0008] Step S1: performing dead reckoning according to the attitude angle output by the inertial measurement unit IMU and the speed output by the Doppler velocity meter DVL to obtain a dead reckoning odometer factor;

[0009] Step S2: after the first frame is set as a key frame, if the time difference between the current frame and the previous key frame is greater than the time difference threshold, or the posture change of the current frame relative to the previous key frame is greater than the posture change threshold, then the current frame is set as a key frame;

[0010] Step S3: filtering, feature extraction and matching of the sonar image;

[0011] Step S4: extracting the pose according to the matched feature points to obtain the sonar odometer factor;

[0012] Step S5: searching for key frames within a preset range to perform loop detection, and matching to obtain loop constraint factors;

[0013] Step S6: Based on the dead reckoning odometer factor, the sonar odometer factor and the loop constraint factor, in combination with the key frames, a factor graph optimization method is used to perform backend optimization on the pose to be optimized.

[0014] Preferably, the step S1 comprises:

[0015] The three-dimensional attitude angle obtained by IMU is (φ, ψ, θ), and the three-dimensional velocity obtained by DVL is (v x ,v y ,v z ), the dead reckoning odometer factor between two moments is calculated based on the IMU and DVL data:

[0016]

[0017] Among them, (φ, ψ, θ are roll angle, yaw angle and pitch angle respectively; v x 、v y 、v z are the speeds in the x, y and z directions respectively; Δt is the time interval.

[0018] Preferably, the step S3 comprises:

[0019] Use the CFAR filter to filter the sonar image, set the protection unit size, and the average intensity of any point (i, j) on the sonar image is:

[0020]

[0021] If the average intensity of the point is greater than the threshold T CFAR , then the intensity of the point is retained, otherwise the point is considered as noise and the intensity of the point is set to 0; 2N cell +1 is the side length of the square window centered at (i, j);

[0022] The KZAE features and their descriptors are extracted from the filtered sonar image, and the KAZE features of the two frames of sonar images are quickly approximated to perform nearest neighbor matching to obtain the matching feature points. The coordinate sets of the feature points in the sonar image are X = {x1, x2, ..., x i},in d i is the distance from the origin to point i, θ i is the pitch angle of point i, then the center position of the feature point that averages the coordinate set is {u x ,u y}; The optimized position of the spacecraft at the current moment is recorded as (x k ,y k ,φ k ,θ k ,ψ k ), take the center position of the feature point as the position of the sonar image of this frame, and convert it to the global coordinate system. The expression is:

[0023]

[0024] Preferably, the step S4 comprises:

[0025] Use the ICP matching algorithm to match feature points. Suppose the feature frame coordinate sets of the two sonar images are: starting frame X = {x1, x2, ..., x i},in Target frame P = {p1, p2, ..., p i},in

[0026]

[0027] Find the mean value u of the two frame point clouds x and u p ,in k is the optimized x-axis coordinate of the aircraft, and the two frame point clouds are subtracted from their respective average values ​​to obtain a new point cloud set: i '=x i -u x ,p i '=p i -u p , calculate the matrix U and V are orthogonal matrices, ∑ is a diagonal matrix, and the pose transformation of the two frames of sonar images is:

[0028] Preferably, step S5 comprises:

[0029] Calculate the center position {x Imax ,y Imax}, convert it into global coordinates, the expression is:

[0030]

[0031] The nearest sonar image is searched near the global coordinates for loop detection. After feature extraction and matching, if the number of matches is greater than the preset threshold, the loop detection is considered successful. The ICP algorithm is also used to extract the pose and obtain the loop constraint factor.

[0032] The sonar-assisted underwater navigation and positioning system provided by the present invention comprises:

[0033] Module M1: Perform dead reckoning based on the attitude angle output by the inertial measurement unit IMU and the speed output by the Doppler velocity meter DVL to obtain the dead reckoning odometer factor;

[0034] Module M2: After setting the first frame as a key frame, if the time difference between the current frame and the previous key frame is greater than the time difference threshold, or the posture change of the current frame relative to the previous key frame is greater than the posture change threshold, then the current frame is set as a key frame;

[0035] Module M3: filtering, feature extraction and matching of sonar images;

[0036] Module M4: Extract pose based on matched feature points to obtain sonar odometer factor;

[0037] Module M5: Search for key frames within a preset range for loop detection, and match them to obtain loop constraint factors;

[0038] Module M6: Based on the dead reckoning odometry factor, sonar odometry factor and loop constraint factor, combined with key frames, the factor graph optimization method is used to perform back-end optimization on the pose to be optimized.

[0039] Preferably, the module M1 comprises:

[0040] The three-dimensional attitude angle obtained by IMU is (φ, ψ, θ), and the three-dimensional velocity obtained by DVL is (v x ,v y ,v z ), the dead reckoning odometer factor between two moments is calculated based on the IMU and DVL data:

[0041]

[0042] Among them, (φ, ψ, θ are roll angle, yaw angle and pitch angle respectively; v x 、v y 、v z are the speeds in the x, y and z directions respectively; Δt is the time interval.

[0043] Preferably, the module M3 comprises:

[0044] Use the CFAR filter to filter the sonar image, set the protection unit size, and the average intensity of any point (i, j) on the sonar image is:

[0045]

[0046] If the average intensity of the point is greater than the threshold T CFAR , then the intensity of the point is retained, otherwise the point is considered as noise and the intensity of the point is set to 0; 2N cell +1 is the side length of the square window centered at (i, j);

[0047] The KZAE features and their descriptors are extracted from the filtered sonar image, and the KAZE features of the two frames of sonar images are quickly approximated to perform nearest neighbor matching to obtain the matching feature points. The coordinate sets of the feature points in the sonar image are X = {x1, x2, ..., x i},in d i is the distance from the origin to point i, θ i is the pitch angle of point i, then the center position of the feature point that averages the coordinate set is {u x ,u y}; The optimized position of the spacecraft at the current moment is recorded as (x k ,y k ,φ k ,θ k ,ψ k ), take the center position of the feature point as the position of the sonar image of this frame, and convert it to the global coordinate system. The expression is:

[0048]

[0049] Preferably, the module M4 comprises:

[0050] Use the ICP matching algorithm to match feature points. Suppose the feature frame coordinate sets of the two sonar images are: starting frame X = {x1, x2, ..., x i},in Target frame P = {p1, p2, ..., p i},in

[0051]

[0052] Find the mean value u of the two frame point clouds x and u p ,in k is the optimized x-axis coordinate of the aircraft, and the two frame point clouds are subtracted from their respective average values ​​to obtain a new point cloud set: i '=x i -u x ,p i '=p i -u p , calculate the matrix U and V are orthogonal matrices, ∑ is a diagonal matrix, and the pose transformation of the two frames of sonar images is:

[0053] Preferably, the module M5 comprises:

[0054] Calculate the center position {x Imax ,y Imax}, convert it into global coordinates, the expression is:

[0055]

[0056] The nearest sonar image is searched near the global coordinates for loop detection. After feature extraction and matching, if the number of matches is greater than the preset threshold, the loop detection is considered successful. The ICP algorithm is also used to extract the pose and obtain the loop constraint factor.

[0057] Compared with the prior art, the present invention has the following beneficial effects:

[0058] The present invention provides a sonar-assisted underwater navigation and positioning method, which adopts the dead reckoning results of IMU and DVL as odometer factors, which can greatly simplify the calculation of back-end optimization; and preferentially searches for key frames in the nearby range for loop detection. Compared with the brute force matching algorithm, it can quickly match historical data and speed up the back-end optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0060] Figure 1 A flowchart of the method proposed for the present invention;

[0061] Figure 2 Comparison of dead reckoning and GPS trajectory;

[0062] Figure 3 Compare sonar-assisted navigation with GPS tracks;

[0063] Figure 4 Schematic diagram of optimization using factor graphs. DETAILED DESCRIPTION

[0064] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0065] Example 1

[0066] like Figure 1 The present invention provides a sonar-assisted underwater navigation and positioning method, comprising:

[0067] Step S1: Perform dead reckoning based on the attitude angle output by the inertial measurement unit IMU and the speed output by the Doppler velocity meter DVL to obtain a dead reckoning odometer factor.

[0068] The three-dimensional attitude angle obtained by IMU is (φ, ψ, θ), and the three-dimensional velocity obtained by DVL is (v x ,v y ,v z ). The dead reckoning odometer factor between two moments is calculated based on the IMU and DVL data:

[0069]

[0070] Step S2: After setting the first frame as a key frame, if the time difference between the current frame and the previous key frame is greater than the time difference threshold, or the posture change of the current frame relative to the previous key frame is greater than the posture change threshold, then the current frame is set as a key frame, and the expression is:

[0071]

[0072] Step S3: Filter, extract features and match the sonar image.

[0073] The CFAR filter is used to filter the sonar image. The principle is as follows: the protection unit size is set, and the average intensity of a point on the sonar image is calculated using the following formula:

[0074]

[0075] If the average intensity of the point is greater than the threshold T CFAR, the intensity of the point is retained, otherwise the point is considered as noise and the intensity of the point is set to 0.

[0076] The KZAE features and their descriptors are extracted from the filtered sonar image, and the KAZE features of the two frames of sonar images are quickly approximated to perform nearest neighbor matching to obtain the matching feature points. Assume that the coordinate sets of the feature points in the sonar image are X = {x1, x2, ..., x i},in Then the center position of the feature point that averages the coordinate set is {u x ,u y}. The optimized position of the spacecraft at the current moment is recorded as (x k ,y k ,φ k ,θ k ,ψ k ), take the center position of the feature point as the position of the sonar image frame, and convert it to the global coordinate system:

[0077]

[0078] Step S4: extracting the pose according to the matched feature points to obtain the sonar odometer factor.

[0079] Use the ICP matching algorithm to match feature points. Assume that the feature frame coordinate sets of the two sonar images are: starting frame X = {x1, x2, ..., x i},in Target frame P = {p1, p2, ..., p i},in where d i ,θ i and d j ,θ j ;

[0080] Find the mean value u of the two frame point clouds x and u p ,in The two frame point clouds are subtracted from their respective mean values ​​to obtain a new point cloud set: i '=x i -u x ,p i '=p i -u p ,calculate The pose transformation of the two frames of sonar images can be obtained as follows:

[0081] R=UV T

[0082] t=u x -Ru p

[0083] Step S5: Search for key frames in the vicinity for loop detection, and match them to obtain loop constraint factors.

[0084] Calculate the center position {x Imax ,y Imax}, convert it into global coordinates:

[0085]

[0086] The nearest sonar image is searched near the global coordinates for loop detection. After feature extraction and matching, if the number of matches is greater than the threshold, the loop detection is considered successful. The ICP algorithm is also used for pose extraction to obtain the loop constraint factor.

[0087] Step S6: Use the factor graph optimization method to perform backend optimization.

[0088] like Figure 4 , the pose variables to be optimized are calculated by global factor graph optimization. Step S1 constructs the dead reckoning odometer constraint, step S4 constructs the sonar odometer constraint, and step S5 constructs the loop detection constraint. In the back-end optimization process, the relationship between the above constraints is represented and processed in the form of a factor graph, and the factor graph and the value of the factor graph node are called based on the GTSMA library, and the residual and corresponding Jacobian matrix of each observation constraint factor are calculated. The node variables are iteratively optimized by calling the Gauss-Newton optimization algorithm of the optimizer, and the factor graph model and the optimized key frame pose are updated through the sliding window to maintain the global trajectory information.

[0089] The performance of the above algorithm is illustrated by the following test examples. A sonar-assisted navigation test was conducted in a reservoir in Zhejiang Province. The test platform was equipped with GPS, forward-looking sonar, inertial navigation and DVL. During the test, the UUV navigated at a constant depth. The GPS data was used as the test truth value and compared with the dead reckoning results and sonar-assisted navigation results. It can be seen that the sonar-assisted navigation results are significantly better than the dead reckoning results. Figure 2 and Figure 3 .

[0090] Example 2

[0091] The present invention also provides a sonar-assisted underwater navigation and positioning system, which can be implemented by executing the process steps of the sonar-assisted underwater navigation and positioning method, that is, those skilled in the art can understand the sonar-assisted underwater navigation and positioning method as a preferred implementation of the sonar-assisted underwater navigation and positioning system.

[0092] The sonar-assisted underwater navigation and positioning system provided by the present invention comprises: a module M1: performing dead reckoning according to the attitude angle output by an inertial measurement unit IMU and the speed output by a Doppler velocity meter DVL to obtain a dead reckoning odometer factor; a module M2: after setting the first frame as a key frame, if the time difference between the current frame and the previous key frame is greater than a time difference threshold, or the posture change of the current frame relative to the previous key frame is greater than a posture change threshold, then setting the current frame as a key frame; a module M3: filtering, feature extraction and matching of a sonar image; a module M4: performing posture extraction according to matched feature points to obtain a sonar odometer factor; a module M5: searching for key frames within a preset range for loop detection, and matching to obtain a loop constraint factor; and a module M6: based on the dead reckoning odometer factor, the sonar odometer factor and the loop constraint factor, in combination with the key frames, using a factor graph optimization method to perform back-end optimization on the posture to be optimized.

[0093] The module M1 includes: the three-dimensional attitude angle obtained by IMU is (φ, ψ, θ), the three-dimensional velocity obtained by DVL is (v x ,v y ,v z ), the dead reckoning odometer factor between two moments is calculated based on the IMU and DVL data:

[0094]

[0095] Among them, (φ, ψ, θ are roll angle, yaw angle and pitch angle respectively; v x 、v y 、v z are the speeds in the x, y and z directions respectively; Δt is the time interval.

[0096] The module M3 includes: filtering the sonar image using a CFAR filter, setting the protection unit size, and the average intensity of any point (i, j) on the sonar image is:

[0097]

[0098] If the average intensity of the point is greater than the threshold T CFAR , then the intensity of the point is retained, otherwise the point is considered as noise and the intensity of the point is set to 0; 2N cell +1 is the side length of the square window centered at (i, j);

[0099] The KZAE features and their descriptors are extracted from the filtered sonar image, and the KAZE features of the two frames of sonar images are quickly approximated to perform nearest neighbor matching to obtain the matching feature points. The coordinate sets of the feature points in the sonar image are X = {x1, x2, ..., x i},in di is the distance from the origin to point i, θ i is the pitch angle of point i, then the center position of the feature point that averages the coordinate set is {u x ,u y}; The optimized position of the spacecraft at the current moment is recorded as (x k ,y k ,φ k ,θ k ,ψ k ), take the center position of the feature point as the position of the sonar image of this frame, and convert it to the global coordinate system. The expression is:

[0100]

[0101] The module M4 includes: using the ICP matching algorithm to match the feature points, assuming that the feature frame coordinate sets of the two sonar images are: starting frame X = {x1, x2, ..., x i},in Target frame P = {p1, p2, ..., p i},in

[0102] Find the mean value u of the two frame point clouds x and u p ,in k is the optimized x-axis coordinate of the aircraft, and the two frame point clouds are subtracted from their respective average values ​​to obtain a new point cloud set: i '=x i -u x ,p i '=p i -u p , calculate the matrix U and V are orthogonal matrices, ∑ is a diagonal matrix, and the pose transformation of the two frames of sonar images is:

[0103] The module M5 comprises: calculating the center position {x Imax ,y Imax}, convert it into global coordinates, the expression is:

[0104]

[0105] The nearest sonar image is searched near the global coordinates for loop detection. After feature extraction and matching, if the number of matches is greater than the preset threshold, the loop detection is considered successful. The ICP algorithm is also used to extract the pose and obtain the loop constraint factor.

[0106] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0107] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A sonar-assisted underwater navigation and positioning method, characterized in that: include: Step S1: performing dead reckoning according to the attitude angle output by the inertial measurement unit IMU and the speed output by the Doppler velocity meter DVL to obtain a dead reckoning odometer factor; Step S2: after the first frame is set as a key frame, if the time difference between the current frame and the previous key frame is greater than the time difference threshold, or the posture change of the current frame relative to the previous key frame is greater than the posture change threshold, then the current frame is set as a key frame; Step S3: filtering, feature extraction and matching of the sonar image; Step S4: extracting the pose according to the matched feature points to obtain the sonar odometer factor; Step S5: searching for key frames within a preset range to perform loop detection, and matching to obtain loop constraint factors; Step S6: Based on the dead reckoning odometer factor, the sonar odometer factor and the loop constraint factor, in combination with the key frames, a factor graph optimization method is used to perform backend optimization on the pose to be optimized.

2. The sonar-assisted underwater navigation and positioning method according to claim 1, characterized in that: The step S1 comprises: The three-dimensional attitude angle obtained by IMU is (φ, ψ, θ), and the three-dimensional velocity obtained by DVL is (v x ,v y ,v z ), the dead reckoning odometer factor between two moments is calculated based on the IMU and DVL data: Among them, (φ, ψ, θ are roll angle, yaw angle and pitch angle respectively; v x 、v y 、v z are the speeds in the x, y and z directions respectively; Δt is the time interval.

3. The sonar-assisted underwater navigation positioning method according to claim 2, characterized in that: The step S3 comprises: Use the CFAR filter to filter the sonar image, set the protection unit size, and the average intensity of any point (i, j) on the sonar image is: If the average intensity of the point is greater than the threshold T CFAR , then the intensity of the point is retained, otherwise the point is considered as noise and the intensity of the point is set to 0; 2N cell +1 is the side length of the square window centered at (i, j); The KZAE features and their descriptors are extracted from the filtered sonar image, and the KAZE features of the two frames of sonar images are quickly approximated to perform nearest neighbor matching to obtain the matching feature points. The coordinate sets of the feature points in the sonar image are X = {x1, x2, ..., x i },in d i is the distance from the origin to point i, θ i is the pitch angle of point i, then the center position of the feature point that averages the coordinate set is {u x ,u y }; The optimized position of the spacecraft at the current moment is recorded as (x k ,y k ,φ k ,θ k ,ψ k ), take the center position of the feature point as the position of the sonar image of this frame, and convert it to the global coordinate system. The expression is:

4. The sonar-assisted underwater navigation and positioning method according to claim 3, characterized in that: The step S4 comprises: Use the ICP matching algorithm to match feature points. Suppose the feature frame coordinate sets of the two sonar images are: starting frame X = {x1, x2, ..., x i },in Target frame P = {p1, p2, ..., p i },in Find the mean value u of the two frame point clouds x and u p ,in k is the optimized x-axis coordinate of the aircraft, and the two frame point clouds are subtracted from their respective average values ​​to obtain a new point cloud set: i '=x i -u x ,p i '=p i -u p , calculate the matrix U and V are orthogonal matrices, ∑ is a diagonal matrix, and the pose transformation of the two frames of sonar images is:

5. The sonar-assisted underwater navigation and positioning method according to claim 4, characterized in that: The step S5 comprises: Calculate the center position {x Imax ,y Imax }, convert it into global coordinates, the expression is: The nearest sonar image is searched near the global coordinates for loop detection. After feature extraction and matching, if the number of matches is greater than the preset threshold, the loop detection is considered successful. The ICP algorithm is also used to extract the pose and obtain the loop constraint factor.

6. A sonar-assisted underwater navigation and positioning system, characterized in that: include: Module M1: Perform dead reckoning based on the attitude angle output by the inertial measurement unit IMU and the speed output by the Doppler velocity meter DVL to obtain the dead reckoning odometer factor; Module M2: After setting the first frame as a key frame, if the time difference between the current frame and the previous key frame is greater than the time difference threshold, or the posture change of the current frame relative to the previous key frame is greater than the posture change threshold, then the current frame is set as a key frame; Module M3: filtering, feature extraction and matching of sonar images; Module M4: Extract pose based on matched feature points to obtain sonar odometer factor; Module M5: Search for key frames within a preset range for loop detection, and match them to obtain loop constraint factors; Module M6: Based on the dead reckoning odometry factor, sonar odometry factor and loop constraint factor, combined with key frames, the factor graph optimization method is used to perform back-end optimization on the pose to be optimized.

7. The sonar-assisted underwater navigation and positioning system according to claim 6, characterized in that: The module M1 comprises: The three-dimensional attitude angle obtained by IMU is (φ, ψ, θ), and the three-dimensional velocity obtained by DVL is (v x ,v y ,v z ), the dead reckoning odometer factor between two moments is calculated based on the IMU and DVL data: Among them, (φ, ψ, θ are roll angle, yaw angle and pitch angle respectively; v x 、v y 、v z are the speeds in the x, y and z directions respectively; Δt is the time interval.

8. The sonar-assisted underwater navigation and positioning system according to claim 7, characterized in that: The module M3 comprises: Use the CFAR filter to filter the sonar image, set the protection unit size, and the average intensity of any point (i, j) on the sonar image is: If the average intensity of the point is greater than the threshold T CFAR , then the intensity of the point is retained, otherwise the point is considered as noise and the intensity of the point is set to 0; 2N cell +1 is the side length of the square window centered at (i, j); The KZAE features and their descriptors are extracted from the filtered sonar image, and the KAZE features of the two frames of sonar images are quickly approximated to perform nearest neighbor matching to obtain the matching feature points. The coordinate sets of the feature points in the sonar image are X = {x1, x2, ..., x i },in d i is the distance from the origin to point i, θ i is the pitch angle of point i, then the center position of the feature point that averages the coordinate set is {u x ,u y }; The optimized position of the spacecraft at the current moment is recorded as (x k ,y k ,φ k ,θ k ,ψ k ), take the center position of the feature point as the position of the sonar image of this frame, and convert it to the global coordinate system. The expression is:

9. The sonar-assisted underwater navigation and positioning system according to claim 8, characterized in that: The module M4 comprises: Use the ICP matching algorithm to match feature points. Suppose the feature frame coordinate sets of the two sonar images are: starting frame X = {x1, x2, ..., x i },in Target frame P = {p1, p2, ..., p i },in Find the mean value u of the two frame point clouds x and u p ,in k is the optimized x-axis coordinate of the aircraft, and the two frame point clouds are subtracted from their respective average values ​​to obtain a new point cloud set: i '=x i -u x ,p i '=p i -u p , calculate the matrix U and V are orthogonal matrices, ∑ is a diagonal matrix, and the pose transformation of the two frames of sonar images is:

10. The sonar-assisted underwater navigation and positioning system according to claim 9, characterized in that: The module M5 comprises: Calculate the center position {x Imax ,y Imax }, convert it into global coordinates, the expression is: The nearest sonar image is searched near the global coordinates for loop detection. After feature extraction and matching, if the number of matches is greater than the preset threshold, the loop detection is considered successful. The ICP algorithm is also used to extract the pose and obtain the loop constraint factor.

Images