Underwater positioning and mapping method based on improved keyframe decision and posture optimization

Abstract

The present invention discloses an underwater positioning and mapping method based on improved keyframe decision and posture optimization. First, the data of the sensor configured by the AUV is obtained; the sensor data obtained is processed, the system parameters are initialized, and the local map is tracked. On the basis of the ORB‑SLAM3 keyframe selection, it is judged whether the current environment is dynamic and whether the AUV is moving violently according to the observation information of the IMU, and then the keyframe selection is continued; the posture of the selected keyframe is optimized, and the posture correction is performed using double constraints, the local trajectory under large-angle rapid bending motion is restored, and new map points are generated; closed-loop detection and correction are performed; and the global trajectory and map are generated. The present invention solves the problem that the autonomous underwater vehicle may encounter a dynamic environment or its own violent movement, resulting in posture drift, and at the same time greatly improves the accuracy and robustness of the visual-inertial synchronous positioning and mapping system.

Description

Technical Field

[0001] The present invention relates to an underwater positioning and mapping method for an autonomous underwater vehicle (AUV), and in particular to an underwater positioning and mapping method based on improved key frame decision and posture optimization. Background Art

[0002] AUVs are needed for underwater detection, and accurate underwater positioning and mapping methods can provide guarantees for the operation of AUVs. The visual SLAM algorithm is a relatively good method for underwater positioning and mapping, but underwater light propagation is easily affected by strong scattering and absorption, resulting in blurred images and affecting the validity of visual data. The inertial measurement unit (IMU) can provide information on the acceleration and angular velocity of the AUV, perform attitude estimation, and compensate for errors in visual data. By combining IMU and visual data and running the visual inertial SLAM algorithm, more accurate positioning and map information can be obtained, greatly improving the robustness and accuracy of the positioning and mapping system.

[0003] In the visual-inertial SLAM system, IMU can provide motion compensation, but it can only provide relatively accurate information in the short term. For a long time, it may encounter a dynamic environment or the AUV itself may move violently, resulting in posture drift and large positioning errors. At present, the key frame selection of the SLAM system is generally based on fixed criteria (such as the change in camera posture, reprojection error, etc.). The key frame decision method can be improved by combining the observation information of the IMU and taking some adaptive strategies to improve the posture drift problem. In addition, in an underwater environment, when the AUV performs rapid bending motion at a wide viewing angle, the selection of key frames is sparse relative to linear motion scenes, which may lead to key frame posture estimation errors and partial trajectory loss. At present, there are few reports on methods for optimizing key frame postures in such large-view rapid bending motion conditions. Summary of the invention

[0004] Purpose of the invention: In order to solve the problem that SLAM may encounter dynamic environments during long-term underwater operation, the AUV itself moves violently or performs rapid bending movements at a large viewing angle, resulting in posture drift and positioning errors, the present invention proposes an underwater positioning and mapping method based on improved key frame decision and posture optimization.

[0005] The above purpose is achieved through the following technical solutions:

[0006] The underwater positioning and mapping method based on improved key frame decision and posture optimization of the present invention specifically comprises the following steps:

[0007] S1. Obtain data from sensors configured on the AUV;

[0008] S2. Process the sensor data obtained in step S1, initialize the system parameters, track the local map, and determine whether the current environment is dynamic and whether the AUV is moving violently based on the observation information of IMU based on the selection of key frames based on ORB-SLAM3, and then continue to select key frames;

[0009] S3. Optimize the posture of the key frame finally selected in step S2, use double constraints to correct the posture, restore the local trajectory under large-viewing angle rapid bending motion, and generate new map points;

[0010] S4. Perform closed-loop detection and calibration;

[0011] S5. Generate global trajectory and map.

[0012] Furthermore, the sensors configured for the AUV in step S1 include a camera and an IMU, which are used to acquire image frames and IMU data.

[0013] Furthermore, the step S2 specifically includes the following sub-steps:

[0014] S21. Perform feature extraction and matching on the image frame obtained in step S1: use the SIFT algorithm to extract feature points, use the brute force matching algorithm to match the features between frames, and establish feature correspondence;

[0015] S22. Integrate the IMU data obtained in step S1: Assume that the IMU provides acceleration and angular velocity , the camera's pose is updated using integral calculation as follows:

[0016] First, the velocity at time t is obtained by integrating the acceleration , as shown in formula (1):

[0017] (1)

[0018] In the formula, represents the speed at time t-1, Indicates the sampling time interval;

[0019] Next, use the velocity integral to get the position at time t , and update the position, as shown in formula (2):

[0020] (2)

