A robot online localization and mapping method for underwater vision-degraded scenes

Abstract

The present invention discloses a robot online positioning and mapping method for underwater visual degradation scenes, the method comprising online external parameter calibration; initialization using camera image data, if failed, initialization using sonar image data; reading imaging sonar, binocular camera image data, obtaining matching sonar image feature points, camera image feature points and judging whether visual degradation occurs, if it occurs, using sonar image feature points to perform sonar pose estimation to optimize the pose obtained by IMU propagation estimation; reading IMU sensor data and performing IMU pre-integration, using pre-integrated quantities to perform IMU propagation estimation, and obtaining the pose of the robot's IMU at the current moment relative to the world coordinate system; optimizing the robot pose based on the bundle adjustment method, and mapping. The present invention improves the underwater robot positioning and mapping capabilities in visual degradation scenes more accurately and robustly by fusing binocular camera, IMU, and imaging sonar data.

Description

Technical Field

[0001] The present invention belongs to the technical field of online positioning and mapping of underwater robots, and in particular relates to a robot online positioning and mapping method for underwater vision-degraded scenes. Background Art

[0002] Underwater online positioning and mapping technology plays an indispensable role in various underwater missions, such as environmental monitoring, marine infrastructure inspection, marine exploration, and search and rescue operations. It provides underwater vehicles with accurate pose estimation and detailed maps of the surrounding environment. However, in underwater environments, robot positioning and mapping face unique challenges that are fundamentally different from current ground or aerial scenes, such as the inability to use GPS navigation, rapid illumination changes caused by light attenuation, and the lack of structure and features in open water.

[0003] Although cameras can provide rich information, making them key visual positioning and mapping sensors, unfortunately, camera sensors lack robustness in underwater visually degraded environments. For example, in turbid environments, image quality will be severely degraded; in open waters without structure or texture, visual features will be weakened, which will seriously reduce the accuracy and robustness of robot positioning and mapping. On the other hand, compared with camera sensors, acoustic sensors such as sonar are not easily affected by underwater visually degraded scenes, so they have ideal perception capabilities in dark underwater environments. However, it is difficult to estimate the robot's 6-DOF posture using only sonar sensors for robot positioning, and it is difficult for sonar to build a high-resolution 3D map compared to cameras. Therefore, robot positioning and mapping of underwater scenes remains a very challenging task.

[0004] Patent CN 114485613 A discloses a multi-information fusion underwater robot positioning method. The method proposes to fuse a binocular camera, a heading reference system that can output three-dimensional magnetic field strength, attitude angle, angular velocity and acceleration, and a depth meter that can measure the depth of the underwater robot to improve its positioning accuracy, making up for the shortcomings of single visual positioning that is sensitive to lighting changes and has poor effect in weak texture areas. This method improves its positioning accuracy by fusing the heading reference system and the depth meter, but it cannot build a map in scenes with visual degradation. At the same time, the magnetic field strength, attitude angle, and angular velocity will introduce cumulative drift to the system through long-term integration.

[0005] Patent CN 115272217 A discloses a method for positioning an underwater robot based on sonar images. The method proposes to use a single multi-beam sonar to detect underwater space, and to solve the sonar pose by processing the image and extracting line segments in the image. The experimental accuracy can reach within 20 cm in a specific rectangular underwater space. This solution using a single sonar sensor for positioning is difficult to perform 6-DOF pose estimation using sonar, and it is difficult to build a 3D map, because the sonar acquires 2D images and it is very challenging to restore the sonar lift angle.

[0006] Patent CN 116699618 A discloses an underwater sonar positioning method. The method proposes to set up multiple hydrophones on the hull carrying the submerged tube to collect sound wave signals, calibrate the relative position relationship between each hydrophone and the hull, use the sonar positioning system to locate the submerged tube underwater, record the initial sampling time value and position coordinates of each hydrophone, and estimate the initial value of the position coordinates of the sound source to preliminarily estimate the sound source's sound time; establish a measurement equation between the sound source and all hydrophones and perform iterative solution to obtain the optimal solution value of the sound source position coordinates, and then obtain the real-time position coordinates of the submerged tube. This underwater positioning method uses hydrophones to measure the distance and direction from point to point, and can only solve the relative posture coordinates but not the posture itself. At the same time, the use of multiple hydrophones greatly increases the cost.

[0007] Authorized patent CN 117310773 B discloses an autonomous positioning method and system for underwater robots based on binocular stereo vision. When the lighting is poor, this method uses an unscented Kalman filter algorithm to fuse the binocular stereo vision measurement module, torque mechanics measurement module, Beidou navigation module data and BCF-PPSO-ESN neural network prediction to achieve rapid and high-precision positioning of underwater robots when feature points are insufficient. The Beidou navigation module used in this method has unstable signals underwater. As the underwater depth increases, the signal is more affected and the help for positioning is less. At the same time, the fusion of the torque mechanics measurement module and the Beidou navigation module cannot help with mapping in visually degraded scenes.

[0008] Patent CN 117346795 A discloses a combined navigation method for a submersible that integrates inertial navigation, Doppler odometer and long baseline. This method combines the measurement matrix of three-dimensional position measurement composed of inertial navigation and long baseline with the measurement matrix of three-dimensional velocity measurement composed of inertial navigation and Doppler odometer, thereby improving the robustness and accuracy of positioning. Although the long baseline sensor used in this method can directly provide relative coordinate position, it cannot estimate the robot's own posture. At the same time, the long baseline sensor is expensive and cannot perceive the environment in a visually degraded scene.

