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

Abstract

The invention discloses an unmanned ship positioning method and system based on fusion of a side-scan sonar and an inertial sensor, and a medium. The method comprises the following steps: extracting ORB feature points from a side-scan sonar image; executing a parallel target detection algorithm on the side-scan sonar image to obtain semantic information of a current frame; screening background points and target points in combination with the depth of the feature points and semantic information of target detection; performing pose calculation based on an IMU (Inertial Measurement Unit) pre-integration method; according to the IMU resolving result and the feature point information, solving the initial pose value of the unmanned ship, and judging the dynamic and static conditions of the target; tight coupling optimization is carried out on the pose of the unmanned ship based on fusion information of the side-scan sonar and the inertial sensor, and a more accurate pose is obtained. According to the invention, fusion calculation is carried out on the side-scan sonar image and the measurement information of the inertial sensor, and semantic information of target detection is introduced, so that the unmanned ship can still realize high-precision and high-robustness real-time positioning in a complex dynamic marine environment.

Description

Technical Field

[0001] The present invention relates to the field of unmanned boat positioning, and in particular to an unmanned boat positioning method, system and medium based on the fusion of side-scan sonar and inertial sensor. Background Art

[0002] In recent years, unmanned marine vehicle (UAV) technology has demonstrated significant potential and broad application prospects in areas such as military reconnaissance, environmental monitoring, resource exploration, and search and rescue. To ensure efficient mission execution, the development of high-precision UAV positioning systems is crucial, as they play a key role in improving the UAV's perception capabilities and motion control accuracy. Therefore, achieving precise positioning of UAVs in complex and dynamic marine environments has become a cutting-edge research topic.

[0003] Although the unmanned vehicle positioning method based on real-time kinematic carrier phase differential (RTK) technology is simple to operate, its signal strength and positioning accuracy are easily affected by weather conditions and topography. It is also expensive and has limited accuracy. Side-scan sonar (SSS) is a commonly used device that can accurately measure the seabed topography by emitting fan-shaped acoustic pulses on both sides of the unmanned vehicle or towed body. Side-scan sonar has the advantages of a wide measurement range, high resolution, and good cost-effectiveness. The stability and structural characteristics of the seabed topography provide reliable reference information for unmanned vehicle positioning. However, side-scan sonar images are easily affected by factors such as seabed reverberation and water noise, resulting in insufficient robustness of positioning systems that rely solely on side-scan sonar.

[0004] Inertial measurement units (IMUs) can quickly and accurately measure rapid motion, complementing side-scan sonar. Therefore, achieving high-precision positioning for unmanned vehicles by integrating side-scan sonar and IMU measurement data, taking into account the unique characteristics of the ocean environment, has become a pressing technical challenge.

[0005] In the ocean, dynamic objects such as schools of fish, underwater robots, and submersibles can interfere with the precise positioning of unmanned vehicles (UAVs). Using existing target detection algorithms (such as the YOLO family of networks) to process side-scan sonar images can extract semantic information about dynamic objects. However, given the high real-time requirements of UAV positioning systems, achieving fast and accurate target detection remains a challenge. Further exploring how to effectively integrate target semantic information into UAV positioning systems to enhance their positioning performance in complex and dynamic ocean environments is an important research topic worthy of in-depth exploration. Summary of the Invention

[0006] The main purpose of the present invention is to overcome the shortcomings and deficiencies of the existing technology and provide an unmanned vehicle positioning method, system and medium based on the fusion of side-scan sonar and inertial sensors. The method fuses and calculates the side-scan sonar image and the measurement information of the inertial sensor, and introduces the semantic information of target detection, so that the unmanned vehicle can still achieve high-precision and high-robust real-time positioning in a complex and dynamic marine environment.

[0007] In order to achieve the above object, the present invention adopts the following technical solutions:

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

[0009] Extract ORB feature points from side scan sonar images;

[0010] Execute a parallel target detection algorithm on the side-scan sonar image, combine it with optical flow tracking to predict the target detection box of the current frame, and obtain the semantic information of the current frame;

