Unmanned ship positioning method and system based on fusion of side scan sonar and inertial sensor and medium

Abstract

The application discloses a kind of based on side scan sonar and inertial sensor fusion unmanned ship positioning method, system and medium, method includes: extracting ORB feature point from side scan sonar image;Parallel target detection algorithm is executed to side scan sonar image, and the semantic information of current frame is obtained;Combining feature point depth and the semantic information of target detection, background point and target point are screened;Pose solution is carried out based on IMU pre-integration method;According to IMU solution result and feature point information, the initial value of unmanned ship pose is solved, and the dynamic and static conditions of target are discriminated;Unmanned ship pose is tightly coupled optimization by based on side scan sonar and inertial sensor fusion information, and more accurate pose is obtained.The measurement information of side scan sonar image and inertial sensor is fused and calculated, and the semantic information of target detection is introduced, so that unmanned ship can still achieve high-precision, high-robustness real-time positioning in complex dynamic marine environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned surface vessel (USV) positioning, specifically to a USV positioning method, system, and medium based on the fusion of side-scan sonar and inertial sensors. Background Technology

[0002] In recent years, marine unmanned surface vessel (USV) technology has demonstrated significant potential and broad application prospects in fields such as military reconnaissance, environmental monitoring, resource exploration, and search and rescue. To ensure the efficient execution of missions, developing high-precision USV positioning systems is crucial, as it plays a key role in improving the perception capabilities and motion control accuracy of USVs. Therefore, achieving precise positioning of USVs in complex and dynamic marine environments has become one of the cutting-edge research topics.

[0003] While unmanned surface vessel (USV) positioning methods based on real-time dynamic carrier phase differential (RTK) technology are simple to operate, their signal strength and positioning accuracy are easily affected by weather conditions and topography, and they are also costly and have limited accuracy. Side-scan sonar (SSS), as a commonly used device, can accurately measure seabed topography by emitting fan-shaped acoustic pulses from both sides of the USV or towed vessel. SSS sonar has advantages such as wide measurement range, high resolution, and cost-effectiveness. The stability and structural characteristics of the seabed topography provide reliable reference information for USV positioning. However, SSS sonar images are susceptible to seabed reverberation and water noise, resulting in insufficient robustness for positioning systems relying solely on SSS sonar.

[0004] Inertial measurement units (IMUs) can perform high-precision measurements of rapid motion in a short time, complementing side-scan sonar. Therefore, how to combine the characteristics of the marine environment and achieve high-precision positioning of unmanned surface vessels by fusing measurement data from side-scan sonar and IMU has become a pressing technical problem to be solved.

[0005] In the marine environment, dynamic objects such as schools of fish, underwater robots, and submersibles exist, which can interfere with the accurate positioning of unmanned surface vessels (USVs). Using existing target detection algorithms (such as the YOLO series of networks) to process side-scan sonar images can extract semantic information about dynamic objects. However, given the high real-time requirements of USV positioning systems, achieving fast and accurate target detection remains a challenge. Based on the obtained target semantic information, further exploring how to effectively integrate it into the USV positioning system to enhance the positioning performance of USVs in complex and dynamic marine environments is an important direction worthy of in-depth research. Summary of the Invention

[0006] The main objective of this invention is to overcome the shortcomings and deficiencies of the prior art and provide a method, system and medium for unmanned surface vessel (USV) positioning based on the fusion of side-scan sonar and inertial sensor. This method fuses and calculates side-scan sonar images and inertial sensor measurement information, and introduces semantic information of target detection, enabling USVs to achieve high-precision and robust real-time positioning in complex and dynamic marine environments.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] In a first aspect, the present invention provides a method for unmanned surface vessel (USV) positioning based on the fusion of side-scan sonar and inertial sensors, comprising the following steps:

[0009] ORB feature points are extracted from side-scan sonar images;

[0010] A parallel target detection algorithm is executed on the side-scan sonar image, and the target detection box of the current frame is predicted by combining optical flow tracing to obtain the semantic information of the current frame;

[0011] By combining feature point depth and semantic information from object detection, background points and target points are filtered out.

[0012] Pose calculation based on IMU pre-integration method;

[0013] Based on the IMU calculation results and feature point information, the initial pose of the unmanned surface vessel is calculated, and the dynamic and static status of the target is determined.

[0014] By tightly coupling and optimizing the pose of the unmanned surface vessel based on the fusion information of side-scan sonar and inertial sensors, a more accurate pose can be obtained.

[0015] As a preferred technical solution, the extraction of ORB feature points from the side-scan sonar image specifically includes:

[0016] For the input side-scan sonar image, two parallel threads are started to extract ORB feature points from the two side-scan sonar images respectively. After constructing the Gaussian pyramid, all pixels of the image at different scales are traversed to extract the FAST key points with orientation and calculate the rotation-invariant BRIEF descriptor. Finally, the extracted ORB feature points are homogenized.