[0009] Patent CN 117760409 A discloses an underwater visual inertial positioning method, system, device and medium. The method proposes to pre-process underwater images, and pre-integrate the IMU data between two adjacent frames of underwater images, and then adaptively estimate and feature match the pre-processed underwater images to obtain the adaptive threshold of the visual weight factor and the initial pose information of the camera, and align the initial pose information with the IMU pre-integration to obtain the visual inertial initialization information, obtain the visual reprojection error according to the visual inertial initialization information, and construct the visual dynamic weight factor using the visual reprojection error and the adaptive threshold of the visual weight factor. The residual equation is constructed according to the visual dynamic weight factor, the visual reprojection error and the IMU residual, and the residual equation is solved to obtain the pose information, give full play to the coupling information of vision and inertia, and improve the underwater positioning accuracy. The method improves the accuracy of visual-IMU fusion positioning by the method of adaptive visual weight factor. Even if the weight of the visual reprojection error can be reduced under visual degradation, due to IMU noise and drift, over time, relying solely on IMU for pose estimation will introduce more and more cumulative errors. Summary of the invention

[0010] The main purpose of the present invention is to overcome the shortcomings and deficiencies of the prior art in underwater vision-degraded scenes, in which the IMU cannot be used for long-term dead reckoning due to the lack of visual features, making it impossible for underwater robots to accurately locate and build maps, and to provide a robot online positioning and mapping method for underwater vision-degraded scenes.

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

[0012] One aspect of the present invention provides a robot online positioning and mapping method for underwater visually degraded scenes, comprising the following steps:

[0013] Perform online external parameter calibration of binocular camera and imaging sonar;

[0014] The binocular camera and imaging sonar collect image data, and use the camera image data to initialize the robot's position and three-dimensional map; if the initialization fails, the sonar image data is used for initialization;

[0015] Read the real-time image data of the imaging sonar and process it using median filtering and Gaussian filtering, extract sonar image feature points for matching, and remove external points to obtain matching sonar image feature points;

[0016] Read IMU sensor data and perform IMU pre-integration, and use the pre-integration quantity to estimate IMU propagation;

[0017] Read the real-time image data of the binocular camera, extract the camera image feature points and match them, obtain the matched camera image feature points and determine whether visual degradation occurs; if visual degradation occurs, use the matched sonar image feature points to solve the sonar pose transformation and estimate the robot pose based on the sonar data, and use the sonar pose transformation to optimize the IMU propagation estimate to obtain the IMU pose of the robot in the next frame relative to the world coordinate system;

[0018] Read the matching camera image feature points, the IMU pose of the robot in the current frame relative to the world coordinate system, and the matching sonar image feature points, optimize using the bundle adjustment method, solve the optimized robot pose, and use the sliding window to maintain the key frames of the window number;

[0019] Read the optimized robot posture, and project the matching camera image feature points and matching sonar image feature points into three-dimensional space for mapping;

[0020] The key frame of the sliding window is read to determine whether a loop occurs. If a loop occurs, the optimization step using the bundle adjustment method is re-executed.

[0021] As a preferred technical solution, the online external parameter calibration of the binocular camera and the imaging sonar is specifically as follows:

[0022] The binocular camera and the imaging sonar respectively collect images of a calibration plate placed in an underwater environment in real time, and a plurality of round copper columns are arranged on the surface of the calibration plate;

[0023] Hough transform is used to identify the copper cylindrical target in the images collected by the binocular camera and imaging sonar, and the coordinates of the target point on the camera image are obtained as follows: , the coordinates on the sonar image are ,in j is the sequence number of the target point;

[0024] The target point of the camera image Use binocular stereoscopic projection to restore to 3D point in three-dimensional space , the target point of the sonar image Project to three-dimensional space to get the 3D point of the target ;

[0025] Set space distance e ,according to Perform data association for each target point and use Solving the extrinsic parameters of imaging sonar relative to the stereo camera T CS ;

[0026] Using the solved external parameters T CSThe 3D point in the camera coordinate system Transform to sonar coordinate system to get ,calculate and The average distance , n 1 is the total number of 3D points, if dis < m , which means after transformation to the sonar coordinate system and target points of sonar images If the image is close to the original one, the calibration is successful. Otherwise, re-acquire the image at another location and perform the same calibration steps. m is a constant.

[0027] As a preferred technical solution, the use of camera image data to initialize the robot's posture and three-dimensional map is specifically as follows:

[0028] The binocular camera and imaging sonar collect images and determine whether it is the first frame. If it is the first frame, it is set as a key frame with a sequence number of k =1,2,…, K , K is the total number of key frames, and the key frame feature points of the camera image are extracted And sonar image key frame feature points ;

[0029] Extract camera image feature points from images captured by the binocular camera in real time ,in, m Indicates the left and right camera numbers of the binocular camera. n Indicates the serial number of the camera image feature point, and is related to the camera image key frame feature point Matching is performed. If the number of matching camera image feature points is less than the set value d , it is judged that the visual features are insufficient, visual degradation occurs, and initialization fails;

[0030] Camera image feature points and camera image keyframe feature points The matching steps are as follows: create a camera descriptor for each camera image feature point, set the camera descriptor distance c , filter and retain descriptors with distance less than c The camera image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching camera image feature points.

[0031] If the number of matched camera image feature points is greater than the set value d , then the matched camera image feature points are projected into three-dimensional space through binocular stereo to obtain two sets of three-dimensional space points and , according to the pose solution principle , using two sets of 3D space points to solve the pose transformation , and decompose the rotation matrix and translation matrix ;