[0011] Combine the depth of feature points and the semantic information of target detection to filter background points and target points;

[0012] Position and posture calculation based on IMU pre-integration method;

[0013] Based on the IMU calculation results and feature point information, the initial value of the unmanned boat's posture is solved, and the target's movement and stillness are judged;

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

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

[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 images of different scales are traversed to extract FAST key points with directions, and the BRIEF descriptor with rotation invariance is calculated. Finally, the extracted ORB feature points are homogenized.

[0017] As a preferred technical solution, the parallel target detection algorithm is executed on the side scan sonar image, and the target detection frame of the current frame is predicted in combination with optical flow tracking to obtain the semantic information of the current frame, specifically:

[0018] Start the tracking thread and 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 with a certain radius is drawn at the center of the 2D detection frame. Only the Shi-Tomasi corner points in the circular mask are retained, and 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 through the optical flow tracking method; for the corner point of the k-th target in the previous frame, the deviation between its corner point center and the 2D detection frame center is calculated, and the 2D detection frame center of the current frame is estimated based on the deviation and the corner point center of the current frame to construct the 2D detection frame of the current frame.

[0021] As a preferred technical solution, the background points and target points are screened by combining the depth of feature points and the semantic information of target detection, specifically:

[0022] Assign the extracted ORB feature points to each 2D detection frame; obtain the depth value of each feature point, and calculate the median depth of the corner points in each 2D detection frame as the depth of the corresponding target;

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

[0024] As a preferred technical solution, the posture solution is performed based on the IMU pre-integration method, specifically:

[0025] Assume that the i-th moment and the j-th moment correspond to two side-scan sonar image frames respectively, and the rotation matrix R of the i-th frame is obtained by accumulating the IMU measurement values. i , speed v i and displacement p i , find the rotation matrix R at the jth frame j , speed v j and displacement p j :

[0026]

[0027] in, and Represents angular velocity and acceleration measurements; and Indicates the gyroscope and accelerometer bias, and represents the measurement noise of gyroscope and accelerometer; g is the acceleration of gravity; Δt is the IMU measurement time interval, To avoid frequent re-integration in back-end optimization, the pre-integration amount between two side-scan sonar image frames is calculated, namely:

[0028]

[0029] Where, ΔR ij , Δv ij and Δp ij The rotation pre-integration quantity, velocity pre-integration quantity and displacement pre-integration quantity between the i-th frame and the j-th frame are respectively Δv ik =v k -v i .

[0030] As a preferred technical solution, the initial value of the unmanned boat's posture is solved based on the IMU solution results and feature point information, and the dynamic and static conditions of the target are judged, specifically:

[0031] The current frame pose is obtained by solving the IMU pose Perform nearest neighbor matching of ORB feature points through this pose, transform the 3D point corresponding to the ORB feature point in the previous frame to the image plane of the current frame through the pose of the current frame, and search for matching feature points in the neighborhood;

[0032] According to the matching results, the pose is optimized by BA. The BA optimization problem is expressed as:

[0033]

[0034] in, represents the homogeneous coordinates of the i-th background point in the world coordinate system, and Observations express The slant range of the sidescan sonar measurement at frame t; ρ(·) represents the robust kernel function; ∑ t,i represents the covariance matrix;

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

[0036] If the ratio of outliers 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 outliers are marked as dynamic points and the remaining feature points are marked as background points.

[0037] As a preferred technical solution, the posture of the unmanned boat is tightly coupled and optimized based on the fusion information of the side scan sonar and inertial sensor to obtain a more accurate posture, specifically:

[0038] Tightly coupled optimization is expressed as:

[0039]

[0040] in, represents the side scan sonar measurement constraint factor; represents the pre-integration factor; represents the motion constraint factor of the unmanned boat, ∑ t,i ,∑ t,t+1 and ∑ t,t-r The covariance matrices corresponding to the three factors respectively;

[0041] The side scan sonar measurement constraint factor: The side scan sonar measurement constraint factor of the t-th key frame pose and the i-th map point in the frame is the difference between the observation value and the reprojection;

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

[0043] Shrinking the unmanned boat motion constraint factor: For the pose of the t-th key frame, trace back to the pose of the tr-th frame, calculate the relative pose, restrict its y-axis translation and x- and z-axis rotations, obtain the se(2) Lie algebra, and then convert it back to the transformation matrix to construct the unmanned boat motion constraint factor.

[0044] In a second aspect, the present invention provides an unmanned boat positioning system based on the fusion of side-scan sonar and inertial sensors, which is applied to the unmanned boat positioning method based on the fusion of side-scan sonar and inertial sensors, and includes a feature extraction module, a target detection module, a screening module, a posture solution module, a posture initial value solution 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 execute a parallel target detection algorithm on the side scan sonar image, combine optical flow tracking to predict the target detection frame of the current frame, and obtain the semantic information of the current frame;

[0047] The screening module is used to screen background points and target points by combining the depth of feature points and semantic information of target detection;

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

[0049] The initial pose solution and discrimination module is used to solve the initial pose of the unmanned boat based on the IMU solution results and feature point information, and to discriminate the movement and stillness of the target;

[0050] The tightly coupled optimization module is used to perform tightly coupled optimization on the posture of the unmanned boat based on the fusion information of the side scan sonar and the inertial sensor to obtain a more accurate posture.

[0051] In a third aspect, the present invention provides an unmanned boat, 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, and the computer program instructions are executed by the at least one processor so that the at least one processor can execute the unmanned boat positioning method based on the fusion of side scan sonar and inertial sensors.

[0055] In a fourth aspect, the present invention provides a computer-readable storage medium storing a program, which, when executed by a processor, implements the unmanned boat positioning method based on the fusion of side-scan sonar and inertial sensor.

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

[0057] 1. The present invention combines the characteristics of side-scan sonar and inertial sensors to tightly couple and fuse their measurement information. Based on the motion characteristics of the unmanned vehicle in the marine environment, the invention imposes unmanned vehicle motion constraints on the estimated position, enabling the unmanned vehicle to achieve high-precision and high-robust positioning in complex marine environments.

[0058] 2. By introducing semantic information of target detection, the present invention can identify potential dynamic objects and determine the dynamic and static conditions of the target by solving the initial value of the unmanned boat's posture, thereby avoiding the adverse effects of dynamic objects on system positioning and enabling the unmanned boat to maintain high-precision positioning even in a highly dynamic marine environment.

[0059] 3. Based on multi-threaded programming, the present invention introduces the optical flow tracking method and runs it in parallel with the YOLOX target detection network to achieve fast and accurate target detection, thereby ensuring the real-time positioning of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0061] Figure 1 The figure is a flow chart of an unmanned boat positioning method based on the fusion of side-scan sonar and inertial sensor in an embodiment of the present invention.