[0017] As a preferred technical solution, the parallel target detection algorithm is executed on the contralateral scan sonar image, and the target detection box of the current frame is predicted by optical flow tracing to obtain the semantic information of the current frame, specifically:

[0018] Start the tracking thread and the target detection thread;

[0019] In the target detection thread, Shi-Tomasi corner points are extracted from the current frame side-scan sonar image, and a circular mask of a certain radius is drawn at the center of the 2D detection box. Only the Shi-Tomasi corner points in the circular mask are retained. At the same time, the YOLOX network is called to perform target detection on the side-scan sonar image.

[0020] In the tracking thread, the Shi-Tomasi corner points of the previous frame are tracked to the current frame using optical flow tracing. For the corner point of the k-th target in the previous frame, the deviation between its corner point center and the center of the 2D detection box is calculated. Based on this deviation and the corner point center of the current frame, the center of the 2D detection box of the current frame is estimated to construct the 2D detection box of the current frame.

[0021] As a preferred technical solution, the step of combining feature point depth and semantic information of target detection to filter background points and target points specifically involves:

[0022] The extracted ORB feature points are assigned to each 2D detection box; the depth value of each feature point is obtained, and the median depth of the corner points of each 2D detection box is calculated as the depth of the corresponding target.

[0023] For each ORB feature point, count the number of 2D detection boxes containing that feature point, n. If n = 0, mark the feature point as a background point. If n = 1, compare the depth of the feature point with the depth of the target. If the depth difference is less than a threshold and the point is close to the center of the detection box, mark the feature point as a target point; otherwise, mark it as a background point. If n > 1, calculate the depth difference between the feature point and each target, and take the minimum difference. If the minimum difference is less than a threshold and the point is close to the center of the detection box, mark the feature point as the corresponding target point; otherwise, mark it as a background point.

[0024] As a preferred technical solution, the pose calculation based on the IMU pre-integration method specifically includes:

[0025] Let time i and time j correspond to two side-scan sonar image frames, respectively. The rotation matrix R of the i-th frame is obtained by accumulating and integrating the IMU measurements. i Speed ​​v i and displacement p i Find the rotation matrix R at frame j. j Speed ​​v j and displacement p j :

[0026]

[0027] in, and This represents the measured values ​​of angular velocity and acceleration; and This indicates that the gyroscope and accelerometer have zero bias. and This represents the noise level measured by the gyroscope and accelerometer; g is the acceleration due to gravity; Δt is the IMU measurement time interval. To avoid frequent re-integration during backend optimization, the pre-integration between two side-scan sonar image frames is calculated, i.e.:

[0028]

[0029] Where, ΔR ij Δv ij and Δp ij Let the rotation pre-integral, velocity pre-integral, and displacement pre-integral values ​​be respectively between frame i and frame j. Δv ik =v k -v i .

[0030] As a preferred technical solution, the step of solving the initial pose value of the unmanned surface vessel based on the IMU calculation results and feature point information, and judging the dynamic and static status of the target, specifically involves:

[0031] The pose of the current frame is obtained by the pose calculation of the IMU. The pose is used to perform nearest neighbor matching of ORB feature points. The 3D points corresponding to the ORB feature points in the previous frame are transformed to the image plane of the current frame through the pose of the current frame, and matching feature points are searched in the neighborhood.

[0032] Based on the matching results, the pose is optimized using Beta Analysis (BA). The BA optimization problem is described as follows:

[0033]

[0034] in, Let represent the homogeneous coordinates of the i-th background point in the world coordinate system, and Observations express The slant range measured by the side-scan sonar in frame t; ρ(·) represents the robust kernel function; ∑ t,i Represent the covariance matrix;

[0035] Traverse each target point in the current frame, calculate its measurement error, and mark it as an outlier if the error is greater than the dynamic threshold.

[0036] If the proportion of outer points in a target is greater than a threshold, the target is marked as a dynamic target and all its feature points are marked as dynamic points; otherwise, only the outer points are marked as dynamic points and the remaining feature points are marked as background points.

[0037] As a preferred technical solution, the method of tightly coupling and optimizing the pose of the unmanned surface vessel based on the fusion information of side-scan sonar and inertial sensors to obtain a more accurate pose specifically involves:

[0038] Tight coupling optimization is expressed as:

[0039]

[0040] in, This represents the side-scan sonar measurement constraint factor. Represents the pre-integral factor; Represents the motion constraint factor of the unmanned surface vessel, ∑ t,i ,∑ t,t+1 and ∑ t,t-r Covariance matrices for the three factors respectively;

[0041] The side-scan sonar measurement constraint factor is the difference between the observed value and the reprojection of the side-scan sonar measurement constraint factor between the pose of the t-th keyframe and the i-th map point in the frame.

[0042] The IMU pre-integration factor includes the rotation residual, velocity residual, and translation residual between the pose of frame t and frame t+1.