[0021] In the formula represents the position at time t-1;

[0022] Then, the angular velocity integral is used to obtain the attitude at time t , perform posture update, as shown in formula (3):

[0023] (3)

[0024] in, (·) is the exponential mapping of the rotation matrix, represents the posture at time t-1;

[0025] S23. By using the feature points matched in step S21 and the IMU data integrated in step S22, the camera position is optimized by minimizing the reprojection error to achieve local map tracking; wherein the reprojection error The calculation is shown in formula (4):

[0026] (4)

[0027] in, It is The observed positions of feature points on the image; is the projection function of the camera, mapping the three-dimensional points to the two-dimensional image plane; yes The camera rotation matrix at the moment; yes The camera translation vector at the moment; It is The position of feature points in the world coordinate system; Represents the Euclidean norm, which calculates the distance between the actual observation point and the reprojection point;

[0028] S24. Select key frames based on ORB-SLAM3 algorithm: set a threshold E1 to determine the reprojection error of feature points in the current frame Whether it exceeds the threshold E1, if so, it means that the current frame provides new visual information, and it is used as the main key frame to provide visual information;

[0029] S25. Key frame decision based on IMU observation information: The root mean square of the acceleration in the IMU observation data obtained in step S1 , the root mean square of the angular velocity ,in Refers to time, Indicates The sampling time, represents the total number of sampled data points, if or , where T1 is the threshold of the set acceleration root mean square, and T2 is the threshold of the set angular velocity root mean square, it means that the environment is very dynamic or the movement is intense. At this time, a new key frame is selected according to the new threshold E2 and used as a secondary key frame to provide inertial information. E2 is shown in formula (5):

[0030] (5)

[0031] Wherein, m is the set adjustment coefficient.

[0032] Furthermore, the step S3 specifically includes the following sub-steps:

[0033] S31. Establishing a dual measurement constraint between the main key frame in step S24 and the secondary key frame in step S25: First, the main key frame passes the error Establish visual constraints, The calculation is shown in formula (6):

[0034] (6)

[0035] in, and The main keyframes are The rotation matrix and translation vector of

[0036] Then the secondary keyframe describes the motion between the two keyframes through IMU pre-integration to establish inertial constraints. The IMU pre-integration is shown in formula (7):

[0037] ,

[0038] in, is the covariance matrix of the camera pose change, and Secondary keyframes The rotation matrix and translation vector, yes The transposed matrix of and The main keyframes are and minor keyframes The position vector of

[0039] S32. Set the optimization goal and optimize the camera pose by minimizing the objective function. Set the objective function to , where the first refers to visual error, the second Refers to the IMU pre-integration error;

[0040] S33. The sliding mode filter algorithm is used to handle fast and large curvature motion, and the trajectory under curved motion is optimized and restored. First, the camera and IMU system are modeled as a state space model, as shown in equation (8):

[0041] ,

[0042] in, , is the system state vector (including camera pose, velocity, etc.), is the IMU measurement (acceleration, angular velocity), is the observation data (location of visual feature points, etc.), and are process noise and observation noise, respectively. is the state transition matrix, is the control input matrix, is the observation matrix; then design the sliding surface Used to guide the estimated state Converges to the expected value, As shown in formula (9):

[0043] (9)

[0044] in, is the sliding surface design matrix, is the expected value (usually zero); then select the sliding mode control law , so that the estimated state Keep stable on the sliding surface, As shown in formula (10):

[0045] (10)

[0046] in, is the control gain matrix, is a sign function; finally, a sliding mode filter is designed and the sliding mode control law is applied to state update, as shown in formula (11):

[0047] ,

[0048] The sliding mode filter algorithm processes dynamic changes in fast and large curvature motion by optimizing the camera's position and the state of the IMU, recovering local curved motion, thereby improving the overall accuracy and stability of the visual inertial fusion system.

[0049] S34. Generate new map points according to the optimized camera pose.

[0050] Furthermore, the step S4 specifically includes the following sub-steps:

[0051] S41. Perform loop closure detection: match feature points in the current frame and the historical frame, and start loop correction when similar areas are detected;

[0052] S42. Perform loop correction: Use a graph optimization algorithm to globally minimize the entire map and trajectory after detecting the loop to reduce system errors.

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

[0054] The present invention provides an underwater positioning and mapping method based on improved key frame decision and posture optimization, provides a key frame decision method in dynamic or violent camera motion conditions based on IMU observation information, establishes dual measurement constraints between primary and secondary key frames, processes fast and large curvature motion conditions through a sliding mode filtering algorithm, optimizes and restores the motion trajectory at this time, and improves the accuracy and robustness of the visual inertial SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 is an overall block diagram of the present invention;