[0062] Figure 2 Schematic diagram of the principle of the optical flow-assisted target detection method in an embodiment of the present invention.

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

[0064] Figure 4 This is a diagram of the estimated trajectory of the unmanned boat in a marine environment test experiment according to an embodiment of the present invention. DETAILED DESCRIPTION

[0065] In order to enable those skilled in the art to better understand the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

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

[0067] like Figure 1 As shown, this embodiment provides an unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor, including the following steps:

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

[0069] This example launches two subthreads to concurrently extract 2000 ORB feature points from the input left and right sidescan sonar images. First, an 8-layer Gaussian pyramid is constructed for the input sidescan sonar image, with a scale factor of 1.2. Next, directional FAST keypoints are extracted from each layer of the side-scan sonar image. These keypoints are filtered using a threshold of 20. A rotationally invariant BRIEF descriptor is then calculated. Finally, the extracted ORB feature points are homogenized using an octree algorithm.

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

[0071] In order to ensure the real-time performance of target detection, the target detection results of the previous frame can be combined with the optical flow tracking technology to obtain the target detection results of the current frame. This embodiment will start two sub-threads, called the tracking thread and the target detection thread. In the tracking thread, optical flow-assisted target detection will be performed in combination with the detection results of the previous frame. In the target detection thread, target detection using the YOLOX network (trained on a side-scan sonar image dataset) and extraction of Shi-Tomasi corner points will be performed in parallel, and the results obtained 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 more accurate positions are obtained through sub-pixel optimization. In order to retain the corner points on the target in the 2D detection frame, this embodiment adopts a corner point screening method based on a circular mask. This method draws a circular mask of a certain radius at the center of the 2D detection frame and only retains the Shi-Tomasi corner points in the circular mask. For 2D detection frames larger than 100 pixels, the circular mask radius is set to 60 pixels; for 2D detection frames between 40 pixels and 100 pixels, the circular mask radius is set to 0.5 times the size of the frame; and for 2D detection frames smaller than 40 pixels, the circular mask will not be drawn because the area is too small to extract a sufficient number of corner points. This method ensures that a sufficiently large area in the 2D detection frame is considered while avoiding the problem of corner points falling on the background.