[0043] The motion constraint factor for the unmanned surface vessel (USV) is as follows: For the pose of the t-th keyframe, the pose of the tr-th keyframe is traced back, the relative pose is calculated, and its y-axis translation and x and z-axis rotation are restricted to obtain the se(2) Lie algebra. Then, the transformation matrix is ​​converted back to construct the motion constraint factor for the USV.

[0044] Secondly, the present invention provides an unmanned surface vessel (USV) positioning system based on the fusion of side-scan sonar and inertial sensor, which is applied to the aforementioned unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensor, including a feature extraction module, a target detection module, a filtering module, a pose calculation module, a pose initial value calculation and discrimination module, and a tightly coupled optimization module.

[0045] The feature extraction module is used to extract ORB feature points from the side-scan sonar image;

[0046] The target detection module is used to perform a parallel target detection algorithm on the side-scan sonar image, and combine optical flow tracing to predict the target detection box of the current frame to obtain the semantic information of the current frame;

[0047] The filtering module is used to filter background points and target points by combining feature point depth and semantic information of target detection.

[0048] The pose calculation module is used to perform pose calculation based on the IMU pre-integration method;

[0049] The pose initial value calculation and discrimination module is used to calculate the initial pose value of the unmanned surface vessel based on the IMU calculation results and feature point information, and to discriminate the dynamic and static status of the target.

[0050] The tightly coupled optimization module is used to perform tightly coupled optimization of the unmanned surface vessel's attitude based on the fusion information of side-scan sonar and inertial sensors, so as to obtain a more accurate attitude.

[0051] Thirdly, the present invention provides an unmanned surface vessel, the unmanned surface vessel comprising:

[0052] At least one processor; and,

[0053] A memory communicatively connected to the at least one processor; wherein,

[0054] The memory stores computer program instructions that can be executed by the at least one processor, which are executed by the at least one processor to enable the at least one processor to execute the unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensors.

[0055] Fourthly, the present invention provides a computer-readable storage medium storing a program, which, when executed by a processor, implements the unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensors.

[0056] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0057] 1. This invention combines the characteristics of side-scan sonar and inertial sensors, tightly couples and fuses the measurement information of the two, and applies motion constraints to the estimated pose of the unmanned surface vessel based on the motion characteristics of the unmanned surface vessel in the marine environment, so that the unmanned surface vessel can achieve high-precision and high-robust positioning in complex marine environments.

[0058] 2. By introducing semantic information for target detection, this invention can identify potential dynamic objects and determine the motion status of the target by solving the initial pose value of the unmanned surface vessel, thereby avoiding the adverse effects of dynamic objects on the system positioning and enabling the unmanned surface vessel to maintain high-precision positioning in a highly dynamic marine environment.

[0059] 3. Based on multi-threaded programming, this invention introduces optical flow tracing method and runs in parallel with the YOLOX target detection network to achieve fast and accurate target detection, thereby ensuring the real-time positioning of the system. Attached Figure Description

[0060] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0061] Figure 1 This is a flowchart of the unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensors in an embodiment of the present invention.

[0062] Figure 2 This is a schematic diagram illustrating the principle of the optical flow-assisted target detection method in an embodiment of the present invention.

[0063] Figure 3 This is an alignment diagram of the timestamps of the side-scan sonar image frames and IMU measurement data in an embodiment of the present invention.

[0064] Figure 4 This is an estimated trajectory diagram of an unmanned surface vessel in a marine environment testing experiment according to an embodiment of the present invention. Detailed Implementation

[0065] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of the present application, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0066] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application can be combined with other embodiments.

[0067] like Figure 1 As shown in the figure, this embodiment of an unmanned surface vessel (USV) positioning method based on the fusion of side-scan sonar and inertial sensors includes the following steps:

[0068] S1. ORB feature points are extracted in parallel from the side-scan sonar images.

[0069] This embodiment extracts 2000 ORB feature points from the input left and right side-scan sonar images in parallel by starting two sub-threads. First, an 8-layer Gaussian pyramid is constructed for the input side-scan sonar image, with a scale factor set to 1.2. Second, directional FAST keypoints are extracted from each layer of the side-scan sonar image, and a threshold of 20 is used to filter the keypoints. Then, a rotation-invariant BRIEF descriptor is calculated. Finally, an octree method is used to homogenize the extracted ORB feature points.

[0070] S2. Perform a parallel target detection algorithm on the side-scan sonar image to obtain the semantic information of the current frame.

[0071] To ensure real-time target detection, the target detection results of the previous frame can be combined with optical flow tracing technology to obtain the target detection results of the current frame. This embodiment will start two sub-threads: a tracking thread and a target detection thread. In the tracking thread, optical flow-assisted target detection will be performed using the detection results from the previous frame. In the target detection thread, target detection using the YOLOX network (trained on a side-scan sonar image dataset) and Shi-Tomasi corner point extraction will be performed in parallel, and the results will be used in the tracking thread of the next frame.