[0056] Figure 2 Schematic diagram of key frame selection method;

[0057] Figure 3 Schematic diagram of key frame pose optimization method;

[0058] Figure 4 Schematic diagram of underwater feature extraction;

[0059] Figure 5 A schematic diagram of a map constructed according to the present invention. DETAILED DESCRIPTION

[0060] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

[0061] like Figure 1 As shown, the underwater positioning and mapping method based on improved key frame decision and posture optimization of this embodiment includes the following steps:

[0062] S1. Obtain the data of the sensors configured on the AUV, including image frames captured by the camera and IMU data.

[0063] S2. Process the sensor information, initialize the system parameters, track the local map, and determine whether the current environment is dynamic or the camera is moving violently based on the observation information of IMU based on the ORB-SLAM3 key frame selection, and then continue to select the key frame. It includes the following sub-steps:

[0064] S21. Extracting and matching features of the image frames in step S1: extracting feature points using the SIFT algorithm, matching features between frames using a brute force matching algorithm, and establishing feature correspondences;

[0065] S22. Integrate the IMU data in step S1: Assume that the IMU provides acceleration and angular velocity , use the integral to calculate the camera's pose update. First, the velocity is obtained by integrating the acceleration , as shown in formula (1):

[0066] (1)

[0067] Next, we use the velocity integral to get the position , and update the position, as shown in formula (2):

[0068] (2)

[0069] Then, the attitude is obtained by integrating the angular velocity , perform posture update, as shown in formula (3):

[0070] (3)

[0071] in, (·) is the exponential mapping of the rotation matrix;

[0072] S23. By using the feature points matched in step S21 and the IMU data integrated in step S22, the camera position is optimized by minimizing the reprojection error to achieve local map tracking; wherein the reprojection error The calculation formula is shown in formula (4):

[0073] (4)

[0074] in, It is The observed positions of feature points on the image; is the projection function of the camera, mapping the three-dimensional points to the two-dimensional image plane; It's time The camera rotation matrix; It's time The camera translation vector of It is The position of feature points in the world coordinate system; Represents the Euclidean norm, which calculates the distance between the actual observation point and the reprojection point;

[0075] S24. Select key frames, such as Figure 2 As shown, first, the key frame is selected based on the ORB-SLAM3 algorithm: the threshold E1 is set to 3, and the reprojection error of the feature points in the current frame is determined. Does it exceed the threshold E1? If it does, it means that the current frame provides new visual information, and it is used as the main key frame, which mainly provides visual information. Then, the key frame decision is made based on the IMU observation information: Using IMU observation information to assist SLAM key frame selection can enhance the system's perception of the motion state, improve the accuracy of key frame selection and the robustness of the system. When the IMU observation data indicates that the current environment is dynamic or the movement is intense, increase the frequency of key frame selection. The root mean square of the acceleration in the IMU observation data obtained in step S1 , the root mean square of the angular velocity ,if or (in Refers to time, Indicates The sampling time, Indicates the total number of sampled data points. T1 is the threshold of the RMS acceleration. Set T1 to 1m / s 2 , T2 is the threshold of the set angular velocity root mean square, T2 is set to 0.5rad / s), it means that the environment is very dynamic or the movement is intense. At this time, a new key frame is selected according to the new threshold E2 and used as a secondary key frame, which mainly provides inertial information. E2 is shown in formula (5):

[0076] (5)

[0077] Among them, m is the adjustment coefficient, and its value range is 0.8-1.

[0078] S3. Optimize the pose of the key frame obtained at the end of step S2, such as Figure 3 As shown in the figure, firstly, double constraints are used to correct the posture, then the local trajectory under large-viewing angle fast bending motion is restored, and finally new map points are generated. The specific steps include the following:

[0079] S31. Establishing the dual measurement constraint between the main key frame and the secondary key frame in step S24: First, the main key frame passes the error Establish visual constraints, The calculation is shown in formula (6):

[0080] (6)

[0081] in, and The main keyframes are The rotation matrix and translation vector of the secondary keyframe are then used to describe the motion between the two keyframes through IMU pre-integration to establish inertial constraints. IMU pre-integration is shown in formula (7):

[0082] ,

[0083] in, is the covariance matrix of the camera pose change, and Secondary keyframes The rotation matrix and translation vector, yes The transposed matrix of and The main keyframes are and minor keyframes The position vector of

[0084] S32. Set the optimization goal and optimize the camera pose by minimizing the objective function. The objective function can be set to , where the first refers to visual error, the second Refers to the IMU pre-integration error;