[0073] In the tracking thread, such as Figure 2 As shown, optical flow tracking is used to predict the 2D detection box in the t-th frame from the target detection results in the t-1th frame. By using the optical flow tracking method, the Shi-Tomasi corner points in the t-1th frame are tracked to the t-th frame. For the corner point of the kth target in the t-1th frame, its corner point center can be easily obtained. And the center of its 2D detection box set Similarly, the center angle of the corner point of the tth frame Then, the deviation between the two centers in the t-1 frame is for:

[0074]

[0075] Since the time interval between the two frames is very short, the deviation between the two centers in the t-th frame is very small. Therefore, the center of the 2D detection box in the t-th frame can be obtained for:

[0076]

[0077] Since the size of the 2D detection frame does not change much between the two frames, we can use Construct a 2D detection box for the center of frame t.

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

[0079] After performing target detection on the input sidescan sonar image, the extracted ORB feature points are assigned to each 2D detection frame, which may contain both background and target feature points. Simply removing all points may result in too few feature points, affecting pose estimation accuracy. Therefore, it is necessary to filter out background and target points. Furthermore, the 2D detection frames of different targets may overlap, so the feature points in these overlapping areas need to be assigned to the corresponding 2D detection frames.

[0080] To address these two issues, this embodiment employs a feature point assignment method based on target depth. First, the depth value of each corner point in the sonar image frame is obtained. For each corner point in the 2D detection frame, the median is calculated and set as the corresponding target depth.

[0081] After that, traverse each ORB feature point and count the number of 2D detection boxes containing the feature point n. If n = 0, set the feature point label to background; if n = 1, compare the depth of the feature point with the target depth, and if the depth difference is less than the threshold t d If the point is close to the center of the detection frame, the feature point label is set to the target, otherwise it is set to the background point; if n>1, calculate the depth difference between the feature point and each target, count the minimum difference, and if the difference is less than the threshold t d If the point is close to the center of the detection frame, the feature point label is set to the corresponding target, otherwise it is set to the background.

[0082] S4. Perform posture calculation based on the IMU pre-integration method to obtain the current posture 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. Assume that 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 integral quantities between v i and v j , p i and p j all depend on R k . Therefore, these two integral quantities also need to be recalculated. In the backend optimization, the pose is updated frequently, and frequent recalculation of integration will affect the real-time performance of the system. Therefore, the relative change amount of motion between two sidescan sonar image frames is used to replace the direct integration, that is, pre-integration, and the obtained relative change amount is called the pre-integral quantity, namely:

[0086]

[0087] where, ΔR ij , Δv ij and Δp ij are the rotation pre-integral quantity, velocity pre-integral quantity and displacement pre-integral 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, the initial value of the unmanned boat's posture is solved, and the target's movement and stillness are judged.

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

[0090]

