A multi-sensor fusion SLAM method for near-shore unmanned surface vessels

Abstract

This invention discloses a multi-sensor fusion-based near-shore unmanned surface vessel (USV) SLAM method. It tightly couples LiDAR inertial odometry (LIO) and visual inertial odometry (VIO), and jointly processes data using semantic information from the water surface and shoreline. The prior distribution of the USV's state is obtained through forward propagation using the inertial measurement unit (IMU). In the LIO, the posterior distribution of the USV's state is obtained using the point-to-surface ICP method, which is then used to update the state and point cloud map in conjunction with the prior distribution. In the VIO, the posterior distribution of the USV's state is obtained using a two-step tracking method for non-water surface LiDAR keypoints and a two-step PnP method based on shoreline information, which is also used to update the state in conjunction with the prior distribution. Finally, the USV's trajectory and a point cloud map of the surrounding environment are obtained. This multi-sensor fusion-based near-shore USV SLAM method does not rely on the grayscale invariance assumption, can operate in highly reflective water environments, is unaffected by water reflections, and improves robustness and accuracy.

Description

Technical Field

[0001] This invention relates to the field of Simultaneous Localization and Mapping (SLAM) technology, specifically to a multi-sensor fusion SLAM method for near-shore unmanned surface vessels. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a technique that uses information acquired by sensors to build a map of the environment while moving, and simultaneously estimates its own motion, without prior environmental information. This technology is currently widely used in fields such as autonomous vehicles, drones, robots, and XR.

[0003] Currently, the two most mainstream SLAM technologies are LiDAR SLAM and visual SLAM. In addition, Inertial Measurement Units (IMUs) are also frequently used; the data collected by the IMU is called IMU data. LiDAR provides precise depth information, cameras provide rich texture and color information, and the IMU provides its own acceleration and angular velocity information. Nowadays, SLAM that fuses information from these three sensors is increasingly being proposed. Multi-sensor SLAM has better robustness and can provide more accurate localization and mapping.

[0004] In fact, SLAM technology also has broad application prospects in the field of near-shore unmanned vessels, including unmanned cleaning vessels, autonomous cruise ships, and unmanned exploration vessels. However, the application of SLAM technology in water surface scenarios encounters some new challenges, including: 1) Strong reflections on the water surface affect camera exposure, making it difficult to satisfy the grayscale invariance assumption in optical flow and direct methods. 2) Image extraction can extract feature points from water reflections, leading to false images in the mapping and affecting the accuracy of feature point matching and tracking. 3) LiDAR can acquire noisy point clouds on the water surface. 4) Rapid ripples on the water surface can cause distortion in the point clouds acquired by LiDAR.

[0005] In summary, current SLAM methods face certain challenges in application to aquatic environments, and their robustness and accuracy need to be improved. Summary of the Invention

[0006] The purpose of this invention is to overcome the difficulties of applying existing technologies in nearshore water environments and to provide a multi-sensor fusion nearshore unmanned surface vessel SLAM method. This method integrates information from three sensors: lidar, camera, and IMU, thereby compensating for the shortcomings of applying these three sensors individually in water environments and enabling robust and high-precision positioning and mapping of unmanned surface vessels in nearshore waters.

[0007] The objective of this invention can be achieved by adopting the following technical solutions:

[0008] A multi-sensor fusion SLAM method for near-shore unmanned surface vessels, the SLAM method comprising the following steps:

[0009] S1. Sensor Data Acquisition: The unmanned surface vessel (USV) moves freely in the near-shore water environment. It acquires point cloud data of the surrounding environment in the lidar coordinate system through lidar, image data of the surrounding environment in the camera coordinate system through camera, and acceleration and angular velocity data of the USV in the IMU coordinate system through inertial measurement unit (IMU). The IMU data is called IMU data. Let i be the index of the IMU data and k be the index of the point cloud data and the image data.

[0010] S2. Initialization: After acquiring the first frame of IMU data, set the IMU coordinate system at that moment as the origin of the map coordinate system, and update the unmanned surface vessel's state vector x using the IMU data of each subsequent frame. The definition of is:

[0011] in,() T This indicates that the elements within the parentheses are transposed. G r I This represents the rotation vector from the IMU coordinate system to the map coordinate system. G p I Indicates the position of the IMU in the map coordinate system, [ I r C , I p C ]and[ I r L , I p L ] represent the external parameters between the IMU and the camera and LiDAR, respectively. G v represents the velocity of the IMU in the map coordinate system, b a and b g These represent the biases of the accelerometer and gyroscope, respectively, and Gg represents the acceleration due to gravity. I t C φ represents the time offset between the camera and the IMU, φ = [f x ,f y ,c x ,c y ] T This represents the camera's intrinsic parameters, (f x ,f y (c) represents the horizontal and vertical focal lengths of the camera. x ,c y ) represents the horizontal and vertical offsets of the image origin relative to the camera's optical center;

[0012] After acquiring the first frame of point cloud data, the distortion of the point cloud in that frame is compensated by backpropagation of the IMU, and the pose estimated by forward propagation of the IMU is used to register the point cloud in the map coordinate system to form the initial map.