[0085] S33. The sliding mode filter algorithm is used to handle fast and large curvature motion, and the trajectory under curved motion is optimized and restored. First, the camera and IMU system are modeled as a state space model, as shown in equation (8):

[0086] ,

[0087] in, , is the system state vector (including camera pose, velocity, etc.), is the IMU measurement (acceleration, angular velocity), is the observation data (location of visual feature points, etc.), and are process noise and observation noise, respectively. is the state transition matrix, is the control input matrix, is the observation matrix; then design the sliding surface Used to guide the estimated state Converges to the expected value, As shown in formula (9):

[0088] (9)

[0089] in, is the sliding surface design matrix, is the expected value (usually zero); then select the sliding mode control law , so that the estimated state Keep stable on the sliding surface, As shown in formula (10):

[0090] (10)

[0091] in, is the control gain matrix, is a sign function; finally, a sliding mode filter is designed and the sliding mode control law is applied to state update, as shown in formula (11):

[0092] ,

[0093] The sliding mode filter algorithm processes dynamic changes in fast and large curvature motion by optimizing the camera's position and the state of the IMU, recovering local curved motion, thereby improving the overall accuracy and stability of the visual inertial fusion system.

[0094] S34. Generate new map points according to the optimized camera pose.

[0095] S4. Perform closed-loop detection and correction, including the following sub-steps:

[0096] S41. Perform loop closure detection: match feature points in the current frame and the historical frame, and start loop correction when similar areas are detected;

[0097] S42. Perform loop correction: Use a graph optimization algorithm to globally minimize the entire map and trajectory after detecting the loop to reduce system errors.

[0098] S5. Generate globally consistent trajectories and maps, that is, the trajectory finally generated by the system has no obvious drift or error in the global coordinate system, the camera pose at each moment matches the point in the global map, and the map can correctly represent all 3D points in the scene without being affected by system drift or local errors.

[0099] The experimental simulation environment for underwater positioning and mapping method based on improved key frame decision and posture optimization is: GPUNVIDIA RTX4050, CPU I5-13450HX, Ubuntu 22.04.

[0100] In order to verify the effectiveness of the method of the present invention, the method of the present invention is compared with ORB-SLAM3 and VINS-Mono respectively to compare the absolute trajectory error. The experimental results are shown in Table 1. It can be seen that the absolute trajectory error of the method of the present invention is smaller than that of ORB-SLAM3 and VINS-Mono.

[0101] Table 1 Absolute trajectory errors of ORB-SLAM3, VINS-Mono and the proposed method

[0102] ORB-SLAM3 VINS-Mono Method of the present invention Absolute trajectory error (m) 1.41 2.52 1.05

[0103] It can be seen that the underwater positioning and mapping method based on improved key frame decision and posture optimization in the present invention can significantly improve the accuracy of AUV system positioning and mapping in underwater environment. Figure 4 and Figure 5 ,in Figure 4 Schematic diagram of underwater feature extraction. Figure 5 A schematic diagram of a map constructed according to the present invention.

Claims