[0091] Afterwards, you can use the current frame pose To match ORB feature points. The 3D points corresponding to the ORB feature points in the t-1 frame are matched by Transform the image to the image plane of frame t and search for the best matching feature point in a neighborhood with a certain radius centered on the transformed position. Compared with exhaustive matching, this method can narrow the search space, speed up matching, and improve matching accuracy.

[0092] After performing motion model tracking, the initial pose of the side scan sonar can be obtained Then the pose is optimized by BA to improve its estimation accuracy. Consider the set of all background points in the current frame The BA optimization problem can be expressed as:

[0093]

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

[0095] The BA optimization problem can be iteratively optimized using the Levenberg-Marquardt method, where only the side scan sonar pose is optimized. Perform iterative updates without changing the 3D points Perform iterative updates. Since it integrates information from side-scan sonar images and inertial sensors, it has a relatively high accuracy and can be used to judge the movement and stillness of the target.

[0096] Traverse 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 the target point is marked as an outlier. If the proportion of outliers in a target to all feature points of the target is r d Greater than the threshold t rd =0.3, then the target is considered dynamic, and all feature points in the target are marked as dynamic points. Otherwise, the target is considered static, and only the outer points are marked as dynamic points, while the remaining feature points are marked as static points, that is, background points. These dynamic target points will not participate in the subsequent BA optimization, but will still participate in the subsequent side scan sonar image matching because a moving target may switch to a stationary state.

[0097] S6. By tightly coupling and optimizing the posture of the unmanned boat based on the fusion information of side-scan sonar and inertial sensors, a more accurate posture is obtained.

[0098] For the tth key frame, the unmanned boat pose It can be abbreviated as T t ∈SE(3), the position of the unmanned boat T t is the optimization variable. In addition, the optimization variable also includes the speed v in the world coordinate system t , gyroscope bias and accelerometer bias These optimization variables can be collectively recorded as state variables

[0099]

[0100] Assuming there are k+1 keyframes and l map points in total, the keyframe state variables can be uniformly recorded as The unity of map points is recorded as Then the BA optimization problem can be expressed as:

[0101]

[0102] in, represents the side scan sonar measurement constraint factor between the t-th keyframe pose and the i-th map point in the frame, Represents the set of map points in the t-th keyframe; Represents the pre-integration factor between the t-th keyframe pose and the t+1-th keyframe pose, which is composed of the rotation residual Velocity residual and translation residuals constitute; Represents the motion constraint factor of the unmanned boat between the pose of the t-th key frame and the pose of the tr-th key frame. For each key frame, it will be traced back n times. r key frames and construct the motion constraint factors of the unmanned boat respectively. t,i ,∑ t,t+1and ∑ t,t-r The covariance matrices corresponding to the three factors respectively.

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

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

[0105]

[0106] in, T sb It is the external parameter between IMU and side scan sonar. and Perform iterative updates.

[0107] S62. Construction of IMU pre-integration factor.

[0108] Three pre-integrated components can be calculated from the IMU measurements, and the corresponding three residuals can be constructed. Among them, the rotation residual between the t-th frame and the t+1-th frame pose is

[0109]

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

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

[0112]

[0113] in, is the velocity pre-integration between two frames.

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

[0115]

[0116] in, is the pre-integrated translation between two frames.

[0117] S63. Construction of unmanned boat motion constraint factors.

[0118] This embodiment adopts an unmanned boat motion constraint factor, which assumes that the relative motion within a small range on the ocean obeys planar motion, and constrains one translational degree of freedom and two rotational degrees of freedom of the relative motion to optimize the two forward and backward postures of the relative motion.

[0119] Use v = [v1v2θ] TIndicates a (2) Lie algebra, where [v1v2] T represents the translation component, θ represents the pitch angle; ξ=[r1r2r3ω1ω2ω3] T Indicates a (3) Lie algebra, where [r1r2r3] T represents the translation component, and [ω1ω2ω3] represents the rotation component. Considering the limitation of the translational freedom of the y-axis and the rotational freedom of the x- and z-axes, the conversion relationship between them can be defined as:

[0120]

[0121] For the pose of the tth key frame The pose of the tr frame can be traced back Constitute a pair of key frames, and their relative poses are:

[0122]

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

[0124]

[0125] Then get (2) Lie algebra Reconvert the transformation matrix, we have:

[0126]

[0127] Then, the motion constraint factor of the unmanned boat can be constructed as:

[0128]

[0129] in, and All will be updated iteratively.

[0130] S7. Test this embodiment in an actual ocean environment and estimate the trajectory of the unmanned boat.

[0131] In this example, a positioning test experiment for an unmanned vehicle (UAV) was conducted in a real ocean environment. An EdgeTech 2205 side-scan sonar and an IMU sensor were installed on the UAV to control its movement over the ocean. Table 1 shows the relevant parameters of the side-scan sonar and IMU.

[0132] Table 1

[0133]

[0134]

[0135] During the test, by reading the side-scan sonar image and IMU measurement values ​​and executing steps S1-S6, this embodiment can estimate the position of the unmanned boat at each moment. Figure 4 The estimated trajectory of the unmanned vehicle in this test is shown.

[0136] In another embodiment of the present application, an unmanned boat positioning system based on the fusion of side-scan sonar and inertial sensors is provided, the system comprising a feature extraction module, a target detection module, a screening module, a posture solution module, a posture initial value solution 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 execute a parallel target detection algorithm on the side scan sonar image, combine optical flow tracking to predict the target detection frame of the current frame, and obtain the semantic information of the current frame;

[0139] The screening module is used to screen background points and target points by combining the depth of feature points and semantic information of target detection;

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

[0141] The initial pose solution and discrimination module is used to solve the initial pose of the unmanned boat based on the IMU solution results and feature point information, and to discriminate the movement and stillness of the target;

[0142] The tightly coupled optimization module is used to perform tightly coupled optimization on the posture of the unmanned boat based on the fusion information of the side scan sonar and the inertial sensor to obtain a more accurate posture.

[0143] It should be noted that the unmanned boat positioning system based on the fusion of side scan sonar and inertial sensors of the present invention corresponds one to one with the unmanned boat 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 above-mentioned embodiment of the unmanned boat positioning method based on the fusion of side scan sonar and inertial sensors are applicable to the embodiment of the unmanned boat positioning based on the fusion of side scan sonar and inertial sensors. For specific contents, please refer to the description in the embodiment of the method of the present invention. No further details will be given here. This is hereby declared.

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

[0145] In another embodiment, an unmanned boat is provided that implements an unmanned boat positioning method based on the fusion of side-scan sonar and inertial sensors. The unmanned boat 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 an unmanned boat positioning program based on the fusion of side-scan sonar and inertial sensors.

[0146] In which, the first memory includes at least one type of readable storage medium, and the readable storage medium includes a flash memory, a mobile hard disk, a multimedia card, a card-type memory (for example: SD or DX memory, etc.), a magnetic memory, a disk, an optical disk, etc. In some embodiments, the first memory can be an internal storage unit of the unmanned boat, such as a mobile hard disk of the unmanned boat. In other embodiments, the first memory can also be an external storage device of the unmanned boat, such as a plug-in mobile hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. equipped on the unmanned boat. Furthermore, the first memory can also include both an internal storage unit of the unmanned boat and an external storage device. The first memory can not only be used to store application software and various types of data installed on the unmanned boat, such as the code of the unmanned boat positioning program based on the fusion of side scan sonar and inertial sensors, but can also be used to temporarily store data that has been output or is to be output.

[0147] In some embodiments, the first processor may be composed of an integrated circuit, for example, a single packaged integrated circuit, or a plurality of packaged integrated circuits with the same or different functions, including one or more central processing units (CPUs), microprocessors, digital processing chips, graphics processors, and a combination of various control chips. The first processor is the control core (Control Unit) of the electronic device, connecting the various components of the entire electronic device using various interfaces and lines, and executing or executing programs or modules stored in the first memory, as well as calling data stored in the first memory, to perform various functions of the unmanned vehicle and process data.