[0072] In the target detection thread, a large number of Shi-Tomasi corner points are extracted, and their positions are optimized through sub-pixel analysis to obtain more precise locations. To retain corner points on the target within the 2D detection bounding box, this embodiment employs a corner point filtering method based on a circular mask. This method draws a circular mask of a certain radius at the center of the 2D detection bounding box, retaining only the Shi-Tomasi corner points within the circular mask. For 2D detection bounding boxes larger than 100 pixels, the circular mask radius is set to 60 pixels; for 2D detection bounding boxes between 40 and 100 pixels, the circular mask radius is 0.5 times the size of the box; and for 2D detection bounding boxes smaller than 40 pixels, since the area is too small to extract a sufficient number of corner points, a circular mask is not drawn. This method ensures that a sufficiently large area within the 2D detection bounding box is considered while avoiding the problem of corner points falling on the background.

[0073] In the tracing thread, such as Figure 2 As shown, optical flow tracing is used to predict the 2D bounding box in frame t from the target detection results in frame (t-1). The Shi-Tomasi corner points in frame (t-1) are traced to frame t using optical flow tracing. For the corner point of the k-th target in frame (t-1), its corner center can be easily calculated. and its 2D detection frame set center Similarly, the center angle of the corner point in frame t. This can also be obtained. Therefore, the deviation between the two centers in the (t-1)th frame... for:

[0074]

[0075] Because the time interval between two consecutive frames is very short, the deviation between the two centers in frame t changes very little. Therefore, the center of the 2D detection box in frame t can be determined. for:

[0076]

[0077] Since the size of the 2D detection box does not change much between two frames, it can be used as follows: Construct a 2D detection box for frame t centered at the specified location.

[0078] S3. Combine feature point depth and semantic information of target detection to filter background points and target points.

[0079] After performing target detection on the input side-scan sonar image, the extracted ORB feature points are assigned to each 2D detection bounding box, which may contain both background and target feature points. Directly discarding all points might result in too few feature points, affecting pose estimation accuracy; therefore, it is necessary to filter out background and target points. Furthermore, 2D detection bounding boxes for different targets may overlap, requiring the feature points at overlapping locations to be assigned to their respective 2D detection bounding boxes.

[0080] To address the two issues mentioned above, this embodiment employs a feature point allocation method based on target depth. First, the depth value of each corner point in the current frame can be obtained from the sonar image. For each corner point within a 2D detection box, its median can be calculated, and this median is set as the depth of the corresponding target.

[0081] Next, iterate through each ORB feature point and count the number n of 2D detection boxes containing that feature point. If n = 0, set the label of that feature point to background; if n = 1, compare the depth of that feature point with the depth of the target. If the depth difference is less than a threshold t... d If the location of the feature point is close to the center of the detection box, then the feature point is labeled as the target; otherwise, it is labeled as a background point. If n>1, calculate the depth difference between the feature point and each target, and count the minimum difference. If this difference is less than the threshold t, then... d If the location of the point is close to the center of the detection box, then the label of the feature point is set as the corresponding target; otherwise, it is set as the background.

[0082] S4. Perform pose calculation based on the IMU pre-integration method to obtain the current pose and pre-integration quantity.

[0083] like Figure 3As shown, the measurement frequency of the sidescan sonar is 10 Hz, while the measurement frequency of the IMU is 100 Hz. The time interval between two sidescan sonar image frames is much larger than the time interval Δt between two IMU measurements. Therefore, it is often necessary to integrate several IMU measurements between two sidescan sonar images. Suppose the i-th and j-th moments correspond to two sidescan sonar image frames respectively. Then, through the cumulative integration of IMU measurements, the rotation matrix R i at the i-th frame, the velocity v i and the displacement p i can be obtained, and the rotation matrix R j at the j-th frame, the velocity v j and the displacement p j are calculated as follows:

[0084]

[0085] where, and represent the angular velocity and acceleration measurement values; and represent the gyroscope and accelerometer biases, and represent the gyroscope and accelerometer measurement noises; g is the gravitational acceleration; Δt is the IMU measurement time interval. When R i is updated, R k (i ≤ k < j) needs to be recalculated, and the integration quantities between v i and v j , and the integration quantities between p i and p j both depend on R k . Therefore, these two integration quantities also need to be recalculated. In the backend optimization, the pose is updated frequently, and frequent re-integration will affect the real-time performance of the system. Therefore, the relative change in motion between two sidescan sonar image frames is used to replace the direct integration, that is, pre-integration, and the obtained relative change quantity is called the pre-integration quantity, that is:

[0086]

[0087] where, ΔR ij , Δv ij and Δp ij are the rotation pre-integration quantity, velocity pre-integration quantity and displacement pre-integration quantity respectively from the i-th frame to the j-th frame, and Δv ik = v k - v i . Pre-integration effectively solves the problem of repeated integration and will be utilized in the backend optimization.

[0088] S5. Based on the IMU calculation results and feature point information, solve for the initial pose of the unmanned surface vessel and determine the dynamic and static status of the target.