[0032] Determine whether the solved posture transformation has rotated or translated. If so, the initialization is determined to be successful, and the camera image feature points in the three-dimensional space are used as landmarks to establish a map; if not, return to the step of collecting the binocular camera image.

[0033] As a preferred technical solution, when initialization fails, sonar image data is used for initialization, specifically:

[0034] Extract sonar image feature points from images collected by imaging sonar in real time , and the sonar image key frame feature points Make a match;

[0035] Sonar image feature points and sonar image key frame feature points The matching steps are as follows: create a sonar descriptor for each sonar image feature point, set the sonar descriptor distance or , filter and retain the description distance less than or The sonar image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching sonar image feature points.

[0036] The matched sonar image feature points are projected into three-dimensional space using the sonar imaging principle to obtain and and through Calculate the current frame i Relative to the k Sonar pose transformation of keyframes ;

[0037] Determine the sonar pose transformation to be solved Whether rotation or translation occurs, if so, the system is determined to be initialized successfully, and the sonar image feature points in the three-dimensional space are used as landmarks to establish a map; if not, return to the step of collecting sonar images.

[0038] As a preferred technical solution, the real-time image data of the imaging sonar is read and processed using median filtering and Gaussian filtering, sonar image feature points are extracted for matching, and external points are removed to obtain matching sonar image feature points, specifically:

[0039] The sonar image is preprocessed by using median filtering combined with Gaussian filtering.

[0040] Extracting feature points of sonar images using AKAZE algorithm , and create a sonar descriptor for each sonar image feature point, and set the sonar descriptor distance or , filter and retain the description distance less than or The sonar image feature points are selected and the RANSAC algorithm is used to further remove the external points to obtain the matched sonar image feature points.

[0041] As a preferred technical solution, the IMU sensor data is read and IMU pre-integration is performed, and the IMU propagation estimation is performed using the pre-integration amount, specifically:

[0042] ;

[0043] ;

[0044] ;

[0045] Constructing IMU error terms Used for pose optimization, as follows:

[0046] ;

[0047] ;

[0048] ;

[0049] ;

[0050] ;

[0051] in Indicates i Rotation from the IMU frame to the world frame, subscript i and i +1 indicates the frame number. Indicates i The rotation from the world coordinate system to the IMU coordinate system, Indicates i Translation of the frame IMU coordinate system to the world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i+1 frame of pre-integration, b a , b g Indicates the bias of the acceleration and gyroscope.

[0052] As a preferred technical solution, the real-time image data of the binocular camera is read, the camera image feature points are extracted and matched, the matched camera image feature points are obtained and it is determined whether visual degradation occurs, specifically:

[0053] Determine whether the real-time image data of the binocular camera is the first frame image. If it is the first frame image, set it as a key frame with the sequence number k =1,2,…, K , K is the total number of key frames, and the key frame feature points of the camera image are extracted ,in, m Indicates the left and right camera numbers of the binocular camera. n Indicates the serial number of the camera image feature point;

[0054] Using the Brisk algorithm to extract camera image feature points from real-time image data of a binocular camera , and the camera image key frame feature points Make a match;

[0055] Camera image feature points and camera image keyframe feature points The matching steps are as follows: create a camera descriptor for each camera image feature point, set the camera descriptor distance c , filter and retain descriptors with distance less than c The camera image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching camera image feature points.

[0056] If the number of matched camera image feature points is less than the set value d , it is judged that the visual features are insufficient and visual degradation occurs.

[0057] As a preferred technical solution, the sonar pose transformation is solved by using the matched sonar image feature points and the robot pose based on the sonar data is estimated. At the same time, the pose of the robot's IMU relative to the world coordinate system in the next frame obtained by optimizing the IMU propagation estimation by using the sonar pose transformation is specifically as follows:

[0058] Using the current frame i Relative to the k Sonar pose transformation of keyframes , estimate the robot pose based on sonar data for the current frame ,T bs Represents the external reference between sonar and IMU, Indicates k The robot pose of the frame keyframe;

[0059] Decomposing robot pose based on sonar data Get the translation and the amount of rotation , and replace the robot pose during IMU propagation ;

[0060] Update IMU error term , as follows:

[0061] ;

[0062] ;

[0063] ;

[0064] ;

[0065] ;

[0066] Subscript i and i +1 indicates the frame number. represents the first i The rotation from the world coordinate system to the IMU coordinate system, represents the first i Translation of the frame world coordinate system to the IMU coordinate system, Indicates i +1 frame translation from IMU coordinate system to world coordinate system, Indicates i +1 frame rotation from IMU coordinate system to world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of the acceleration and gyroscope.

[0067] As a preferred technical solution, the reading of the matched camera image feature points, the IMU posture of the robot in the current frame relative to the world coordinate system, and the matched sonar image feature points are optimized using the bundle adjustment method to solve the optimized robot posture, specifically:

[0068] According to the projection principle Solving for sonar feature points The corresponding three-dimensional space point , and use the current frame i and k Sonar 3D space points of keyframes and Solving the pose transformation between two sonar frames , specifically: ,in, r Indicates the distance between the three-dimensional sonar point and the sonar receiving array;

[0069] Construct the camera reprojection error term: , are camera image feature points, m Indicates the left and right camera numbers of the binocular camera. n Indicates the sequence number of the camera image feature point, express The corresponding three-dimensional space point;

[0070] Constructing IMU error terms , as follows:

[0071] ;

[0072] ;

[0073] ;

[0074] ;

[0075] ;

[0076] in Indicates i Rotation of the frame IMU coordinate system to the world coordinate system, subscript i and i +1 indicates the frame number. Indicates i The rotation from the world coordinate system to the IMU coordinate system, Indicates i Translation of the frame IMU coordinate system to the world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system,g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of acceleration and gyroscope;

[0077] Construct the sonar reprojection error term: ,in , Respectively represent k Frame keyframe and i The robot pose of the frame, T bs Represents the external reference between sonar and IMU;

[0078] Construct an optimization function based on the above error terms J ( X ), and use the bundle adjustment method to optimize the robot posture of the current frame ,in α is a weight factor, which is used as an empirical value to adjust the weight of the sonar reprojection error. Specifically:

[0079] ;

[0080] in, Indicates k Frame keyframe n’ Sonar reprojection error term for sonar image feature points, Indicates k The information matrix of the sonar reprojection error terms in the frame keyframe, K is the total number of key frames, T represents the transpose of a matrix, Indicates k The IMU error term of the frame keyframe, Indicates k The information matrix of the IMU error term of the frame keyframe, Indicates k Frame keyframe n The camera reprojection error term for the camera image feature points, Indicates k The information matrix of the camera reprojection error terms of the frame keyframe;

[0081] Using the current robot position Camera image feature points , sonar image feature points Project into three-dimensional space and construct a point cloud map.

[0082] As a preferred technical solution, the key frame of the sliding window is read to determine whether a loop occurs, specifically:

[0083] If all key frame feature points in the sliding window match the stored key frame feature points successfully, it is determined that a loop has occurred.

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

[0085] (1) The present invention proposes a method for online calibration of cameras and sonars, which realizes plug-and-play of sensors without the need for time-consuming and labor-intensive sensor external parameter calibration process, simplifies the operation process after robot installation, and improves operation efficiency;

[0086] (2) The fusion imaging sonar of the present invention can sense the environment in visually degraded scenes, improve the positioning accuracy of the robot, and use sonar for mapping;

[0087] (3) The present invention proposes to use imaging sonar to provide a better prior for IMU pose estimation in visually degraded scenarios, so as to obtain higher robot positioning accuracy;

[0088] (4) The present invention proposes to use imaging sonar to initialize the robot's posture and map under vision degradation conditions to achieve more robust and accurate positioning and mapping capabilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 is a flow chart of a robot online positioning and mapping method for underwater vision-degraded scenes according to an embodiment of the present invention;

[0090] Figure 2 is a schematic diagram of an underwater sensing device deployed in an embodiment of the present invention;

[0091] Figure 3 is a flow chart of a binocular camera-imaging sonar online calibration algorithm proposed in an embodiment of the present invention;

[0092] Figure 4 is a specific algorithm flow chart of initializing the system using imaging sonar proposed in an embodiment of the present invention;

[0093] Figure 5 is a flowchart of a specific algorithm for IMU propagation in a visually degraded scenario proposed in an embodiment of the present invention;

[0094] Explanation of the figure numbers: 1. Binocular camera; 2. IMU sensor; 3. Main control platform; 4. Imaging sonar; 5. Sealed waterproof housing; 6. Underwater obstacles / landmarks; 7. Binocular camera field of view; 8. Sensing range of imaging sonar. DETAILED DESCRIPTION

[0095] In order 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 in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present application.

[0096] Example

[0097] like Figure 1 As shown, this embodiment provides a robot online positioning and mapping method for underwater visual degradation scenes, which is deployed in an underwater sensing device, such as Figure 2 As shown, it includes a binocular camera 1, an IMU sensor 2, a main control platform 3, and an imaging sonar 4; the binocular camera 1, the IMU sensor 2, and the main control platform 3 are placed in a sealed waterproof housing 5; when the system is running, the binocular camera 1 captures the data of the environment in real time at a rate of 15Hz. When the underwater obstacle / road sign 6 is within the binocular camera field of view 7, it can be captured by the binocular camera 1 and transmitted to the main control platform 3. At the same time, the imaging sonar 4 perceives the environment in real time at a rate of 15Hz. When a landmark point appears within the sensing range 8 of the imaging sonar, the imaging sonar 4 can also capture and transmit it to the main control platform 3. In addition, the IMU sensor 2 measures the robot posture at a rate of 100Hz and transmits the data to the main control platform 3. After the main control platform 3 obtains the data of the binocular camera 1, the IMU sensor 2, and the imaging sonar 4, the following steps are performed.

[0098] Step 1: First, determine whether the external parameter calibration of the binocular camera 1 and the imaging sonar 4 has been performed. If the calibration parameters cannot be obtained, perform online external parameter calibration of the binocular camera 1 and the imaging sonar 4 first.

[0099] Furthermore, in step 1 of this embodiment, the online external parameter calibration of the binocular camera 1 and the imaging sonar 4 is performed, such as Figure 3 As shown, specifically:

[0100] The binocular camera 1 and the imaging sonar 4 respectively collect images of the aprilgrid calibration plate placed in the underwater environment in real time; the aprilgrid calibration plate placed in the underwater environment is divided into two parts, including a 30*30 cm aprilgrid calibration plate, and 24 copper pillars with a diameter of 10 mm and a height of 10 mm are arranged on the surface of the calibration plate to enhance the reflection signal of the sonar, so that the copper pillars can be better identified on the sonar image;