[0013] After acquiring the first frame of image data, the water surface portion of the image data is segmented using the Deeplab V3+ image segmentation network, and the image is binarized. The pixel values ​​of the water surface pixels are set to 255, and the pixel values ​​of the non-water surface pixels are set to 0. Then, the shoreline portion of the image is extracted using Canny edge detection. The point cloud registered in the map in the first frame is projected onto this image as key points to be tracked, and the points in the point cloud whose projected points fall on the shoreline are stored in a queue of size M. middle;

[0014] DeepLab V3+ is an image semantic segmentation network proposed by Liang-Chieh Chen in 2018 in "Encoder-Decoder with Atrous Separable Convolution for Semantic Image Segmentation".

[0015] S3. Unmanned Surface Vessel Pose Update: Upon receiving IMU data, x is updated using IMU forward propagation; the pose information is then found in x. G r I , G p I ], and calculate the prior distribution of x; when point cloud data is received, first use IMU backpropagation to compensate for the distortion of the point cloud, and then project the distortion-free point cloud onto the nearest frame image, assuming L P j It is a point in the point cloud after distortion removal of the point cloud data of this frame, where L represents the point in the lidar coordinate system, and j represents the index of the point. Let L be the coordinate system of the lidar. C p j yes L P j At the projection point in the most recent frame image, if C p j The part of the image that falls on the water surface will... C p j and L P j Delete them all; if C p j If the image falls on the non-water surface, then find it on the map. L P j Using the five nearest points, fit a plane α.j ,by L P j To α j The distance is used as the residual to obtain the posterior distribution of x. After processing each point in the point cloud after distortion removal of the frame, x is updated using the prior distribution of x obtained by IMU forward propagation and error state iterative Kalman filtering. When image data is received, the water surface part of the image is segmented using the Deeplab V3+ image segmentation network. After segmentation, the image is binarized, and the pixel values ​​of the water surface pixels are set to 255, while the pixel values ​​of the non-water surface pixels are set to 0. Then, the shoreline part of the image is extracted using Canny edge detection. The non-water surface LiDAR key point two-step tracking method is used to track the key points to be tracked in the previous frame. G P s It is one of the 3D points corresponding to the key points to be tracked in the previous frame, where G represents the point in the map coordinate system and s represents the index of the point. C p s yes G P s The pixels tracked in this frame image, C p s and G P s The reprojection error between points is used as the residual to obtain the posterior distribution of x. After processing each keypoint, the prior distribution of x obtained from IMU forward propagation is used, and x is updated using error state iterative Kalman filtering. The pose information in the updated x is then used to... The points in the image are projected onto the frame image, let... G P m yes One point in the middle, C p m It is in this frame image that is related to G P m The nearest shoreline pixel to the projection point, with C p m and G P m The reprojection error between the two is used as the residual to obtain the posterior distribution of x, after processing. After each point in the map, the prior distribution of x obtained from IMU forward propagation is used to update x using error state iterative Kalman filtering. Finally, the point cloud registered in the map in the most recent frame is projected onto the image of that frame as key points to be tracked, and the points in the point cloud whose projected points fall on the shoreline are saved. middle;

[0016] S4. Map Update: After receiving point cloud data and updating the pose using that frame of point cloud data, the updated pose is used to update the map for each point in that frame of point cloud. LP j Registering to the map coordinate system completes the map update.

[0017] A multi-sensor fusion SLAM method for near-shore unmanned surface vessels uses the IMU as the volume coordinate system to obtain the following continuous kinematic model:

[0018]

[0019]

[0020] Among them, a m and ω m n represents the raw data from the accelerometer and gyroscope, respectively. a and n g b represents white noise in IMU data a and b g Modeled as Gaussian white noise with derivatives respectively and Random walk.

[0021] Furthermore, the IMU forward propagation process in the SLAM method is as follows:

[0022] Define the prior estimate of x as IMU propagates forward with zero-order hold That is, assuming the IMU measurement value is constant within a sampling period and the process noise is set to zero, we have

[0023]

[0024] in, and Let x represent the prior estimates of x for the i-th and i+1-th IMU measurements, respectively, and let Δt represent the time interval between the i-th and i+1-th IMU measurements. i This represents the raw data from the i-th IMU measurement.

[0025] Known The symbol [+] is defined as

[0026]

[0027] Among them, a mi and ω mi Let represent the raw data from the accelerometer and gyroscope respectively during the i-th IMU measurement. Let represent the prior estimate of the rotation vector from the IMU coordinate system to the map coordinate system during the i-th IMU measurement. This represents the prior estimate of the IMU's position in the map coordinate system during the i-th IMU measurement. and Let represent the prior estimates of the extrinsic parameters between the IMU, the camera, and the lidar, respectively, for the i-th IMU measurement. Let represent the prior estimate of the IMU's velocity in the map coordinate system during the i-th IMU measurement. and Let represent the prior estimates of the biases of the accelerometer and gyroscope during the i-th IMU measurement, respectively. Let represent the prior estimate of gravitational acceleration during the i-th IMU measurement. Let represent the prior estimate of the time offset between the camera and the IMU during the i-th IMU measurement. This represents the prior estimate of the camera's intrinsic parameters during the i-th IMU measurement, 0 n×m Let represent the zero matrix of n×m, and Exp() and Log() are the Rodrigues transformations between the rotation vector and the rotation matrix.