[0148] The unmanned boat positioning program based on the fusion of side-scan sonar and inertial sensor stored in the first memory of the unmanned boat is a combination of multiple instructions. When running in the first processor, it can achieve the following:

[0149] Extract ORB feature points from side scan sonar images;

[0150] Execute a parallel target detection algorithm on the side-scan sonar image, combine it with optical flow tracking to predict the target detection box of the current frame, and obtain the semantic information of the current frame;

[0151] Combine the depth of feature points and the semantic information of target detection to filter background points and target points;

[0152] Position and posture calculation based on IMU pre-integration method;

[0153] Based on the IMU calculation results and feature point information, the initial value of the unmanned boat's posture is solved, and the target's movement and stillness are judged;

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

[0155] Furthermore, if the modules / units integrated into the unmanned vehicle 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 mobile hard drive, a magnetic disk, an optical disk, a computer memory, or a read-only memory (ROM).

[0156] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database 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), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0157] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, 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 implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. The unmanned boat positioning method based on the fusion of side scan sonar and inertial sensor is characterized by: The steps include: Extract ORB feature points from side scan sonar images; Execute a parallel target detection algorithm on the side-scan sonar image, combine it with optical flow tracking to predict the target detection box of the current frame, and obtain the semantic information of the current frame; Combine the depth of feature points and the semantic information of target detection to filter background points and target points; Position and posture calculation based on IMU pre-integration method; Based on the IMU calculation results and feature point information, the initial value of the unmanned boat's posture is solved, and the target's movement and stillness are judged; By tightly coupling and optimizing the posture of the unmanned boat based on the fusion information of side-scan sonar and inertial sensors, a more accurate posture can be obtained.

2. The unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor according to claim 1 is characterized in that: The extraction of ORB feature points from the side scan sonar image is specifically as follows: 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 images of different scales are traversed to extract FAST key points with directions, and the BRIEF descriptor with rotation invariance is calculated. Finally, the extracted ORB feature points are homogenized.

3. The unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor according to claim 1 is characterized in that: The parallel target detection algorithm is executed on the side scan sonar image, and the target detection frame of the current frame is predicted in combination with optical flow tracking to obtain the semantic information of the current frame. Specifically, Start the tracking thread and 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 with a certain radius is drawn at the center of the 2D detection frame. Only the Shi-Tomasi corner points in the circular mask are retained, and 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 through the optical flow tracking method; for the corner point of the k-th target in the previous frame, the deviation between its corner point center and the 2D detection frame center is calculated, and the 2D detection frame center of the current frame is estimated based on the deviation and the corner point center of the current frame to construct the 2D detection frame of the current frame.

4. The unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor according to claim 1 is characterized in that: The method combines the depth of feature points and the semantic information of target detection to filter background points and target points, specifically: Assign the extracted ORB feature points to each 2D detection frame; obtain the depth value of each feature point, and calculate the median depth of the corner points in each 2D detection frame as the depth of the corresponding target; Traverse each ORB feature point and count the number n of 2D detection boxes containing the feature point; if n = 0, mark the feature point as a background point; If n=1, the depth of the feature point is compared with the target depth. If the depth difference is less than the threshold and the point position is close to the center of the detection frame, the feature point is marked as the 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 point position is close to the center of the detection frame, the feature point is marked as the corresponding target point, otherwise it is marked as a background point.

5. The unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor according to claim 1 is characterized in that: The posture solution based on the IMU pre-integration method is specifically as follows: Assume that the i-th moment and the j-th moment correspond to two side-scan sonar image frames respectively, and the rotation matrix R of the i-th frame is obtained by accumulating the IMU measurement values. i , speed v i and displacement p i , find the rotation matrix R at the jth frame j , speed v j and displacement p j : in, and Represents angular velocity and acceleration measurements; and Indicates the gyroscope and accelerometer bias, and represents the measurement noise of gyroscope and accelerometer; g is the acceleration of gravity; Δt is the IMU measurement time interval, To avoid frequent re-integration in back-end optimization, the pre-integration amount between two side-scan sonar image frames is calculated, namely: Where, ΔR ij , Δv ij and Δp ij The rotation pre-integration quantity, velocity pre-integration quantity and displacement pre-integration quantity between the i-th frame and the j-th frame are respectively Δv ik =v k -v i .