[0101] Hough transform is used to identify the copper cylindrical target in the images collected by the binocular camera 1 and the imaging sonar 4, and the coordinates of the target point on the camera image are obtained as follows: , the coordinates on the sonar image are ,in j is the sequence number of the target point;

[0102] The target point of the camera image Use binocular stereoscopic projection to restore to 3D point in three-dimensional space , the target point of the sonar image Project to three-dimensional space to get the 3D point of the target ;

[0103] Set space distance e ,according to Perform data association for each target point and use Solve the external parameters of imaging sonar 4 relative to binocular camera 1 T CS ;

[0104] In order to verify whether the solved external parameters of the binocular camera 1 and the imaging sonar 4 are correct, the solved external parameters are used. T CS The 3D point in the camera coordinate system Transform to sonar coordinate system to get ,calculate and The average distance , n 1 is the total number of 3D points, if dis < m , which means after transformation to the sonar coordinate system and target points of sonar images If the image is close to the original one, the calibration is successful. Otherwise, re-acquire the image at another location and perform the same calibration steps. m is a constant.

[0105] Step 2: If the sensor calibration parameters are successfully obtained, the system is initialized. If the system initialization fails, the sonar image data is used for initialization. The system initialization includes the initialization of the robot's posture and three-dimensional map.

[0106] Further, in step 2 of this embodiment, if Figure 4 As shown, the camera image data is used to initialize the robot's posture and three-dimensional map, specifically:

[0107] The binocular camera 1 and the imaging sonar 4 collect images and determine whether it is the first frame image. If it is the first frame image, it is set as a key frame with a sequence number of k =1,2,…, K , K is the total number of key frames, and the key frame feature points of the camera image are extracted And sonar image key frame feature points ;

[0108] Extract camera image feature points from the image captured in real time by the binocular camera 1 ,in, m Indicates the left and right camera numbers of binocular camera 1. n Indicates the serial number of the camera image feature point, and is related to the camera image key frame feature point Matching is performed. If the number of matching camera image feature points is less than the set value d , it is judged that the visual features are insufficient, visual degradation occurs, and initialization fails;

[0109] Camera image feature points and camera image keyframe feature points The matching steps are as follows: Create a camera descriptor for each camera image feature point, first set the camera descriptor distance c , initially screen and retain descriptors with a distance greater than c The camera image feature points are then filtered using the RANSAC algorithm to further remove external points to obtain matching camera image feature points.

[0110] If the number of matched camera image feature points is greater than the set value d , then the matched camera image feature points are projected into three-dimensional space through binocular stereo to obtain two sets of three-dimensional space points and ,in n Indicates the sequence number of the camera image feature points, according to the pose solution principle , using two sets of 3D space points to solve the pose transformation , and decompose the rotation matrix and translation matrix ;

[0111] Determine whether the solved posture transformation has rotated or translated. If so, the initialization is determined to be successful, and the camera image feature points in the three-dimensional space are used as landmarks to establish a map; if not, return to the step of collecting the binocular camera image.

[0112] Furthermore, when initialization fails, sonar image data is used for initialization, specifically:

[0113] Extract sonar image feature points from images collected in real time by imaging sonar 4 , and the sonar image key frame feature points Make a match;

[0114] Sonar image feature points and sonar image key frame feature points The matching steps are as follows: create a sonar descriptor for each sonar image feature point, set the sonar descriptor distance or , filter and retain the description distance less than or The sonar image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching sonar image feature points.

[0115] The matched sonar image feature points are projected into three-dimensional space using the sonar imaging principle to obtain and and through Calculate the current frame i Relative to the k Sonar pose transformation of frame keyframe ;

[0116] Determine the sonar pose transformation to be solved Whether rotation or translation occurs, if so, the system is determined to be initialized successfully, and the sonar image feature points in the three-dimensional space are used as landmarks to establish a map; if not, return to the step of collecting sonar images.

[0117] Step 3: After successful initialization, the system creates 6 threads, such as Figure 5As shown in the figure, thread 1 reads the camera image data, uses the Brisk algorithm to extract the camera image feature points and match them, and determines whether visual degradation occurs; thread 2 reads the IMU sensor data and performs IMU pre-integration, and uses the pre-integrated quantity to perform IMU propagation to estimate the robot posture; thread 3 reads the imaging sonar image data, first pre-processes the image using median filtering and Gaussian filtering, then uses the AKAZE algorithm to extract the sonar feature points and match them, and then uses RANSAC to remove the external points again to obtain accurately matched sonar image feature points; thread 4 reads the thread-matched camera image feature points, the robot's current frame IMU posture relative to the world coordinate system, and the matched sonar image feature points, and uses the bundle adjustment method (Bundle Thread 5 reads the optimized robot pose and projects the matched camera image feature points and sonar image feature points into three-dimensional space for mapping. Thread 6 reads the key frames of the sliding window to determine whether a loop occurs. If a loop occurs, thread 4 is re-executed to further optimize the robot pose and global map. In addition, thread 1 also determines whether visual degradation occurs. If visual degradation occurs, thread 3 uses the matched sonar feature points to solve the sonar pose transformation and estimate the robot pose based on the sonar data. At the same time, the sonar pose transformation is used to optimize the IMU pose of the robot in the next frame relative to the world coordinate system obtained by the IMU propagation estimation in thread 2.

[0118] Furthermore, in step 3-thread 1 of this embodiment, the real-time image data of the binocular camera 1 is read, the camera image feature points are extracted and matched, the matched camera image feature points are obtained, and it is determined whether visual degradation occurs, specifically:

[0119] Determine whether the real-time image data of binocular camera 1 is the first frame image. If it is the first frame image, set it as a key frame with a sequence number of k =1,2,…, K , K is the total number of key frames, and the key frame feature points of the camera image are extracted ,in, m Indicates the left and right camera numbers of binocular camera 1. n Indicates the serial number of the camera image feature point;

[0120] Extract the camera image from the real-time image data of the binocular camera 1 using the Brisk algorithm , and the camera image key frame feature points to make a match.

[0121] Camera image feature points and camera image keyframe feature points The matching steps are as follows: create a camera descriptor for each camera image feature point, set the camera descriptor distance c , filter and retain descriptors with distance less than c The camera image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching camera image feature points.

[0122] If the number of matched camera image feature points is less than the set value d , it is judged that the visual features are insufficient and visual degradation occurs.

[0123] Furthermore, in step 3-thread 2 of this embodiment, the data of the IMU sensor 2 is read and the IMU pre-integration is performed, and the IMU propagation estimation is performed using the pre-integration amount to obtain the position and posture of the IMU of the robot in the current frame relative to the world coordinate system, specifically:

[0124] ;

[0125] ;

[0126] ;

[0127] Constructing IMU error terms Used for pose optimization, as follows:

[0128] ;

[0129] ;

[0130] ;

[0131] ;

[0132] ;

[0133] in Indicates i Rotation of the frame IMU coordinate system to the world coordinate system, subscript i and i +1 indicates the frame number. Indicates i The rotation from the world coordinate system to the IMU coordinate system, Indicates i Translation of the frame IMU coordinate system to the world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g wrepresents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of the acceleration and gyroscope.

[0134] Further, in step 3-thread 3 of this embodiment, the real-time image data of the imaging sonar 4 is read and processed using median filtering and Gaussian filtering, sonar image feature points are extracted for matching, and external points are removed to obtain matching sonar image feature points, specifically:

[0135] The sonar image is preprocessed by using median filtering combined with Gaussian filtering.

[0136] Extracting feature points of sonar images using AKAZE algorithm , and create a sonar descriptor for each sonar image feature point, and set the sonar descriptor distance or , filter and retain the description distance less than or The sonar image feature points are selected and the RANSAC algorithm is used to further remove the external points to obtain the matched sonar image feature points.

[0137] Further, in step 3-thread 4 of this embodiment, the matched camera image feature points, the position and posture of the IMU of the robot's current frame relative to the world coordinate system, and the matched sonar image feature points are read, and the bundle adjustment method is used for optimization to solve the optimized robot posture, specifically:

[0138] According to the projection principle Solving for sonar feature points The corresponding three-dimensional space point , and use the current frame i and k Sonar 3D space points of keyframes and Solving the pose transformation between two sonar frames , specifically: ,in, r Indicates the distance between the three-dimensional sonar point and the sonar receiving array;

[0139] Construct the camera reprojection error term: , are camera image feature points, m Indicates the left and right camera numbers of binocular camera 1. n Indicates the sequence number of the camera image feature point, express The corresponding three-dimensional space point;

[0140] Constructing IMU error terms , as follows:

[0141] ;

[0142] ;

[0143] ;

[0144] ;

[0145] ;

[0146] in Indicates i Rotation from the IMU frame to the world frame, subscript i and i +1 indicates the frame number. Indicates i The rotation from the world coordinate system to the IMU coordinate system, Indicates i Translation of the frame IMU coordinate system to the world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of acceleration and gyroscope;

[0147] Construct the sonar reprojection error term: ,in , Respectively represent k Frame keyframe and i The robot pose of the frame, T bs Represents the external reference between sonar and IMU;

[0148] Construct an optimization function based on the above error terms J ( X ), and use the bundle adjustment method to optimize the robot posture of the current frame ,in α is a weight factor, which is used as an empirical value to adjust the weight of the sonar reprojection error. Specifically:

[0149] ;

[0150] in, Indicates k Frame keyframe n’ Sonar reprojection error term for sonar image feature points, Indicates k The information matrix of the sonar reprojection error terms in the frame keyframe, K is the total number of key frames, T represents the transpose of a matrix, Indicates k The IMU error term of the frame keyframe, Indicates k The information matrix of the IMU error term of the frame keyframe, Indicates k Frame keyframe n The camera reprojection error term for the camera image feature points, Indicates k The information matrix of the camera reprojection error terms of the frame keyframe;

[0151] Using the robot pose of the current frame Camera image feature points , sonar image feature points Project into three-dimensional space and construct a point cloud map.

[0152] Furthermore, in step 3-thread 6 of this embodiment, the key frame of the sliding window is read to determine whether a loop occurs, specifically:

[0153] If all key frame feature points in the sliding window match the stored key frame feature points successfully, it is determined that a loop has occurred.

[0154] Furthermore, in step 3 of this embodiment, the sonar pose is estimated using the matched sonar image feature points, and the sonar pose obtained is used to optimize the pose obtained by IMU propagation estimation, specifically:

[0155] Step 1: Using the current frame i Relative to the k Sonar pose transformation of keyframes , estimate the robot pose based on sonar data for the current frame , T bs Represents the external reference between sonar and IMU, Indicates kThe robot pose of the frame keyframe;

[0156] Step 2: Decomposing the robot pose based on sonar data Get the translation and the amount of rotation , and replace the robot pose during IMU propagation ;

[0157] Step 3: Update IMU error term , as follows:

[0158] ;