[0089] If the IMU has been initialized or the velocity information from the previous frame exists, a motion model tracking method is used to solve for the initial pose. If the IMU has been initialized, the IMU measurements between the current frame and the previous frame can be pre-integrated to obtain the relative pose between the two frames. The pose of the current frame is:

[0090]

[0091] Then, the pose of the current frame can be used as a reference. To perform ORB feature point matching, the 3D points corresponding to the ORB feature points in frame t-1 are... The image is transformed to the image plane of frame t, and the optimal matching feature point is searched in a neighborhood of a certain radius centered on the transformed position. Compared with exhaustive matching, this method can reduce the search space, speed up the matching process, and improve the matching accuracy.

[0092] After performing motion model tracking, the initial pose of the side-scan sonar can be obtained. Then, BA optimization is performed on the pose to improve its estimation accuracy. This involves considering the set of all background points in the current frame. The BA optimization problem can be described as follows:

[0093]

[0094] in, Let represent the homogeneous coordinates of the i-th background point in the world coordinate system, and Observations express The slant range measured by the side-scan sonar in frame t; ρ(·) represents the robust kernel function; ∑ t,i Let represent the covariance matrix.

[0095] This BA optimization problem can be iteratively optimized using the Levenberg-Marquardt method, where only the side-scan sonar pose is considered. Perform iterative updates, without affecting 3D points. Iterative updates will be performed. The optimized version... Because it integrates information from side-scan sonar images and inertial sensors, it has high accuracy and can be used to determine the motion or stillness of a target.

[0096] Iterate through each target point in the current frame and calculate its measurement error e. If e is greater than the set dynamic threshold t... d=0.24, then mark the target point as an outside point. If the proportion r of outside points to all feature points of a target is 0.24, then mark the target point as an outside point. d Greater than the threshold t rd If the value is 0.3, the target is considered a dynamic target, and all feature points within that target are also marked as dynamic points. Otherwise, the target is considered a static target, and only the outer points are marked as dynamic points, while the remaining feature points are marked as static points, i.e., background points. These dynamic target points will not participate in subsequent BA optimization, but they will still participate in the matching of subsequent side-scan sonar images because a moving target may switch to a stationary state.

[0097] S6. By tightly coupling and optimizing the pose of the unmanned surface vessel based on the fusion information of side-scan sonar and inertial sensors, a more accurate pose can be obtained.

[0098] For the t-th keyframe, the pose of the unmanned surface vessel It can be abbreviated as T t ∈SE(3), unmanned surface vessel pose T t In addition to the optimization variables, there is also the velocity v in the world coordinate system. t gyroscope zero bias and accelerometer zero bias These optimization variables can be collectively referred to as state variables.

[0099]

[0100] Assuming there are a total of k+1 keyframes and l map points, the keyframe state variables can be uniformly denoted as... Map points are uniformly denoted as Therefore, the BA optimization problem can be expressed as:

[0101]

[0102] in, The side-scan sonar measurement constraint factor represents the pose of the t-th keyframe and the i-th map point in the frame. This represents the set of map points in the t-th keyframe; This represents the pre-integration factor between the pose of keyframe t and keyframe (t+1), and it is composed of the rotation residual. velocity residual Translational residual constitute; The motion constraint factor of the unmanned surface vessel (USV) represents the pose between the t-th keyframe and the tr-th keyframe. For each keyframe, it is traced back n... r Each keyframe is used to construct motion constraint factors for the unmanned surface vessel. t,i ,∑ t,t+1and ∑ t,t-r The covariance matrices for the three factors are respectively.

[0103] S61. Construction of side-scan sonar measurement constraint factors.

[0104] The side-scan sonar measurement constraint factor between the pose of the t-th keyframe and the i-th map point in the frame is:

[0105]

[0106] in, T sb The extrinsic parameters between the IMU and the side-scan sonar are optimized only for... and Iterative updates will be performed.

[0107] S62, Construction of IMU pre-integration factors.

[0108] Three pre-integral quantities can be calculated from IMU measurements, and corresponding three residuals can be constructed. The rotational residual between the pose of frame t and frame t+1 is:

[0109]

[0110] Where, Log(·) = ln(·) ∨ , This is the pre-integral value of the rotation between two frames.

[0111] The velocity residual between the poses of frame t and frame t+1 is:

[0112]

[0113] in, Pre-integral velocity between two frames.

[0114] The translation residual between the poses of frame t and frame t+1 is:

[0115]

[0116] in, This is the pre-integral quantity for translation between two frames.

[0117] S63. Construction of motion constraint factors for unmanned surface vessels.

[0118] This embodiment employs an unmanned surface vessel motion constraint factor, which assumes that the relative motion within a small area on the ocean follows planar motion and constrains one translational degree of freedom and two rotational degrees of freedom of the relative motion in order to optimize the two poses of the relative motion before and after.