6. The unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor according to claim 1 is characterized in that: According to the IMU calculation results and feature point information, the initial value of the unmanned boat's posture is solved, and the target's dynamic and static conditions are judged. Specifically: The current frame pose is obtained by solving the IMU pose Perform nearest neighbor matching of ORB feature points through this pose, transform the 3D point corresponding to the ORB feature point in the previous frame to the image plane of the current frame through the pose of the current frame, and search for matching feature points in the neighborhood; According to the matching results, the pose is optimized by BA. The BA optimization problem is expressed as: in, represents the homogeneous coordinates of the i-th background point in the world coordinate system, and Observations express The slant range of the sidescan sonar measurement at frame t; ρ(·) represents the robust kernel function; ∑ t,i represents the covariance matrix; Traverse each target point in the current frame and calculate its measurement error. If the error is greater than the dynamic threshold, it is marked as an outlier. If the ratio of outliers 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 outliers are marked as dynamic points and the remaining feature points are marked as background points.

7. The unmanned vehicle positioning method based on the fusion of side scan sonar and inertial sensor according to claim 1 is characterized in that: The above method achieves a more accurate posture by tightly coupling optimization of the UAV posture based on the fusion information of side scan sonar and inertial sensor, specifically: Tightly coupled optimization is expressed as: in, represents the side scan sonar measurement constraint factor; represents the pre-integration factor; represents the motion constraint factor of the unmanned boat, ∑ t,i ,∑ t,t+1 and ∑ t,t-r The covariance matrices corresponding to the three factors respectively; The side scan sonar measurement constraint factor: The side scan sonar measurement constraint factor of the t-th key frame pose and the i-th map point in the frame is the difference between the observation value and the reprojection; The IMU pre-integration factor includes the rotation residual, velocity residual and translation residual between the t-th frame and the t+1-th frame pose; Shrinking the unmanned boat motion constraint factor: For the pose of the t-th key frame, trace back to the pose of the tr-th frame, calculate the relative pose, restrict its y-axis translation and x- and z-axis rotations, obtain the se(2) Lie algebra, and then convert it back to the transformation matrix to construct the unmanned boat motion constraint factor.

8. The unmanned boat positioning system based on the fusion of side-scan sonar and inertial sensor is characterized by: An unmanned boat positioning method based on side-scan sonar and inertial sensor fusion, applied to any one of claims 1-7, comprising a feature extraction module, a target detection module, a screening module, a posture solution module, a posture initial value solution 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 execute a parallel target detection algorithm on the side scan sonar image, combine optical flow tracking to predict the target detection frame of the current frame, and obtain the semantic information of the current frame; The screening module is used to screen background points and target points by combining the depth of feature points and semantic information of target detection; The posture calculation module is used to perform posture calculation based on the IMU pre-integration method; The initial pose solution and discrimination module is used to solve the initial pose of the unmanned boat based on the IMU solution results and feature point information, and to discriminate the movement and stillness of the target; The tightly coupled optimization module is used to perform tightly coupled optimization on the posture of the unmanned boat based on the fusion information of the side scan sonar and the inertial sensor to obtain a more accurate posture.

9. An unmanned boat, characterized in that: The unmanned boat comprises: 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, and the computer program instructions are executed by the at least one processor so that the at least one processor can execute the unmanned boat positioning method based on the fusion of side scan sonar and inertial sensors as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a program, characterized in that: When the program is executed by the processor, the unmanned boat positioning method based on the fusion of side-scan sonar and inertial sensor according to any one of claims 1 to 7 is implemented.

Images