[0159] ;

[0160] ;

[0161] ;

[0162] ;

[0163] Subscript i and i +1 indicates the frame number. represents the first i The rotation from the world coordinate system to the IMU coordinate system, represents the first i Translation of the frame world coordinate system to the IMU coordinate system, Indicates i +1 frame translation from IMU coordinate system to world coordinate system, Indicates i +1 frame rotation from IMU coordinate system to world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of the acceleration and gyroscope.

[0164] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0165] 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 equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A robot online positioning and mapping method for underwater visually degraded scenes, characterized in that: The steps include: Perform online external parameter calibration of binocular camera and imaging sonar; The binocular camera and imaging sonar collect image data, and use the camera image data to initialize the robot's position and three-dimensional map; if the initialization fails, the sonar image data is used for initialization; Read the real-time image data of the imaging sonar and process it using median filtering and Gaussian filtering, extract sonar image feature points for matching, and remove external points to obtain matching sonar image feature points; Read IMU sensor data and perform IMU pre-integration, and use the pre-integration quantity to estimate IMU propagation; Read the real-time image data of the binocular camera, extract the camera image feature points and match them, obtain the matched camera image feature points and determine whether visual degradation occurs; If visual degradation occurs, the sonar pose transformation is solved using the matched sonar image feature points and the robot pose based on the sonar data is estimated. At the same time, the sonar pose transformation is used to optimize the IMU propagation estimate to obtain the IMU pose of the robot in the next frame relative to the world coordinate system. Read the matching camera image feature points, the IMU pose of the robot in the current frame relative to the world coordinate system, and the matching sonar image feature points, optimize using the bundle adjustment method, solve the optimized robot pose, and use the sliding window to maintain the key frames of the window number; Read the optimized robot posture, and project the matching camera image feature points and matching sonar image feature points into three-dimensional space for mapping; The key frame of the sliding window is read to determine whether a loop occurs. If a loop occurs, the optimization step using the bundle adjustment method is re-executed.

2. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: The online external parameter calibration of the binocular camera and the imaging sonar is specifically as follows: The binocular camera and the imaging sonar respectively collect images of a calibration plate placed in an underwater environment in real time, and a plurality of round copper columns are arranged on the surface of the calibration plate; Hough transform is used to identify the copper cylindrical target in the images collected by the binocular camera and imaging sonar, and the coordinates of the target point on the camera image are obtained as follows: , the coordinates on the sonar image are ,in j is the sequence number of the target point; The target point of the camera image Use binocular stereoscopic projection to restore to 3D point in three-dimensional space , the target point of the sonar image Project to three-dimensional space to get the 3D point of the target ; Set space distance ε ,according to Perform data association for each target point and use Solving the extrinsic parameters of imaging sonar relative to the binocular camera T CS ; Using the solved external parameters T CS The 3D point in the camera coordinate system Transform to sonar coordinate system to get ,calculate and The average distance , n 1 is the total number of 3D points, if dis < μ , which means after transformation to the sonar coordinate system and target points of sonar images If the image is close to the original one, the calibration is successful. Otherwise, re-acquire the image at another location and perform the same calibration steps. μ is a constant.

3. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: The initialization of the robot posture and three-dimensional map using camera image data is specifically as follows: The binocular camera and imaging sonar collect images and determine whether it is the first frame. If it is the first frame, it is set as a key frame with a sequence number of k =1,2,…, K , K is the total number of key frames, and the key frame feature points of the camera image are extracted And sonar image key frame feature points ; Extract camera image feature points from images captured by the binocular camera in real time ,in, m Indicates the left and right camera numbers of the binocular camera. n Indicates the serial number of the camera image feature point, and is related to the camera image key frame feature point Matching is performed. If the number of matching camera image feature points is less than the set value δ , it is judged that the visual features are insufficient, visual degradation occurs, and initialization fails; Camera image feature points and camera image keyframe feature points The matching steps are as follows: create a camera descriptor for each camera image feature point, set the camera descriptor distance γ , filter and retain descriptors with distance less than γ The camera image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching camera image feature points. If the number of matched camera image feature points is greater than the set value δ , then the matched camera image feature points are projected into three-dimensional space through binocular stereo to obtain two sets of three-dimensional space points and , according to the pose solution principle , using two sets of 3D space points to solve the pose transformation , and decompose the rotation matrix and translation matrix ; Determine whether the solved posture transformation has rotated or translated. If so, the initialization is determined to be successful, and the camera image feature points in the three-dimensional space are used as landmarks to establish a map; if not, return to the step of collecting the binocular camera image.

4. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 3, characterized in that: When initialization fails, the sonar image data is used for initialization, specifically: Extract sonar image feature points from images collected by imaging sonar in real time , and the sonar image key frame feature points Make a match; Sonar image feature points and sonar image key frame feature points The matching steps are as follows: create a sonar descriptor for each sonar image feature point, set the sonar descriptor distance η , filter and retain the description distance less than η The sonar image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching sonar image feature points. The matched sonar image feature points are projected into three-dimensional space using the sonar imaging principle to obtain and and through Calculate the current frame i Relative to the k Sonar pose transformation of frame keyframe ; Determine the sonar pose transformation to be solved Whether rotation or translation occurs, if so, the system is determined to be initialized successfully, and the sonar image feature points in the three-dimensional space are used as landmarks to establish a map; if not, return to the step of collecting sonar images.

5. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: The real-time image data of the imaging sonar is read and processed by using median filtering and Gaussian filtering, sonar image feature points are extracted for matching, and external points are removed to obtain matching sonar image feature points, specifically: The sonar image is preprocessed by using median filtering combined with Gaussian filtering. Extracting feature points of sonar images using AKAZE algorithm , and create a sonar descriptor for each sonar image feature point, and set the sonar descriptor distance η , filter and retain the description distance less than η The sonar image feature points are selected and the RANSAC algorithm is used to further remove the external points to obtain the matched sonar image feature points.

6. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: The IMU sensor data is read and IMU pre-integration is performed, and the IMU propagation estimation is performed using the pre-integration quantity, specifically: ； ； ； Constructing IMU error terms Used for pose optimization, as follows: ； ； ； ； ； in Indicates i Rotation from the IMU frame to the world frame, subscript i and i +1 indicates the frame number. Indicates i The rotation from the world coordinate system to the IMU coordinate system, Indicates i Translation of the frame IMU coordinate system to the world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of the acceleration and gyroscope.

7. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: Read the real-time image data of the binocular camera, extract the camera image feature points and match them, obtain the matched camera image feature points and determine whether visual degradation occurs, specifically: Determine whether the real-time image data of the binocular camera is the first frame image. If it is the first frame image, set it as a key frame with the sequence number k =1,2,…, K , K is the total number of key frames, and the key frame feature points of the camera image are extracted ,in, m Indicates the left and right camera numbers of the binocular camera. n Indicates the serial number of the camera image feature point; Using the Brisk algorithm to extract camera image feature points from real-time image data of a binocular camera , and the camera image key frame feature points Make a match; Camera image feature points and camera image keyframe feature points The matching steps are as follows: create a camera descriptor for each camera image feature point, set the camera descriptor distance γ , filter and retain descriptors with distance less than γ The camera image feature points are further filtered using the RANSAC algorithm to remove external points and obtain matching camera image feature points. If the number of matched camera image feature points is less than the set value δ , it is judged that the visual features are insufficient and visual degradation occurs.

8. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: The method uses the matched sonar image feature points to solve the sonar pose transformation and estimate the robot pose based on the sonar data, and uses the sonar pose transformation to optimize the IMU propagation estimate of the robot's IMU pose relative to the world coordinate system in the next frame, specifically: Using the current frame i Relative to the k Sonar pose transformation of frame keyframe , estimate the robot pose based on sonar data for the current frame , T bs Represents the external reference between sonar and IMU, Indicates k The robot pose of the frame keyframe; Decomposing robot poses based on sonar data Get the translation and the amount of rotation , and replace the robot pose during IMU propagation ; Update IMU error term , as follows: ； ； ； ； ； Subscript i and i +1 indicates the frame number. represents the first i The rotation from the world coordinate system to the IMU coordinate system, represents the first i Translation of the frame world coordinate system to the IMU coordinate system, Indicates i +1 frame translation from IMU coordinate system to world coordinate system, Indicates i +1 frame rotation from IMU coordinate system to world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of the acceleration and gyroscope.

9. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: The reading of the matched camera image feature points, the position and posture of the IMU of the robot's current frame relative to the world coordinate system, and the matched sonar image feature points is optimized using the bundle adjustment method to solve the optimized robot position and posture, specifically: According to the projection principle Solving for sonar feature points The corresponding three-dimensional space point , and use the current frame i and k Sonar 3D space points of keyframes and Solving the pose transformation between two sonar frames , specifically: ,in, r Indicates the distance between the three-dimensional sonar point and the sonar receiving array; Construct the camera reprojection error term: , are camera image feature points, m Indicates the left and right camera numbers of the binocular camera. n Indicates the sequence number of the camera image feature point, express The corresponding three-dimensional space point; Constructing IMU error terms , as follows: ； ； ； ； ； in Indicates i Rotation from the IMU frame to the world frame, subscript i and i +1 indicates the frame number. Indicates i The rotation from the world coordinate system to the IMU coordinate system, Indicates i Translation of the frame IMU coordinate system to the world coordinate system, Indicates i The speed from the IMU coordinate system to the world coordinate system, g w represents the gravitational acceleration in the world coordinate system, , , Indicates that IMU is from i Frame to i +1 frame of pre-integration, b a , b g Indicates the bias of acceleration and gyroscope; Construct the sonar reprojection error term: ,in , Respectively represent k Frame keyframe and i The robot pose of the frame, T bs Represents the external reference between sonar and IMU; Construct an optimization function based on the above error terms J ( X ), and use the bundle adjustment method to optimize the robot posture of the current frame ,in α is a weight factor, which is used as an empirical value to adjust the weight of the sonar reprojection error. Specifically: ； in, Indicates k Frame keyframe n’ Sonar reprojection error term for sonar image feature points, Indicates k The information matrix of the sonar reprojection error terms in the frame keyframe, K is the total number of key frames, T represents the transpose of a matrix, Indicates k The IMU error term of the frame keyframe, Indicates k The information matrix of the IMU error term of the frame keyframe, Indicates k Frame keyframe n The camera reprojection error term for the camera image feature points, Indicates k The information matrix of the camera reprojection error terms of the frame keyframe; Using the current robot position Camera image feature points , sonar image feature points Project into three-dimensional space and construct a point cloud map.

10. The method for online robot positioning and mapping for underwater visually degraded scenes according to claim 1, characterized in that: Read the key frame of the sliding window to determine whether a loop occurs, specifically: If all key frame feature points in the sliding window match the stored key frame feature points successfully, it is determined that a loop has occurred.

Images