[0119] Use v = [v1v2θ] TRepresent a (2) Lie algebras, where [v1v2] T Let ξ represent the translation component, and θ represent the pitch angle; use ξ = [r1r2r3ω1ω2ω3]. T Represent a (3) Lie algebras, where [r1r2r3] T Let [ω1ω2ω3] represent the translational component and [ω1ω2ω3] represent the rotational component. Considering restricting the translational degree of freedom along the y-axis and the rotational degrees of freedom along the x and z axes, the transformation relationship between them can be defined as follows:

[0120]

[0121] For the pose of the t-th keyframe The pose of the tr-th frame can be traced back. The keyframe pairs are composed of the following relative poses:

[0122]

[0123] right By restricting the translational degree of freedom along the y-axis and the rotational degrees of freedom along the x and z axes, we can obtain... (2) The Lie algebra is:

[0124]

[0125] Then get (2) Lie algebra Re-transform the transformation matrix, and we have:

[0126]

[0127] Therefore, the motion constraint factor of the unmanned surface vessel can be constructed as follows:

[0128]

[0129] in, and All of them will undergo iterative updates.

[0130] S7. Test this embodiment in a real marine environment and estimate the trajectory of the unmanned surface vessel.

[0131] In this embodiment, a positioning test experiment of the unmanned surface vessel (USV) was conducted in a real marine environment. An EdgeTech 2205 side-scan sonar and IMU sensor were mounted on the USV, and the USV was controlled to move on the ocean surface. Table 1 shows the relevant parameters of the side-scan sonar and IMU.

[0132] Table 1

[0133]

[0134]

[0135] During the test, by reading side-scan sonar images and IMU measurements, and executing steps S1-S6, this embodiment can estimate the pose of the unmanned surface vessel at each moment. Figure 4 The estimated trajectory of the unmanned surface vessel in this test is shown.

[0136] In another embodiment of this application, an unmanned surface vessel positioning system based on the fusion of side-scan sonar and inertial sensor is provided. The system includes a feature extraction module, a target detection module, a filtering module, a pose calculation module, a pose initial value calculation and discrimination module, and a tightly coupled optimization module.

[0137] The feature extraction module is used to extract ORB feature points from the side-scan sonar image;

[0138] The target detection module is used to perform a parallel target detection algorithm on the side-scan sonar image, and combine optical flow tracing to predict the target detection box of the current frame to obtain the semantic information of the current frame;

[0139] The filtering module is used to filter background points and target points by combining feature point depth and semantic information of target detection.

[0140] The pose calculation module is used to perform pose calculation based on the IMU pre-integration method;

[0141] The pose initial value calculation and discrimination module is used to calculate the initial pose value of the unmanned surface vessel based on the IMU calculation results and feature point information, and to discriminate the dynamic and static status of the target.

[0142] The tightly coupled optimization module is used to perform tightly coupled optimization of the unmanned surface vessel's attitude based on the fusion information of side-scan sonar and inertial sensors, so as to obtain a more accurate attitude.

[0143] It should be noted that the unmanned surface vessel (USV) positioning system based on the fusion of side-scan sonar and inertial sensors of the present invention corresponds one-to-one with the unmanned surface vessel (USV) positioning method based on the fusion of side-scan sonar and inertial sensors of the present invention. The technical features and beneficial effects described in the embodiments of the unmanned surface vessel (USV) positioning method based on the fusion of side-scan sonar and inertial sensors described above are applicable to the embodiments of the unmanned surface vessel (USV) positioning method based on the fusion of side-scan sonar and inertial sensors. For details, please refer to the description in the embodiments of the method of the present invention, which will not be repeated here.

[0144] Furthermore, in the above embodiments of the unmanned surface vessel positioning system based on the fusion of side-scan sonar and inertial sensors, the logical division of each program module is only an example. In actual applications, the above functions can be assigned to different program modules as needed, for example, for the sake of corresponding hardware configuration requirements or the convenience of software implementation. That is, the internal structure of the unmanned surface vessel positioning system based on the fusion of side-scan sonar and inertial sensors can be divided into different program modules to complete all or part of the functions described above.

[0145] In another embodiment, an unmanned surface vessel (USV) is provided to implement a localization method based on the fusion of side-scan sonar and inertial sensors. The USV may include a first processor, a first memory, and a bus, and may also include a computer program stored in the first memory and executable on the first processor, such as a USV localization program based on the fusion of side-scan sonar and inertial sensors.

[0146] The first memory includes at least one type of readable storage medium, such as flash memory, portable hard drive, multimedia card, card-type memory (e.g., SD or DX memory), magnetic memory, disk, optical disk, etc. In some embodiments, the first memory can be an internal storage unit of the unmanned surface vessel (USV), such as the USV's portable hard drive. In other embodiments, the first memory can be an external storage device of the USV, such as a plug-in portable hard drive, smart media card (SMC), secure digital card (SD), flash card, etc., equipped on the USV. Furthermore, the first memory can include both internal and external storage units of the USV. The first memory can be used not only to store application software and various types of data installed on the USV, such as the code for a USV positioning program based on the fusion of side-scan sonar and inertial sensors, but also to temporarily store data that has been output or will be output.