[0028] Furthermore, since each point is collected at a different time and the unmanned vessel is constantly moving, the coordinates of each point are acquired in different coordinate systems, meaning the point cloud data is distorted and needs to be corrected. The SLAM method uses IMU backpropagation to compensate for the point cloud distortion, as follows:

[0029] T1, IMU Backpropagation: Suppose the current processing is the (k+1)th frame of point cloud data, the IMU propagates the state vector backward with a zero-order hold. And if the process noise is set to zero, then we have

[0030]

[0031] in, Let x represent the prior estimates of x for the (j-1)th and jth lidar measurements within the point cloud in the (k+1)th frame, respectively. This represents the acceleration and angular velocity of the unmanned vessel during the j-th lidar measurement within the point cloud in the (k+1)-th frame.

[0032] Backpropagation begins by setting the pose to zero, and the velocity and IMU bias are set to zero. The value in This represents the prior estimate of x when the k+1th frame of the point cloud is acquired. In addition, for any point in a frame of the point cloud, the IMU data that is acquired earlier than that point and is closest to that point is taken as the input of the unmanned ship's acceleration and angular velocity during backpropagation at that point.

[0033] T2, Point Cloud Distortion Correction: Backpropagation generates ρ j Time and t k+1 Relative pose between moments This represents a priori estimate of the transformation matrix from the IMU coordinate system during the j-th lidar measurement within the (k+1)-th frame of the point cloud to the IMU coordinate system when the (k+1)-th frame of the point cloud is acquired. express The rotation matrix part, express The translation vector part utilizes relative pose to measure local values. Measurement in the lidar coordinate system at the end of the scan

[0034] in, For t k Maximum a posteriori (MAP) estimation of the extrinsic parameters between the time-of-flight lidar and the IMU, L j L represents the lidar coordinate system during the j-th lidar measurement within the current frame's point cloud. k+1 This represents the lidar coordinate system when the point cloud of frame k+1 is obtained.

[0035] Furthermore, a multi-sensor fusion SLAM method for near-shore unmanned surface vessels considers the lack of feature information from lidar with a small field of view. Therefore, it does not perform feature extraction on the point cloud data, but directly uses the raw point cloud data. Theoretically, each acquired point falls on a small plane in the point cloud map. The SLAM method uses the distance from the point to the plane as the residual, and the specific process is as follows:

[0036] set up To obtain the maximum a posteriori estimate of x using point cloud computing in the (k+1)th frame, the Rodriguez transform is used. Rotation vector in and Convert to rotation matrix and According to the following formula L P j Transformation from LiDAR coordinate system to map coordinate system: in, and These are the maximum a posteriori estimates of the rotation matrix and translation vector of the IMU coordinate system relative to the map coordinate system and the maximum a posteriori estimates of the rotation matrix and translation vector of the LiDAR coordinate system relative to the IMU coordinate system, respectively, when the point cloud of the (k+1)th frame is obtained. In the map... L P j The nearest face α j Having normal vector u j and a point q in a plane j The measurement residual is:

[0037] Furthermore, a multi-sensor fusion-based near-shore unmanned surface vessel (USV) SLAM method employs a two-step tracking method for non-water surface LiDAR keypoints. When selecting keypoints for an image, this method uses the projection points of the LiDAR point cloud on the image from the non-water surface area as keypoints, ensuring accurate depth information and avoiding the use of pixels reflected in the water. During keypoint tracking, the method calculates the keypoint descriptor and combines it with IMU acceleration and angular velocity information to find the matching pixel in the next frame. This method does not rely on the assumption of grayscale invariance and can work better in highly reflective water environments. The process of the non-water surface LiDAR keypoint two-step tracking method in the SLAM method is as follows:

[0038] R1, Calculate descriptors: Calculate the descriptors of the key points to be tracked in the previous frame image;

[0039] R2. Coarse pose estimation: The acceleration and angular velocity information of the IMU are integrated to coarsely estimate the pose of the unmanned ship in the next image frame. Then, the 3D point cloud corresponding to the key points is projected into the next frame image based on the estimated pose to preliminarily predict the position of the key points in the next frame image.

[0040] R3, Fine Tracking of Keypoints: In the next frame of the image, take an N×N square window centered on the initially predicted keypoints, calculate the descriptors of all pixels within the window, and find the point among these pixels whose descriptor is closest to the descriptor calculated in R1 in the vector space. This pixel is the point that has been finally tracked.

[0041] Furthermore, a multi-sensor fusion-based near-shore unmanned surface vessel (USV) SLAM method employs a two-step PnP method based on the shoreline. Theoretically, the projection point of the 3D point to be tracked in the current frame image coincides with the coordinates of the tracked pixel. The SLAM method uses the reprojection error as the residual, and the process is as follows:

[0042] set up To obtain the maximum a posteriori estimate of x using the (k+1)th frame image, the Rodriguez transform is used. Rotation vector in and Convert to rotation matrix and for C p s and their corresponding points in the 3D map G P s The measurement residual is:

[0043]

[0044] in, express G P s In the camera coordinate system, π(·) represents the pinhole projection model in three dimensions. and These are the maximum a posteriori estimates of the rotation matrix and translation vector of the IMU coordinate system relative to the map coordinate system and the maximum a posteriori estimates of the rotation matrix and translation vector of the camera coordinate system relative to the IMU coordinate system, respectively, when the (k+1)th frame image is acquired.

[0045] Similarly, for C p m and their corresponding points in the 3D map G P m The measurement residual is:

[0046]

[0047] in, express G P m Three-dimensional coordinates in the camera coordinate system.

[0048] The present invention has the following advantages and effects compared with the prior art:

[0049] 1. The two-step tracking method for non-water surface LiDAR key points proposed in this invention uses the projection points of the LiDAR point cloud in the image as key points to be tracked, so that the key points have more accurate depth information. Furthermore, by combining IMU forward propagation and feature matching, the tracking process does not rely on the assumption of grayscale invariance, and can work better in highly reflective water surface environments.

[0050] 2. This invention utilizes image segmentation networks and edge detection algorithms to extract shoreline information from images. During the tracking of key points, the shoreline information is transmitted to the LiDAR point cloud, creating new constraints between the point cloud data and image data. This further fuses the information from the LiDAR and camera, improving the robustness and accuracy of the entire SLAM method. Attached Figure Description

[0051] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings:

[0052] Figure 1 This is an overall framework diagram of a near-shore unmanned surface vessel SLAM method with multi-sensor fusion disclosed in this invention;

[0053] Figure 2 This is a flowchart of the steps of a near-shore unmanned surface vessel SLAM method with multi-sensor fusion disclosed in this invention;

[0054] Figure 3 This is a schematic diagram of the index relationship between IMU frames and lidar / vision frames in this invention;

[0055] Figure 4 This is an application block diagram of the nearshore unmanned vessel SLAM method based on multi-sensor fusion in this embodiment of the invention;

[0056] Figure 5 This is a schematic diagram of the coordinate systems of the system components constructed in this embodiment of the invention;

[0057] Figure 6 This is an image showing the effect of water surface segmentation and shoreline extraction in an embodiment of the present invention. Figure 6 (a) is a segmentation Figure 2 The effect of value-based conversion, Figure 6 (b) is a rendering of the shoreline extraction using Canny edge detection. Figure 6 (c) is a rendering of the shoreline drawn on the original drawing;

[0058] Figure 7 This is a rendering of the point cloud map created by the system in this embodiment of the invention, in which the circled area clearly shows the shoreline. Detailed Implementation

[0059] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0060] Example 1

[0061] This embodiment discloses a specific implementation process of a traditional multi-sensor fusion SLAM method, constructing a system consisting of an unmanned surface vessel, a Livox Mid-40 lidar, an MT9V034 camera, a HiPNUC CH100 IMU, and a personal computer. The various components in the system are connected via ROS and other technologies. Figure 5 The coordinate system shown works together.

[0062] The unmanned boat was piloted to move freely in the West Lake of the Wushan Campus of South China University of Technology. The frequency of LiDAR collecting point cloud data was set to 10Hz, the frequency of camera collecting image data was set to 30Hz, and the frequency of IMU collecting IMU data was set to 400Hz. The collected multi-sensor data was published in the form of ROS topics for all nodes of the system to subscribe to and use.

[0063] During the initialization phase, upon acquiring the first frame of IMU data, the state vector x is... G r I and G p I Set it to the zero vector, and at this time set the covariance matrix Σ of the state vector x. x Set as a unit vector; when the first frame of point cloud data is acquired, the distortion of the point cloud in the frame is compensated by IMU backpropagation, and the pose estimated by IMU forward propagation is used to register the point cloud in the map coordinate system to form an initial map; when the first frame of image data is acquired, the point cloud registered in the map in the first frame is projected into the image in the frame as the key points to be tracked.

[0064] After initialization, when IMU data is received, it propagates forward using a zero-order hold. That is, assuming that the IMU measurement value within a sampling period is constant and the process noise is set to zero, and assuming that the current processing is the i-th IMU measurement data, then...

[0065]