1. An underwater positioning and mapping method based on improved keyframe decision making and posture optimization, characterized in that: The following steps are involved: S1. Obtain data from sensors configured on the AUV; S2. Process the sensor data obtained in step S1, initialize the system parameters, and track the local map. Based on the selection of key frames based on ORB-SLAM3, determine whether the current environment is dynamic and whether the AUV is moving violently according to the observation information of IMU, and then continue to select key frames, specifically including: S21. Perform feature extraction and matching on the image frames obtained in step S1: use the SIFT algorithm to extract feature points, use the brute force matching algorithm to match the features between frames, and establish feature correspondence; S22. Integrate the IMU data obtained in step S1; S23. By using the feature points matched in step S21 and the IMU data integrated in step S22, the camera position is optimized by minimizing the reprojection error to achieve local map tracking; wherein the reprojection error e i The calculation is shown in formula (4): and i =‖x i -π(R t X i +t t )‖2 (4) Among them, x i is the observed position of the i-th feature point on the image; π is the projection function of the camera, which maps the three-dimensional point to the two-dimensional image plane; R t is the camera rotation matrix at time t; t t is the camera translation vector at time t; X i is the position of the i-th feature point in the world coordinate system; ‖·‖2 represents the Euclidean norm, which calculates the distance between the actual observation point and the reprojection point; S24. Select key frames based on ORB-SLAM3 algorithm: set a threshold E1 to determine the reprojection error e of the feature points in the current frame i Whether it exceeds the threshold E1, if so, it means that the current frame provides new visual information, and it is used as the main key frame to provide visual information; S25. Make key frame decisions based on IMU observation information: The root mean square of the acceleration in the IMU observation data obtained in step S1 RMS angular velocity Where t refers to time, b refers to the bth sampling time, and N refers to the total number of sampled data points. If RMS1>T1 or RMS2>T2, where T1 is the set acceleration root mean square threshold and T2 is the set angular velocity root mean square threshold, a new key frame is selected according to the new threshold E2 and used as a secondary key frame to provide inertial information. E2 is shown in formula (5): E2=E1-m·(RMS1+RMS2) (5) Among them, m is the set adjustment coefficient; S3. Optimize the posture of the key frame selected at the end of step S2, use dual constraints to correct the posture, restore the local trajectory under the large-viewing angle fast bending motion, and generate new map points, specifically including: S31. Establish a dual measurement constraint between the main key frame in step S24 and the secondary key frame in step S25: First, the main key frame passes the error r i Establish visual constraints, r i The calculation is shown in formula (6): r i =‖x i -π(R k X i +t k )‖2 (6) Among them, R k and t k They are the rotation matrix and translation vector of the main key frame k respectively; Then the secondary keyframe describes the motion between the two keyframes through IMU pre-integration to establish inertial constraints. The IMU pre-integration is shown in formula (7): Among them, C k,k-1 is the covariance matrix of the camera pose change, R k-1 and t k-1 are the rotation matrix and translation vector of sub-keyframe k-1, YesR k-1 The transposed matrix, p k and p k-1 are the position vectors of the main key frame k and the secondary key frame k-1 respectively; S32. Set the optimization goal and optimize the camera pose by minimizing the objective function. Set the objective function to Among them, the first refers to visual error, the second Refers to the IMU pre-integration error; S33. The sliding mode filtering algorithm is used to handle fast and large curvature motion, optimize and restore the trajectory under curved motion. First, the camera and IMU system are modeled as a state space model, as shown in formula (8): Among them, x k 、x k+1 is the system state vector, u k is the IMU measurement, y k is the observed data, ω k and v k are process noise and observation noise, respectively, and F k is the state transfer matrix, B k is the control input matrix, H k is the observation matrix; then design the sliding surface σ k Used to guide the estimated state x k Converges to the expected value, σ k As shown in formula (9): s k =C k x k -d k (9) Among them, C k is the sliding surface design matrix, d k is the expected value; then select the sliding mode control law z k , so that the estimated state x k Keep stable on the sliding surface, z k As shown in formula (10): z k =-K k sgn(σ k ) (10) Among them, K k is the control gain matrix, sgn(·) is the sign function; finally, the sliding mode filter is designed and the sliding mode control law is applied to the state update, as shown in Equation (11): The sliding mode filter algorithm handles dynamic changes in fast and large curvature motion by optimizing the camera's position and the state of the IMU, and restores local curved motion. S34. Generate new map points according to the optimized camera pose; S4. Perform closed-loop detection and correction; S5. Generate global trajectory and map.

2. The underwater positioning and mapping method based on improved keyframe decision and posture optimization according to claim 1, characterized in that: The sensors configured for the AUV in step S1 include a camera and an IMU, which are used to obtain image frames and IMU data.

3. The underwater positioning and mapping method based on improved keyframe decision and posture optimization according to claim 1, characterized in that: The specific method of step S22 is: Assuming that the IMU provides acceleration α(t) and angular velocity ω(t), the camera's posture update is calculated by integration as follows: First, the velocity v(t) at time t is obtained by integrating the acceleration, as shown in formula (1): v(t)=v(t-1)+a(t)Δt (1) In the formula, v(t-1) represents the velocity at time t-1, and Δt represents the sampling time interval; Next, the velocity integral is used to obtain the position p(t) at time t, and the position is updated, as shown in formula (2): p(t)=p(t-1)+v(t)Δt (2) Where p(t-1) represents the position at time t-1; Then, the angular velocity integral is used to obtain the posture Z(t) at time t, and the posture is updated, as shown in formula (3): Z(t)=Z(t-1)·exp(ω(t)Δt) (3) Among them, exp(·) is the exponential mapping of the rotation matrix, and Z(t-1) represents the posture at time t-1.

4. The underwater positioning and mapping method based on improved keyframe decision and posture optimization according to claim 1, characterized in that: The step S4 specifically includes the following sub-steps: S41. Perform loop closure detection: match feature points in the current frame and the historical frame, and start loop correction when similar areas are detected; S42. Perform loop correction: Use a graph optimization algorithm to globally minimize the entire map and trajectory after detecting the loop to reduce system errors.

Images