[0147] In some embodiments, the first processor may be composed of integrated circuits, such as a single packaged integrated circuit or multiple integrated circuits packaged with the same or different functions, including combinations of one or more central processing units (CPUs), microprocessors, digital processing chips, graphics processors, and various control chips. The first processor is the control unit of the electronic device, connecting various components of the entire electronic device through various interfaces and lines. It executes programs or modules stored in the first memory and calls data stored in the first memory to perform various functions and process data for the unmanned surface vessel.

[0148] The unmanned surface vessel (USV) localization program, based on the fusion of side-scan sonar and inertial sensors and stored in the first memory, is a combination of multiple instructions. When run in the first processor, it can achieve the following:

[0149] ORB feature points are extracted from side-scan sonar images;

[0150] A parallel target detection algorithm is executed on the side-scan sonar image, and the target detection box of the current frame is predicted by combining optical flow tracing to obtain the semantic information of the current frame;

[0151] By combining feature point depth and semantic information from object detection, background points and target points are filtered out.

[0152] Pose calculation based on IMU pre-integration method;

[0153] Based on the IMU calculation results and feature point information, the initial pose of the unmanned surface vessel is calculated, and the dynamic and static status of the target is determined.

[0154] By tightly coupling and optimizing the pose of the unmanned surface vessel based on the fusion information of side-scan sonar and inertial sensors, a more accurate pose can be obtained.

[0155] Furthermore, if the modules / units integrated into the unmanned surface vessel are implemented as software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium. The computer-readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, or a read-only memory (ROM).

[0156] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments described above. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0157] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0158] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A positioning method for an unmanned surface vehicle based on fusion of a side-scan sonar and inertial sensors, characterized in that, Includes the following steps: ORB feature points are extracted from side-scan sonar images; A parallel target detection algorithm is executed on the side-scan sonar image, and the target detection box of the current frame is predicted by combining optical flow tracing to obtain the semantic information of the current frame; By combining feature point depth and semantic information from object detection, background points and target points are filtered out. Pose calculation based on IMU pre-integration method; Based on the IMU calculation results and feature point information, the initial pose of the unmanned surface vessel is calculated, and the dynamic and static status of the target is determined. By tightly coupling and optimizing the pose of the unmanned surface vessel based on the fusion information of side-scan sonar and inertial sensors, a more accurate pose can be obtained.

2. The positioning method of the unmanned surface vehicle based on the fusion of the side scan sonar and the inertial sensor according to claim 1, characterized in that, The extraction of ORB feature points from the side-scan sonar image specifically involves: For the input side-scan sonar image, two parallel threads are started to extract ORB feature points from the two side-scan sonar images respectively. After constructing the Gaussian pyramid, all pixels of the image at different scales are traversed to extract the FAST key points with orientation and calculate the rotation-invariant BRIEF descriptor. Finally, the extracted ORB feature points are homogenized.

3. The positioning method of the unmanned surface vehicle based on the fusion of the side scan sonar and the inertial sensor according to claim 1, characterized in that, The parallel target detection algorithm is executed on the contralateral scan sonar image, and the target detection box of the current frame is predicted by optical flow tracing to obtain the semantic information of the current frame, specifically: Start the tracking thread and the target detection thread; In the target detection thread, Shi-Tomasi corner points are extracted from the current frame side-scan sonar image, and a circular mask of a certain radius is drawn at the center of the 2D detection box. Only the Shi-Tomasi corner points in the circular mask are retained. At the same time, the YOLOX network is called to perform target detection on the side-scan sonar image. In the tracking thread, the Shi-Tomasi corner points of the previous frame are tracked to the current frame using optical flow tracing. For the corner point of the k-th target in the previous frame, the deviation between its corner point center and the center of the 2D detection box is calculated. Based on this deviation and the corner point center of the current frame, the center of the 2D detection box of the current frame is estimated to construct the 2D detection box of the current frame.

4. The positioning method of the unmanned surface vehicle based on the fusion of the side scan sonar and the inertial sensor according to claim 1, characterized in that, The process of combining feature point depth and semantic information from target detection to filter background and target points specifically involves: The extracted ORB feature points are assigned to each 2D detection box; the depth value of each feature point is obtained, and the median depth of the corner points in each 2D detection box is calculated as the depth of the corresponding target. Iterate through each ORB feature point and count the number of 2D detection boxes n containing that feature point; If n=0, then mark the feature point as a background point; If n=1, the depth of the feature point is compared with the depth of the target. If the depth difference is less than the threshold and the feature point is close to the center of the detection box, the feature point is marked as a target point; otherwise, it is marked as a background point. If n>1, the depth difference between the feature point and each target is calculated, and the minimum difference is taken. If the minimum difference is less than the threshold and the feature point is close to the center of the detection box, the feature point is marked as the corresponding target point; otherwise, it is marked as a background point.