[0066] in, and Let x represent the prior estimates of x for the i-th and i+1-th IMU measurements, respectively, and let Δt represent the time interval between the i-th and i+1-th IMU measurements. i This represents the raw data from the i-th IMU measurement. and Let represent the raw data from the accelerometer and gyroscope respectively during the i-th IMU measurement. Let represent the prior estimate of the rotation vector from the IMU coordinate system to the map coordinate system during the i-th IMU measurement. This represents the prior estimate of the IMU's position in the map coordinate system during the i-th IMU measurement. and Let represent the prior estimates of the extrinsic parameters between the IMU, the camera, and the lidar, respectively, for the i-th IMU measurement. Let represent the prior estimate of the IMU's velocity in the map coordinate system during the i-th IMU measurement. and Let represent the prior estimates of the biases of the accelerometer and gyroscope during the i-th IMU measurement, respectively. Let represent the prior estimate of gravitational acceleration during the i-th IMU measurement. Let represent the prior estimate of the time offset between the camera and the IMU during the i-th IMU measurement. This represents the prior estimate of the camera's intrinsic parameters during the i-th IMU measurement, 0 n×mLet represent the zero matrix of n×m, and Exp() and Log() are the Rodrigues transformations between the rotation vector and the rotation matrix.

[0067] The pose information of the unmanned vessel is then propagated forward, i.e., the [in x] G r I , G p I ], and calculate the prior distribution of x.

[0068] When point cloud data is received, the IMU propagates the state vector backward using a zero-order hold. And if the process noise is set to zero, then we have

[0069] Here, assuming the current processing is of the (k+1)th frame of point cloud data, then Let x represent the prior estimates of x for the (j-1)th and jth lidar measurements within the point cloud in the (k+1)th frame, respectively. This represents the acceleration and angular velocity of the unmanned vessel during the j-th lidar measurement within the k-th frame of the point cloud.

[0070] Backpropagation begins by setting the pose to zero, and the velocity and IMU bias are set to zero. The value in This represents the prior estimate of x when the (k+1)th frame of the point cloud is acquired. In addition, for any point in a frame of the point cloud, the IMU data that is acquired earlier than that point and is closest to that point is taken as the input of the unmanned ship's acceleration and angular velocity during backpropagation at that point.

[0071] Back propagation generates ρ j Time IMU coordinate system and t k+1 Relative pose between IMU coordinate systems at any given time This represents a priori estimate of the transformation matrix from the IMU coordinate system during the j-th lidar measurement within the (k+1)-th frame of the point cloud to the IMU coordinate system when the (k+1)-th frame of the point cloud is acquired. express The rotation matrix part, express The translation vector part utilizes relative pose to measure local values. Measurement in the lidar coordinate system at the end of the scan

[0072] in, For t k Maximum a posteriori (MAP) estimation of the extrinsic parameters between the time-of-flight lidar and the IMU, L j L represents the lidar coordinate system during the j-th lidar measurement within the current frame's point cloud. k+1This represents the lidar coordinate system when the point cloud of frame k+1 is obtained.

[0073] set up L P j This is a point in the point cloud after distortion correction of the point cloud data in this frame, where L represents the point in the LiDAR coordinate system, and j represents the index of the point. The point is found in the map... L P j Using the five nearest points, fit a plane α. j α j Having normal vector u j and a point q in a plane j Using the Rodriguez transform to Rotation vector in and Convert to rotation matrix and Maximum a posteriori estimation of the rotation matrix and translation vector of the IMU coordinate system relative to the map coordinate system when the point cloud of frame k+1 is obtained. Maximum a posteriori estimates of the rotation matrix and translation vector of the lidar coordinate system relative to the IMU coordinate system Will L P j Transformation from LiDAR coordinate system to map coordinate system by L P j To α j distance The posterior distribution of x is obtained as the residual. After processing each point in the point cloud after distortion removal of the frame, x is updated by using the error state iterative Kalman filter in conjunction with the prior distribution of x obtained by IMU forward propagation.

[0074] When image data is received, for each key point to be tracked, its descriptor in the previous frame is calculated. The acceleration and angular velocity information of the IMU are integrated to roughly estimate the pose of the unmanned vessel in the current image frame. Then, based on the estimated pose, the 3D point cloud corresponding to the key point is projected into the current frame image to preliminarily predict the position of the key point in the current image. In the current frame image, a 5×5 square window is taken with the preliminarily predicted key point as the center. The descriptors of all pixels in the window are calculated. Among these pixels, the point whose descriptor is closest to the previously calculated descriptor in the vector space is found. This pixel is the finally tracked point.

[0075] Assuming we are currently processing the (k+1)th frame of the image, let... G P s It is one of the 3D points corresponding to the key points to be tracked in the previous frame, where G represents the point in the map coordinate system and s represents the index of the point. C ps yes G P s The pixels tracked in this frame image are then transformed using the Rodriguez transform. Rotation vector in and Convert to rotation matrix and Maximum a posteriori estimation of the rotation matrix and translation vector of the IMU coordinate system relative to the map coordinate system when the (k+1)th frame image is obtained. Maximum a posteriori estimates of the rotation matrix and translation vector of the camera coordinate system relative to the IMU coordinate system Will G P s Transform to the current camera coordinate system. get C p s and G P s Reprojection error between Using this as the residual, the posterior distribution of x is obtained. After processing each key point, the prior distribution of x obtained by forward propagation of IMU is used to update x using error state iterative Kalman filtering. Finally, the point cloud registered in the map in the most recent frame is projected into the image of that frame as the key point to be tracked.

[0076] After receiving point cloud data and updating the pose using that frame of point cloud data, the updated pose is used to update the pose of each point in that frame of point cloud. L P j Registering the map to the map coordinate system completes the map update. The updated map will be subscribed to by RVIZ in real time and displayed in RVIZ.

[0077] In this embodiment, the traditional SLAM method cannot work in a water environment, and the trajectory drift phenomenon is serious, making it impossible to build a map.

[0078] Example 2

[0079] This embodiment discloses a specific implementation process of a near-shore unmanned surface vessel (USV) SLAM method based on multi-sensor fusion. A system was built consisting of an USV, a Livox Mid-40 lidar, an MT9V034 camera, a HiPNUC CH100 IMU, and a personal computer. The proposed SLAM method was deployed in the built system, and the various components of the system were connected via ROS and other technologies. Figure 5 The coordinate system shown works together.

[0080] 1000 images of nearshore environments with water surfaces annotated were used to train the DeeplabV3+ semantic segmentation network. During training, MobileNet V2 was used as the backbone network, with the SGD optimizer and a learning rate of 7E-3. The network was trained for 150 epochs, with the backbone frozen for the first 50 epochs and not frozen for the last 100 epochs. The image resolution before input to the network was set to 512×512. The trained model was converted to ONNX format and deployed to the SLAM method.

[0081] The unmanned boat was piloted to move freely in the West Lake of the Wushan Campus of South China University of Technology, collecting data at the same frequency as in Example 1, and the collected multi-sensor data was published in the form of ROS topics for all nodes of the system to subscribe to and use.

[0082] During the initialization phase, the operations are the same as in Example 1 when acquiring the first frame of IMU data and the first frame of point cloud data. When acquiring the first frame of image data, the water surface portion of the image data is segmented using the trained semantic segmentation network Deeplab V3+, and the image is binarized. The pixel values ​​of the pixels in the water surface portion are set to 255, and the pixel values ​​of the pixels in the non-water surface portion are set to 0. Then, the shoreline portion of the image is extracted using Canny edge detection, with the effect as shown. Figure 6 As shown, the point cloud registered in the map in the first frame is projected onto this frame image as key points to be tracked, and the points in the point cloud whose projected points fall on the shoreline are stored in a queue. In this case, the queue size is set to 20.

[0083] After initialization, when IMU data is received, the operation is the same as in Example 1. When point cloud data is received, the point cloud distortion correction operation is the same as in Example 1, projecting the distortion-corrected point cloud onto the most recent frame image. L P j It is a point in the point cloud after distortion removal of the point cloud data of this frame, where L represents the point in the lidar coordinate system, and j represents the index of the point. Let L be the coordinate system of the lidar. C p j yes L P j At the projection point in the most recent frame image, if C p j The part of the image that falls on the water surface will... C p j and L P j Delete them all; if C p jFor the non-water surface portion of the image, the posterior distribution of x is obtained through the same operation as in Example 1. After processing each non-water surface point in the point cloud after distortion removal of the frame, x is updated using the error state iterative Kalman filter in conjunction with the prior distribution of x obtained by IMU forward propagation.

[0084] Upon receiving image data, the water surface portion of the frame is segmented using the trained semantic segmentation network Deeplab V3+. After segmentation, the frame is binarized, setting the pixel values ​​of pixels in the water surface to 255 and the pixel values ​​of pixels outside the water surface to 0. Then, Canny edge detection is used to extract the shoreline portion of the frame. Each keypoint to be tracked is traced using the same operations as in Example 1.

[0085] Assuming we are currently processing the (k+1)th frame of the image, we first update x using the same operation as in Example 1. Let... G P m yes One point in the middle, C p m It is in this frame image that is related to G P m The nearest shoreline pixel to the projection point, using the updated and Will G P m Transform to the current camera coordinate system. Obtained using the projection function π() C P m The projection point in the current frame image is then found using the BFS algorithm. C p m ,get C p m and G P m Reprojection error between Using this as the residual, the posterior distribution of x is obtained, and then processed. After each point in the map, the prior distribution of x obtained from IMU forward propagation is used to update x using error state iterative Kalman filtering. Finally, the point cloud registered in the map in the most recent frame is projected onto the image of that frame as key points to be tracked, and the points in the point cloud whose projected points fall on the shoreline are saved. middle.

[0086] The point cloud map update process is the same as in Example 1. The updated map will be subscribed to by RVIZ in real time and displayed in RVIZ, as shown in the image. Figure 7 As shown.

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

Claims