5. The positioning method of the unmanned surface vehicle based on the fusion of the side scan sonar and the inertial sensor according to claim 1, characterized in that, The pose calculation based on the IMU pre-integration method is specifically as follows: Set the moment and the moment correspond to two side-scan sonar image frames respectively, through the cumulative integration of IMU measurement values, the rotation matrix , velocity and displacement of the first frame are obtained, and the rotation matrix , velocity and displacement at the first frame are obtained. where and denote angular velocity and acceleration measurements; and denote gyroscope and accelerometer biases, and denote gyroscope and accelerometer measurement noise; is the gravitational acceleration; is the IMU measurement time interval, To avoid frequent re-integrations in the back-end optimization, the pre-integration between two side scan sonar image frames is computed, i.e.: wherein, , and are the rotational, velocity and displacement pre-integrals between the first frame to the first frame, respectively, and , .

6. The positioning method of the unmanned surface vehicle based on the fusion of the side scan sonar and the inertial sensor according to claim 1, characterized in that, The process involves calculating the initial pose of the unmanned surface vessel based on the IMU calculation results and feature point information, and determining the dynamic and static status of the target. Specifically: The current frame pose is obtained by pose solution of the IMU The 3D points corresponding to the ORB feature points in the previous frame are transformed to the image plane of the current frame through the current frame pose, and the matching feature points are searched in the neighborhood. Based on the matching results, the pose is optimized using Beta Analysis (BA). The BA optimization problem is described as follows: in, Indicates the first The homogeneous coordinates of the background point in the world coordinate system, and ; Observed values , express In the Slant range during frame-side scan sonar measurement; Represents a robust kernel function; Represent the covariance matrix; Traverse each target point in the current frame, calculate its measurement error, and mark it as an outlier if the error is greater than the dynamic threshold. If the proportion of outer points in a target is greater than a threshold, the target is marked as a dynamic target and all its feature points are marked as dynamic points; otherwise, only the outer points are marked as dynamic points and the remaining feature points are marked as background points.

7. The positioning method of the unmanned surface vehicle based on the fusion of the side scan sonar and the inertial sensor according to claim 1, characterized in that, The method of tightly coupling and optimizing the pose of the unmanned surface vessel (USV) based on the fusion information of side-scan sonar and inertial sensors to obtain a more accurate pose is as follows: Tight coupling optimization is expressed as: wherein, represents a side-scan sonar measurement constraint factor; represents an IMU pre-integration factor; represents an unmanned surface vehicle motion constraint factor, , and covariance matrices corresponding to the three factors, respectively. The side-scan sonar measurement constraint factor is the difference between the observed value and the reprojection of the side-scan sonar measurement constraint factor between the pose of the t-th keyframe and the i-th map point in the frame. The IMU pre-integration factor includes the rotation residual, velocity residual, and translation residual between the pose of frame t and frame t+1. The motion constraint factor for the unmanned surface vessel (USV) is as follows: For the pose of the t-th keyframe, the pose of the tr-th keyframe is traced back, the relative pose is calculated, and its y-axis translation and x and z-axis rotation are restricted to obtain the se(2) Lie algebra. Then, the transformation matrix is ​​converted back to construct the motion constraint factor for the USV.

8. A positioning system for unmanned surface vessels based on the fusion of side-scan sonar and inertial sensors, characterized in that, The unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensor, as described in any one of claims 1-7, includes a feature extraction module, a target detection module, a filtering module, a pose calculation module, a pose initial value calculation and discrimination module, and a tightly coupled optimization module. The feature extraction module is used to extract ORB feature points from the side-scan sonar image; The target detection module is used to perform a parallel target detection algorithm on the side-scan sonar image, and combine optical flow tracing to predict the target detection box of the current frame to obtain the semantic information of the current frame; The filtering module is used to filter background points and target points by combining feature point depth and semantic information of target detection. The pose calculation module is used to perform pose calculation based on the IMU pre-integration method; The pose initial value calculation and discrimination module is used to calculate the initial pose value of the unmanned surface vessel based on the IMU calculation results and feature point information, and to discriminate the dynamic and static status of the target. The tightly coupled optimization module is used to perform tightly coupled optimization of the unmanned surface vessel's attitude based on the fusion information of side-scan sonar and inertial sensors, so as to obtain a more accurate attitude.

9. An unmanned surface vessel, characterized in that, The unmanned surface vessel includes: At least one processor; and, A memory communicatively connected to the at least one processor; wherein, The memory stores computer program instructions that can be executed by the at least one processor, which enables the at least one processor to perform the unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensors as described in any one of claims 1-7.

10. A computer-readable storage medium storing a program, characterized in that, When the program is executed by the processor, it implements the unmanned surface vessel positioning method based on the fusion of side-scan sonar and inertial sensors as described in any one of claims 1-7.

Images