1. A multi-sensor fusion based SLAM method for near shore unmanned vessel, characterized in that, The SLAM method includes the following steps: S1. Sensor Data Acquisition: The unmanned surface vessel (USV) moves freely in the near-shore water environment. It acquires point cloud data of the surrounding environment in the lidar coordinate system through lidar, image data of the surrounding environment in the camera coordinate system through camera, and acceleration and angular velocity data of the USV in the IMU coordinate system through inertial measurement unit (IMU). The IMU data is called IMU data. Let i be the index of the IMU data and k be the index of the point cloud data and the image data. S2. Initialization: After acquiring the first frame of IMU data, set the IMU coordinate system at the time of data acquisition as the origin of the map coordinate system, and update the unmanned surface vessel's state vector using the IMU data of each frame. , The definition of is: , in,( ) T This indicates that the elements within the parentheses are transposed. This represents the rotation vector from the IMU coordinate system to the map coordinate system. This indicates the position of the IMU in the map coordinate system. and These represent the external parameters between the IMU and the camera and LiDAR, respectively. This indicates the velocity of the IMU in the map coordinate system. and These represent the biases of the accelerometer and gyroscope, respectively. Represents gravitational acceleration. Indicates the time offset between the camera and the IMU. This refers to the camera's internal parameters. For the camera's horizontal and vertical focal lengths, The horizontal and vertical offsets of the image origin relative to the camera optical center; After acquiring the first frame of point cloud data, the distortion of the point cloud in that frame is compensated by backpropagation of the IMU, and the pose estimated by forward propagation of the IMU is used to register the point cloud in the map coordinate system to form the initial map. After acquiring the first frame of image data, the water surface portion of the image data is segmented using the Deeplab V3+ image segmentation network, and the image is binarized. The pixel values ​​of the water surface pixels are set to 255, and the pixel values ​​of the non-water surface pixels are set to 0. Then, the shoreline portion of the image is extracted using Canny edge detection. The point cloud registered in the map in the first frame is projected onto this image as key points to be tracked, and the points in the point cloud whose projected points fall on the shoreline are stored in a queue of size M. middle; S3, Unmanned Surface Vessel Attitude Update: When IMU data is received, the position is updated using IMU forward propagation. The pose information is In and calculate The prior distribution; when point cloud data is received, the distortion of the point cloud is first compensated by backpropagation using the IMU, and then the distortion-free point cloud is projected onto the most recent frame image. It is a point in the point cloud after distortion removal of the point cloud data of this frame, where L represents the point in the lidar coordinate system, and j represents the index of the point. Let L be the coordinate system of the lidar. yes At the projection point in the most recent frame image, if The part of the image that falls on the water surface will... and Delete them all; if If the image falls on the non-water surface, then find it on the map. Find the 5 nearest points and fit a plane using these 5 points. ,by arrive The distance is obtained as the residual. The posterior distribution, obtained by processing each point in the point cloud after distortion correction of the frame, and then combined with IMU forward propagation. The prior distribution is updated using error state iterative Kalman filtering. Upon receiving image data, the Deeplab V3+ image segmentation network is used to segment the water surface portion of the image frame. After segmentation, the image frame is binarized, and Canny edge detection is used to extract the shoreline portion. A two-step tracking method using LiDAR keypoints on the non-water surface is then employed to track the keypoints from the previous frame. It is one of the 3D points corresponding to the key points to be tracked in the previous frame, where G represents the point in the map coordinate system and s represents the index of the point. yes The pixels tracked in this frame image, and The reprojection error between them is obtained as the residual. The posterior distribution, after processing each keypoint, is obtained in conjunction with IMU forward propagation. The prior distribution is updated using error state iterative Kalman filtering. Utilize the updated The pose information in the middle will The points in the image are projected onto the frame image, let... yes One point in the middle, It is in this frame image that is related to The nearest shoreline pixel to the projection point, with and The reprojection error between them is obtained as the residual. The posterior distribution, after processing After each point in the array, the points are obtained through forward propagation using the IMU. The prior distribution is updated using error state iterative Kalman filtering. Finally, the point cloud registered in the map in the most recent frame is projected onto the current frame image as key points to be tracked, and the points in the point cloud that fall on the shoreline are saved. In the middle; the two-step tracking process of non-water surface LiDAR key points is as follows: R1, Calculate descriptors: Calculate the descriptors of the key points to be tracked in the previous frame image; R2. Coarse pose estimation: The acceleration and angular velocity information of the IMU are integrated to coarsely estimate the pose of the unmanned ship in the next image frame. Then, the 3D point cloud corresponding to the key points is projected into the next frame image based on the estimated pose to preliminarily predict the position of the key points in the next frame image. R3, Fine Tracking of Key Points: In the next frame of the image, take an N×N square window centered on the initially predicted key points, calculate the descriptors of all pixels in the window, and find the point in the vector space whose descriptor is closest to the descriptor calculated in R1. This pixel is the point that is finally tracked. S4. Map Update: After receiving point cloud data and updating the pose using that frame of point cloud data, the updated pose is used to update the map for each point in that frame of point cloud. Registering to the map coordinate system completes the map update.

2. The near-shore unmanned surface vessel SLAM method based on multi-sensor fusion according to claim 1, characterized in that, The forward propagation process of the IMU in the SLAM method is as follows: definition The prior estimate is The IMU propagates forward using a zero-order hold. That is, assuming that the IMU measurement value is constant within a sampling period and the process noise is set to zero, we have , in, and These represent the IMU measurements at the i-th and i+1-th times, respectively. Prior estimates, This represents the time interval between the i-th and i+1-th IMU measurements. This represents the raw data from the i-th IMU measurement. , Known , Then symbol Defined as , in, and Let represent the raw data from the accelerometer and gyroscope respectively during the i-th IMU measurement. Let represent the prior estimate of the rotation vector from the IMU coordinate system to the map coordinate system during the i-th IMU measurement. This represents the prior estimate of the IMU's position in the map coordinate system during the i-th IMU measurement. and Let represent the prior estimates of the extrinsic parameters between the IMU, the camera, and the lidar, respectively, for the i-th IMU measurement. Let represent the prior estimate of the IMU's velocity in the map coordinate system during the i-th IMU measurement. and Let represent the prior estimates of the biases of the accelerometer and gyroscope during the i-th IMU measurement, respectively. Let represent the prior estimate of gravitational acceleration during the i-th IMU measurement. Let represent the prior estimate of the time offset between the camera and the IMU during the i-th IMU measurement. This represents the prior estimate of the camera's intrinsic parameters during the i-th IMU measurement. Let represent the zero matrix of n×m, and Exp() and Log() be the Rodrigues transformations between the rotation vector and the rotation matrix.

3. The near-shore unmanned surface vessel SLAM method based on multi-sensor fusion according to claim 2, characterized in that, The SLAM method utilizes IMU backpropagation to compensate for point cloud distortion, and the process is as follows: T1, IMU Backpropagation: Suppose the current processing is the (k+1)th frame of point cloud data, the IMU propagates the state vector backward with a zero-order hold. And if the process noise is set to zero, then we have , in, , These represent the (j-1)th and jth lidar measurements within the point cloud in the (k+1)th frame, respectively. Prior estimates, This represents the acceleration and angular velocity of the unmanned vessel during the j-th lidar measurement within the point cloud in the (k+1)-th frame. Backpropagation begins by setting the pose to zero, and the velocity and IMU bias are set to zero. The value in This indicates that when the point cloud of frame k+1 is obtained... In addition, for any point in a frame of point cloud, the IMU data that is earlier in acquisition time and closest to the point is taken as the input for the acceleration and angular velocity of the unmanned ship during backpropagation at that point. T2, Point Cloud Distortion Correction: Generated by Backpropagation Time and Relative pose between moments , This represents a priori estimate of the transformation matrix from the IMU coordinate system during the j-th lidar measurement within the (k+1)-th frame of the point cloud to the IMU coordinate system when the (k+1)-th frame of the point cloud is acquired. express The rotation matrix part, express The translation vector part utilizes relative pose to measure local values. Measurement in the lidar coordinate system at the end of the scan , ; in, for Maximum a posteriori (MAP) estimation of the extrinsic parameters between the time-of-flight lidar and the IMU, L j L represents the lidar coordinate system during the j-th lidar measurement within the current frame's point cloud. k+1 This represents the lidar coordinate system when the point cloud of frame k+1 is obtained.

4. The near-shore unmanned surface vessel SLAM method based on multi-sensor fusion according to claim 3, characterized in that, In the SLAM method, the distance from a point to a surface is used as the residual, and the calculation process is as follows: set up To utilize the point cloud computing of the (k+1)th frame Maximum a posteriori estimation, using Rodriguez transform Rotation vector in and Convert to rotation matrix and According to the following formula Transformation from LiDAR coordinate system to map coordinate system: ,in, and These are the maximum a posteriori estimates of the rotation matrix and translation vector of the IMU coordinate system relative to the map coordinate system and the maximum a posteriori estimates of the rotation matrix and translation vector of the LiDAR coordinate system relative to the IMU coordinate system, respectively, when the point cloud of the (k+1)th frame is obtained. In the map... Recent face Having normal vector and a point in a plane The measurement residual is: .

5. The near-shore unmanned surface vessel SLAM method based on multi-sensor fusion according to claim 1, characterized in that, In the SLAM method, the reprojection error is used as the residual, and the calculation process is as follows: set up To calculate using the (k+1)th frame image Maximum a posteriori estimation, using Rodriguez transform Rotation vector in and Convert to rotation matrix and ,for and their corresponding points in the 3D map The measurement residual is: , in, express Three-dimensional coordinates in the camera coordinate system It is a pinhole projection model. and These are the maximum a posteriori estimates of the rotation matrix and translation vector of the IMU coordinate system relative to the map coordinate system and the maximum a posteriori estimates of the rotation matrix and translation vector of the camera coordinate system relative to the IMU coordinate system, respectively, when the (k+1)th frame image is acquired. Similarly, for and their corresponding points in the 3D map The measurement residual is: , in, express Three-dimensional coordinates in the camera coordinate system.

6. The near-shore unmanned surface vessel SLAM method based on multi-sensor fusion according to claim 1, characterized in that, In step S3, the pose update of the unmanned vessel is performed by using the Deeplab V3+ image segmentation network to segment the water surface portion of the frame image. After segmentation, the frame image is binarized, and the pixel values ​​of the pixels in the water surface portion are set to 255, while the pixel values ​​of the pixels in the non-water surface portion are set to 0.

Images