CN115290073A - A SLAM method and system under unstructured characteristics of underground mines - Google Patents

Abstract

The invention relates to the technical field of underground positioning and navigation in mines, and solves the technical problem of difficulty in underground positioning and mapping under unstructured environmental characteristics and GPS cannot function, and in particular relates to a SLAM method under the unstructured characteristics of underground mines, comprising: The following process: obtain the point cloud information of the current laser frame of the lidar; calculate the curvature of each point in the point cloud information of the current laser frame of the lidar, and extract the corner feature and plane point feature of each point according to the curvature of each point; Extract the information of the current frame through the camera, use the FAST algorithm to detect the corner of the current frame according to the information of the current frame, and determine whether it is a corner feature point; use the KLT optical flow tracking algorithm to track the key frame of the current sliding window according to the information of the current frame feature points. The invention realizes high-precision positioning in the coal mine, balances the precision and the calculation amount at the same time, and improves the positioning and mapping accuracy and real-time performance of the mine roadway.

Description

A SLAM method and system under unstructured characteristics in underground mines

technical field

The present invention relates to the technical field of underground positioning and navigation in mines, in particular to a SLAM method and system under unstructured features in underground mines.

Background technique

Coal mine roadways, mining working faces and other operating areas have typical unstructured environmental characteristics, and GPS technology cannot be directly applied underground in coal mines, resulting in the inability of the autonomous positioning system of coal mine robots to achieve high-precision positioning and poor attitude perception in underground coal mines, resulting in Mine disasters occur frequently during coal mining, thus reducing the safety of coal mining.

Contents of the invention

Aiming at the deficiencies of the prior art, the present invention provides a SLAM method and system under the unstructured characteristics of underground mines, which solves the technical problem of difficult underground positioning and mapping under unstructured environmental characteristics and GPS cannot function.

In order to solve the problems of the technologies described above, the present invention provides the following technical solutions: a kind of SLAM method under unstructured characteristics in underground mine, comprising the following process:

Obtain the point cloud information of the current laser frame of the lidar;

Calculate the curvature of each point in the point cloud information of the current laser frame of the laser radar, and extract the corner point features and plane point features of each point according to the curvature of each point;

Extract the information of the current frame through the camera, use the FAST algorithm to detect the corner point of the current frame according to the information of the current frame, and judge whether it is a corner feature point;

According to the information of the current frame, the KLT optical flow method tracking algorithm is used to track the feature points of the current sliding window key frame;

Update the pose estimation of the current state according to the reprojection error between the pose estimation of the IMU priori and the feature points tracking the current sliding window keyframe;

The latest visual features are obtained by updating the state of the pose estimation of the current state through ESIKF;

Match the latest visual features with the point cloud information of the current laser frame of the lidar with corner feature point cloud data to obtain the lidar odometry factor and camera odometry factor;

After adding the lidar odometer factor, camera odometer factor and IMU pre-integration factor into the factor map optimization, the 3D map modeling is carried out.

Further, in the step of calculating the curvature of each point in the point cloud information of the current laser frame of the laser radar, and extracting the corner point features and plane point features of each point according to the curvature of each point, the following process is specifically included:

Subscribe to the point cloud information after the motion distortion correction of the current laser frame;

Traversing the effective point cloud after the motion distortion correction of the current laser frame, extracting several points before and after the current laser point from the effective point cloud and calculating their corresponding curvature;

According to the curvature of several points before and after the current laser point, a corner point feature set and a plane point feature set are established;

The corner feature of each point is extracted from the corner feature set, and the plane point feature of each point is extracted from the plane point feature set.

Further, adopting the FAST algorithm to detect the corner point of the current frame according to the information of the current frame, and judging whether it is a corner feature point in this step, specifically include the following process:

Subscribe to the information of the current frame extracted by the current camera;

Select a pixel from the information of the current frame, and judge whether the pixel is a corner feature point.

Further, in the step of selecting a pixel from the information of the current frame and judging whether the pixel is a corner feature point, the following process is specifically included:

Set the luminance value of the selected pixel p as I _p , and the luminance threshold as t;

Set a discretized Bresenham circle with a radius equal to 3 pixels centered on pixel p, and there are 16 pixels on the boundary of the Bresenham circle;

There are 9 consecutive pixel points on the Bresenham circle with a size of 16 pixels. If the pixel value of the 9 consecutive pixel points is greater than or smaller than I _p +t, it is determined that the pixel p is a corner feature point;

If the pixel values of 9 consecutive pixel points are equal to I _p +t, it is determined that the pixel p is not a corner feature point.

Further, in the step of using the KLT optical flow tracking algorithm to track the feature points of the current sliding window key frame according to the information of the current frame, the following processes are specifically included:

Subscribe to the information of the current frame extracted by the current camera;

Perform histogram equalization on the original image in the current frame information;

Extract the pyramid of the image after histogram equalization, and then extract the FAST feature points of the previous frame image;

Perform LK optical flow tracking on the FAST feature points of the previous frame image;

Determine the number of feature points of the previous frame image, if the feature points of the previous frame image are less than 10, directly set it as tracking failure;

If the number of tracking points meets the preset threshold, LK optical flow tracking is performed directly.

Further, in the step of updating the pose estimation of the current state, the following process is specifically included:

Judge whether the current frame is a key frame by the method of marginalization based on the parallax judgment. If the parallax between the new frame and the previous frame is large, the oldest frame will be marginalized, and if the parallax is small, the previous frame will be marginalized;

Add the prior of the new feature point to the IMU, and add all the observations of a feature point into the triangulation calculation according to the pose estimation of the IMU prior to obtain the latest observation frame;

The latest observation frame of a feature point and the IMU pose estimation corresponding to the oldest frame and two frames in the sliding window are used as reprojection errors to establish a residual relationship.

Further, in the step of obtaining the latest visual features through ESIKF to update the state of the pose estimation of the current state, it specifically includes the following process:

Calculate the plane of the current frame through the parameter equation, and match the plane of the current frame with the plane points of the previous frame to obtain the same plane, that is, the observation of pose transformation;

Use the IMU integral as a priori to update the EKF and update the covariance to obtain the optimal pose estimation of the error and the state vector;

When ESIKF finishes exiting, update the covariance matrix of the posterior, and complete the state update of the pose estimation of the current state;

Add some point clouds in the new frame to the kd tree, and update the flag to find it in the map in the next frame;

The result is approximated by several iterations of ESIKF, and the current sliding window is updated with the latest ESKF posterior.

Further, in the step of performing three-dimensional map modeling after adding the lidar odometer factor, camera odometer factor and IMU pre-integration factor to the factor map optimization, the following processes are specifically included:

Add the lidar odometer factor, camera odometer factor and IMU pre-integration factor to the factor graph for optimization;

Calculate the Jacobian matrix of the lidar odometer factor, camera odometer factor and IMU pre-integration factor on the state quantity, and use the Levenberg-Marquardt method to solve the state quantity;

In the ros system, use the map_server server to configure the rgbdslam package and combine all the data optimized by the factor graph for 3D map modeling.

The present invention also provides another technical solution, a system for realizing the SLAM method under unstructured characteristics in underground mines, the lidar odometer factor, camera odometer factor and IMU pre-integration factor after factor graph optimization, By using the map_server server in the ros system to configure the rgbdslam package and combine all the optimized data for 3D map modeling, including:

Point cloud information acquisition module, the point cloud information acquisition module is used to obtain the point cloud information of the laser radar current laser frame;

Feature extraction module, described feature extraction module is used for calculating the curvature of each point in the laser radar current laser frame point cloud information, and extracts the corner point feature and the plane point feature of each point according to the curvature of each point;

Corner feature judging module, described corner feature judging module is used for extracting the information of current frame by camera, adopts FAST algorithm to detect the corner of current frame according to the information of current frame, and judges whether it is a corner feature point;

A feature point tracking module, the feature point tracking module is used to track the feature points of the current sliding window key frame using the KLT optical flow method tracking algorithm according to the information of the current frame;

A pose estimation update module, the pose estimation update module is used to update the pose estimate of the current state according to the reprojection error between the pose estimate of the IMU priori and the feature point tracking the current sliding window keyframe;

Visual feature obtains module, and described visual feature obtains module and is used for carrying out state update to the pose estimation of current state by ESIKF and obtains the latest visual feature;

The odometer factor deriving module, the odometer factor deriving module is used to match the latest visual features with the point cloud information of the current laser frame of the lidar with corner feature point cloud data to obtain the lidar mileage meter factor and camera odometer factor;

Factor graph optimization module, described factor graph optimization module is used for adding laser radar odometer factor, camera odometer factor and IMU pre-integration factor to carry out three-dimensional map modeling after factor graph optimization.

By means of the above technical solution, the present invention provides a SLAM method and system under the unstructured characteristics of underground mines, which at least have the following beneficial effects:

1. The present invention uses laser radar, camera and IMU to realize high-precision positioning underground in coal mines, and improves the attitude perception of the underground environment, thereby realizing high-precision positioning in underground coal mines, and at the same time balancing the accuracy and calculation amount, and improving mine wells. The accuracy and real-time performance of lane positioning and mapping effectively solves the problem of difficult underground positioning and mapping in unstructured environments where GPS cannot function.

2. The present invention uses the SLAM method of laser radar, camera and IMU to calculate the point cloud features of laser radar and the feature information extracted and optimized by IMU data and camera, so as to improve the accuracy of the whole system in an unstructured environment. In addition, key frame matching is carried out to reduce the calculation load of the whole system, to achieve a balance between precision and calculation load, and to ensure the accuracy and real-time performance of the system.

3. The present invention extracts the features of the point cloud from the lidar, and the camera can also provide visual features to match the features of the point cloud, thereby optimizing the entire system, so that the entire system remains stable.

Description of drawings

The accompanying drawings described here are used to provide a further understanding of the application and constitute a part of the application. The schematic embodiments of the application and their descriptions are used to explain the application and do not constitute undue limitations to the application. In the attached picture:

Fig. 1 is the flowchart of SLAM method of the present invention;

Fig. 2 is a schematic diagram of point cloud feature extraction data transmission of the present invention;

Fig. 3 is a schematic diagram of data transmission for visual feature extraction in the present invention;

Fig. 4 is a schematic diagram of visual odometer data transmission of the camera of the present invention;

Fig. 5 is a schematic diagram of factor graph optimization data transmission in the present invention.

In the figure: 100, point cloud information acquisition module; 200, feature extraction module; 300, corner feature judgment module; 400, feature point tracking module; 500, pose estimation update module; 600, visual feature derivation module; 700, The odometer factor derivation module; 800, the factor graph optimization module.

Detailed ways

In order to make the above objects, features and advantages of the present invention more obvious and understandable, the present invention will be described in further detail below in conjunction with the accompanying drawings and specific embodiments. In this way, the realization process of how the application uses technical means to solve technical problems and achieve technical effects can be fully understood and implemented accordingly.

Those of ordinary skill in the art can understand that all or part of the steps in the method of the above-mentioned embodiments can be completed by instructing the relevant hardware through a program. Therefore, the present application can adopt a complete hardware embodiment, a complete software embodiment, or a combination of software and The form of the embodiment in terms of hardware. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.

Please refer to Fig. 1-Fig. 5, which shows a specific implementation of this embodiment, which fully considers the unstructured environmental characteristics of mine shafts and the factors that GPS technology cannot be directly used. , so under certain circumstances, lidar cannot collect enough feature point clouds, but IMU can exist independently of other sensors, so IMU can perform pre-integration when there are not enough feature point clouds to make up for the lack of Deficiencies of adequately characterized point clouds. Furthermore, the feature extraction of the point cloud is performed on the lidar, and the camera can also provide visual features to match with the point cloud features, so as to optimize the entire system, so that the entire system remains stable.

A kind of SLAM method under unstructured characteristic under mine well, comprises the following steps:

S1. Obtain the point cloud information of the current laser frame of the lidar.

S2. Calculate the curvature of each point in the point cloud information of the current laser frame of the lidar, and extract the corner feature and plane point feature of each point according to the curvature of each point.

In step S2, specifically include the following steps:

S21. Subscribing to point cloud information after motion distortion correction of the current laser frame.

S22. Traversing the effective point cloud corrected by the motion distortion of the current laser frame, extracting several points before and after the current laser point from the effective point cloud and calculating their corresponding curvatures, the square of the curvature distance difference at the corner of the current laser point is used as the curvature , and store the curvature value of the point at the same time, several points before and after the current laser point take parameter 5 as an example, take the first five points and the last five points of the current laser point, a total of ten points and calculate the curvature of each point, each The curvature corresponding to each point is the square of the difference in curvature distance at the corner point.

S23. Establish a corner point feature set and a plane point feature set according to the curvature of several points before and after the current laser point. During this process, mark the points belonging to the two situations of occlusion and parallelism from several points without performing feature extraction, and then Traverse the scan lines, divide the point cloud scanned by each scan line into 6 segments, extract 20 corner points and an unlimited number of plane points for each segment, add the points with large curvature in each segment to the corner point feature set, and Points with small curvature are added to the plane point feature set, and finally the non-corner points are considered to be plane points, added to the plane point feature set, and then down-sampled.

S24. Extract the corner feature of each point from the corner feature set, and extract the plane point feature of each point from the plane point feature set. In the process, publish the corner of the corresponding point according to the corner feature and the plane point feature Point cloud and planar point cloud, at the same time release the point cloud information of the current laser frame of the lidar with the corner feature point cloud data.

S3. Extract the information of the current frame through the camera, and use the FAST algorithm to detect the corner point of the current frame according to the information of the current frame, and judge whether it is a corner feature point. Taking a mine shaft as an example, the camera captures the image in the current scene. The image of the frame contains the information of the scene.

In step S3, specifically include the following steps:

S31. Subscribe to the information of the current frame extracted by the current camera.

S32. Select a pixel from the information of the current frame, and judge whether the pixel is a corner feature point.

In step S32, judging whether a pixel is a feature point includes the following steps:

S321. Set the luminance value of the selected pixel p as I _p , and set the luminance threshold as t.

S322. Set a discretized Bresenham circle with a radius equal to 3 pixels centered on the pixel p, and there are 16 pixels on the boundary of the Bresenham circle.

There are 9 consecutive pixels on the Bresenham circle with a size of 16 pixels. If the pixel values of the 9 consecutive pixels are greater or less than I _p +t, then the pixel p is determined to be a corner feature point.

If the pixel values of 9 consecutive pixel points are equal to I _p +t, it is determined that the pixel p is not a corner feature point.

S4. Using the KLT optical flow method tracking algorithm to track the feature points of the current sliding window key frame according to the information of the current frame.

In step S4, specifically include the following steps:

S41. Subscribe to the information of the current frame extracted by the current camera.

S42. Perform histogram equalization on the original image in the current frame information.

S43. Extract the pyramid of the image after the histogram equalization, and then extract the FAST feature points of the previous frame image.

S44. Perform LK optical flow tracking on the FAST feature points of the last frame of image.

S45. Determine the number of feature points of the previous frame of image, if the number of feature points of the previous frame of image is less than 10, directly set it as tracking failure; if the number of tracking points meets the preset threshold, then directly perform LK optical flow tracking, then , firstly remove the feature points of the original image, then use the RANSAC algorithm to remove outliers, and set whether all feature points in the previous frame of image are successfully tracked, and only points that satisfy both optical flow tracking and RANSAC detection are marked as 1.

S5. Update the pose estimation of the current state according to the reprojection error between the IMU prior pose estimation and the feature point tracking the key frame of the current sliding window. The IMU prior pose estimation is obtained from the IMU, and the camera captures the current The initial pose estimation of the current frame of the picture in the scene.

In step S5, specifically include the following steps:

S51. Determine whether the current frame is a key frame by judging which marginalization method to use by parallax. If the parallax between the new frame and the previous frame is large, the oldest frame will be marginalized, and if the parallax is small, the previous frame will be marginalized.

S52. Add the priori of new feature points in the IMU, and add all observations of a feature point into triangulation calculations to obtain the latest observation frame according to the pose estimation of the IMU priori.

S53. The latest observation frame of a feature point and the IMU pose estimation corresponding to the oldest frame and two frames in the sliding window are re-projected to establish a residual relationship. At the beginning of each iteration, the previous visual-inertial system is The camera and IMU joint optimization system uses the optimized pose estimation as the initial value, recalculates within a window, and updates the current state pose estimation by establishing a residual relationship through the reprojection error.

S6, carry out state update to the pose estimation of current state by ESIKF and obtain the latest visual features;

In step S6, specifically include the following steps:

S61. Obtain the plane of the current frame through parameter equation calculation, and match the plane of the current frame with the plane points of the previous frame to obtain the same plane, that is, the observation of pose transformation.

S62. Use the IMU integral as a priori to update the EKF and update the covariance to obtain the optimal pose estimation of the error and the state vector. After solving the gain matrix K, the posteriori of the error amount is obtained, and then the amount of change is calculated. When the change It is judged to converge when the amount is less than the threshold.

S63. When the ESIKF finishes exiting, update the posterior covariance matrix, and complete the state update of the pose estimation of the current state.

S64, add some point clouds in the new frame to the kd tree, and update the next frame to find its sign in the map.

S65. Approximate the result by several iterations of ESIKF, and update the current sliding window with the latest ESKF posterior.

S7. The latest visual feature is matched with the point cloud information of the current laser frame of the lidar with corner feature point cloud data to obtain the lidar odometry factor and the camera odometry factor.

S8. After adding the lidar odometer factor, the camera odometer factor and the IMU pre-integration factor into the factor map optimization, the three-dimensional map modeling is performed.

Add an IMU preintegration factor to the factor graph:

Considering an IMU measurement between two consecutive frames b _k and b _{k − 1} within a sliding window, the IMU pre-integrated measurement residual is defined as follows:

是四元数的三阶误差状态表示。

是两个连续图像帧的时间间隔 内只采用带噪声的加速度计和陀螺仪测量的IMU预积分测量值，加速度计和 陀螺仪偏差还包括在残差项中用于在线矫正。where [.] _xyz extracts the vector part (imaginary part) of the quaternion q used for the error state representation.

is the third-order error state representation of a quaternion.

is the IMU pre-integrated measurement value measured by only the noisy accelerometer and gyroscope in the time interval of two consecutive image frames, and the accelerometer and gyroscope deviations are also included in the residual item for online correction.

表示残差的定义；

分别是对于IMU的 位置速度方向预积分量的差值；δb_a、δb_g分别是对于IMU加速度和角速度的偏 置量差值；

分别是k-1帧IMU坐标系在世界坐标系下对应的位 置、速度、旋转；

分别是k-1时刻到k时刻两时刻间位置、速 度、角速度的预积分估计值；g^W是世界坐标系下的重力矢量；

是k时刻IMU 加速度的偏置量；

是k时刻IMU角速度的偏置量。In the above formula,

Indicates the definition of the residual;

They are the difference of the position and velocity direction pre-integration of the IMU; δb _a and δb _g are the offset difference of the acceleration and angular velocity of the IMU respectively;

They are the position, velocity, and rotation corresponding to the k-1 frame IMU coordinate system in the world coordinate system;

are the pre-integrated estimated values of position, velocity, and angular velocity between time k-1 and time k; g ^W is the gravity vector in the world coordinate system;

is the bias of IMU acceleration at time k;

is the offset of the IMU angular velocity at time k.

Adding a camera odometry factor to the factor graph: Considering the first observed l ^th feature in the i ^th image, the residual of the feature observation in the j ^th image is defined as:

表示视觉残差的定义，b1，b2为正切平面的两个正交基 也就是原点到测量单位球面点的向量a，在球面上的切向单位向量。

为第l 个特征在第j张图像中的像素通过相机内参反向投影到单位球面上的三维坐 标。

为第l个特征在第j张图像中的像素的三维坐标。

表示从IMU坐标 系到相机坐标系的旋转转换矩阵三维坐标表示。

表示从世界坐标系到IMU 坐标系的旋转转换矩阵三维坐标表示。

表示从相机坐标系到IMU坐标系的 位置转换矩阵三维坐标表示。

表示从IMU坐标系到世界坐标系的位置转换 矩阵三维坐标表示。In the above formula,

Represents the definition of visual residuals, b1 and b2 are the two orthogonal bases of the tangent plane, that is, the vector a from the origin to the measurement unit spherical point, and the tangential unit vector on the spherical surface.

is the three-dimensional coordinates of the pixel of the lth feature in the jth image back-projected to the unit sphere through the camera internal reference.

is the three-dimensional coordinates of the pixel of the lth feature in the jth image.

Indicates the three-dimensional coordinate representation of the rotation transformation matrix from the IMU coordinate system to the camera coordinate system.

Indicates the three-dimensional coordinate representation of the rotation transformation matrix from the world coordinate system to the IMU coordinate system.

Indicates the three-dimensional coordinate representation of the position transformation matrix from the camera coordinate system to the IMU coordinate system.

Indicates the three-dimensional coordinate representation of the position transformation matrix from the IMU coordinate system to the world coordinate system.

是使用相机内参将像素坐标转变为单位向量的反投影函数。

是第i^th幅图像中第l^th个特征的第一次观测。

是第j^th幅图像中相 同特征的观测。由于视觉残差的自由度是2，因此将残差向量投影到切平面上。in,

is a backprojection function that converts pixel coordinates into unit vectors using camera intrinsics.

is the first observation of the l ^th feature in the ^ith image.

is the observation of the same feature in the j ^th image. Since the degree of freedom of the visual residual is 2, the residual vector is projected onto the tangent plane.

其中ΔT_k,k+1为状态节点X_k与X_k+1之间的相对变换关系。Add the lidar odometry factor to the factor graph: The laser odometry factor is

Among them, ΔT _k,k+1 is the relative transformation relationship between state nodes X _k and X _k+1 .

Considering that the method of using laser radar and IMU in the mine shaft improves the accuracy of the whole system in an unstructured environment, but it will cause an increase in the amount of calculation, so the embodiment performs key frame matching, so that the accuracy and the amount of calculation can be achieved. Balance ensures the accuracy and real-time performance of the system.

Using the SLAM method of lidar, camera and IMU, the point cloud features of lidar and the feature information extracted and optimized by IMU data and camera are calculated together to improve the accuracy of the whole system in an unstructured environment, but it will cause calculation Therefore, this embodiment performs key frame matching to reduce the calculation amount of the entire system, so as to strike a balance between the accuracy and the calculation amount, and ensure the accuracy and real-time performance of the system.

In step S8, specifically include the following steps:

S81, adding the lidar odometer factor, the camera odometer factor and the IMU pre-integration factor into the factor graph for optimization.

S82. Respectively obtain the Jacobian matrix of the lidar odometer factor, the camera odometer factor and the IMU pre-integration factor about the state quantity, and use the Levenberg-Marquardt method to solve the state quantity.

S83. In the ros system, use the map_server server to configure the rgbdslam package and combine all the optimized data of the factor map to perform three-dimensional map modeling.

In this embodiment, laser radar, camera, and IMU are used to realize high-precision positioning of the coal mine underground, and improve the attitude perception of the underground environment, thereby realizing high-precision positioning of the coal mine underground, and at the same time balancing the accuracy and calculation amount, and improving mine shafts. The positioning and mapping accuracy and real-time performance effectively solve the problem of difficult underground positioning and mapping under unstructured environment characteristics and GPS cannot function.

This embodiment also provides a system applied to the SLAM method under the unstructured characteristics of the above-mentioned underground mine. The lidar odometer factor, camera odometer factor and IMU pre-integration factor after factor graph optimization are passed in the ros system Use the map_server server to configure the rgbdslam package and combine all the optimized data for 3D map modeling, including:

Point cloud information acquisition module 100, the point cloud information acquisition module 100 is used to obtain the point cloud information of the current laser frame of lidar.

Feature extraction module 200, described feature extraction module 200 is used for calculating the curvature of each point in the laser radar current laser frame point cloud information, and extracts the corner point feature and plane point feature of each point according to the curvature of each point.

Corner feature judging module 300, described corner feature judging module 300 is used for extracting the information of current frame by camera, adopts FAST algorithm to detect the corner of current frame according to the information of current frame, and judges whether it is a corner feature point.

Feature point tracking module 400, described feature point tracking module 400 is used for adopting KLT optical flow method tracking algorithm to track the feature point of current sliding window key frame according to the information of current frame.

The pose estimation update module 500, the pose estimation update module 500 is used to update the pose estimate of the current state according to the reprojection error between the pose estimate of the IMU prior and the feature point of the tracking current sliding window key frame.

Visual feature obtains module 600, and described visual feature obtains module 600 and is used for carrying out state update to the pose estimation of current state by ESIKF and obtains the latest visual feature.

The odometer factor deriving module 700, the odometer factor deriving module 700 is used to carry out feature matching with the point cloud information of the current laser frame of the laser radar with corner feature point cloud data to obtain the laser Radar odometry factor and camera odometry factor.

Factor graph optimization module 800, described factor graph optimization module 800 is used for adding lidar odometer factor, camera odometer factor and IMU pre-integration factor to carry out three-dimensional map modeling after factor graph optimization.

In this embodiment, the feature extraction of the point cloud is performed on the lidar, and the camera can also provide visual features to match with the feature of the point cloud, so as to optimize the entire system, so that the entire system remains stable.

Using laser radar, camera and IMU to achieve high-precision positioning of coal mine underground, and improve the attitude perception of the underground environment, thereby achieving high-precision positioning of coal mine underground, while balancing the accuracy and calculation amount, and improving the positioning and positioning of mine shafts Mapping accuracy and real-time performance effectively solve the problem of difficult underground positioning and mapping under unstructured environment characteristics and GPS cannot function.

Each embodiment in this specification is described in a progressive manner, each embodiment focuses on the differences from other embodiments, and the same or similar parts between the various embodiments can be referred to each other. For the above embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and for relevant parts, please refer to the part of the description of the method embodiments.

The above embodiments have introduced the present invention in detail. The principles and implementation modes of the present invention have been explained by using specific examples in this paper. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; meanwhile, for Those skilled in the art will have changes in the specific implementation and scope of application according to the idea of the present invention. In summary, the contents of this specification should not be construed as limiting the present invention.

Claims (9)

1. A SLAM method under unstructured characteristics in underground mine, is characterized in that, comprises following process: Obtain the point cloud information of the current laser frame of the lidar; Calculate the curvature of each point in the point cloud information of the current laser frame of the lidar, and extract the corner point features and plane point features of each point according to the curvature of each point; Extract the information of the current frame through the camera, use the FAST algorithm to detect the corner point of the current frame according to the information of the current frame, and judge whether it is a corner feature point; According to the information of the current frame, the KLT optical flow tracking algorithm is used to track the feature points of the key frame of the current sliding window; Update the pose estimation of the current state according to the reprojection error between the pose estimation of the IMU prior and the feature points tracking the current sliding window keyframe; The latest visual features are obtained by updating the state of the pose estimation of the current state through ESIKF; Match the latest visual features with the point cloud information of the current laser frame of the lidar with corner point feature point cloud data to obtain the lidar odometry factor and camera odometry factor; Add the lidar odometry factor, camera odometry factor and IMU pre-integration factor to the factor map optimization to carry out 3D map modeling. 2. The SLAM method according to claim 1, characterized in that: calculating the curvature of each point in the current laser frame point cloud information of lidar, and extracting the corner feature and plane of each point according to the curvature of each point In this step of point features, the following processes are specifically included: Subscribe to the point cloud information after the motion distortion correction of the current laser frame; Traversing the effective point cloud after the motion distortion correction of the current laser frame, extracting several points before and after the current laser point from the effective point cloud and calculating their corresponding curvature; According to the curvature of several points before and after the current laser point, a corner point feature set and a plane point feature set are established; The corner feature of each point is extracted from the corner feature set, and the plane point feature of each point is extracted from the plane point feature set. 3. The SLAM method according to claim 1, characterized in that: adopting the FAST algorithm to detect the corner point of the current frame according to the information of the current frame, and judging whether it is a corner feature point in this step, specifically comprising the following process: Subscribe to the information of the current frame extracted by the current camera; Select a pixel from the information of the current frame, and judge whether the pixel is a corner feature point. 4. The SLAM method according to claim 3, characterized in that: selecting a pixel from the information of the current frame, and judging whether the pixel is a corner feature point in this step, specifically comprising the following process: Set the luminance value of the selected pixel p as I _p , and the luminance threshold as t; Set a discretized Bresenham circle with a radius equal to 3 pixels centered on pixel p, and there are 16 pixels on the boundary of the Bresenham circle; There are 9 consecutive pixel points on the Bresenham circle with a size of 16 pixels. If the pixel value of the 9 consecutive pixel points is greater than or smaller than I _p +t, it is determined that the pixel p is a corner feature point; If the pixel values of 9 consecutive pixel points are equal to I _p +t, it is determined that the pixel p is not a corner feature point. 5. The SLAM method according to claim 1, characterized in that: in the step of using the KLT optical flow method tracking algorithm to track the feature points of the current sliding window key frame according to the information of the current frame, specifically comprising the following process: Subscribe to the information of the current frame extracted by the current camera; Perform histogram equalization on the original image in the current frame information; Extract the pyramid of the image after histogram equalization, and then extract the FAST feature points of the previous frame image; Perform LK optical flow tracking on the FAST feature points of the previous frame image; Determine the number of feature points in the previous frame of image, if the number of feature points in the previous frame of image is less than 10, directly set it as tracking failure; If the number of tracking points meets the preset threshold, LK optical flow tracking is performed directly. 6. The SLAM method according to claim 1, characterized in that: in the step of updating the pose estimation of the current state, the following processes are specifically included: Judging by the parallax method to determine whether the current frame is a key frame, the oldest frame will be marginalized if the parallax between the new frame and the previous frame is large, and the previous frame will be marginalized if the parallax is small; Add the prior of the new feature point to the IMU, and add all the observations of a feature point into the triangulation calculation according to the pose estimation of the IMU prior to obtain the latest observation frame; The latest observation frame of a feature point and the IMU pose estimation corresponding to the oldest frame and two frames in the sliding window are reprojected to establish a residual relationship. 7. The SLAM method according to claim 1, characterized in that: in the step of obtaining the latest visual features by performing state update on the pose estimation of the current state by ESIKF, the following processes are specifically included: Calculate the plane of the current frame through the parameter equation, and match the plane of the current frame with the plane point of the previous frame to obtain the same plane, that is, the observation of the pose transformation; Use the IMU integral as a priori to update the EKF and update the covariance to obtain the optimal pose estimation of the error and the state vector; When ESIKF ends and exits, the posterior covariance matrix is updated to complete the state update of the pose estimation of the current state; Add some point clouds in the new frame to the kd tree, and update the flag to find it in the map in the next frame; The result is approximated by several iterations of ESIKF, and the current sliding window is updated with the latest ESKF posterior. 8. The SLAM method according to claim 1, characterized in that: in the step of adding the lidar odometer factor, the camera odometer factor and the IMU pre-integration factor into the factor map optimization, and then performing three-dimensional map modeling, specifically comprising The following process: Add the lidar odometer factor, camera odometer factor and IMU pre-integration factor to the factor graph for optimization; Calculate the Jacobian matrix of the lidar odometer factor, camera odometer factor and IMU pre-integration factor on the state quantity, and use the Levenberg-Marquardt method to solve the state quantity; In the ros system, use the map_server server to configure the rgbdslam package and combine all the data optimized by the factor graph for 3D map modeling. 9. A system for realizing the SLAM method under the mine underground unstructured feature described in any one of the above claims 1-8, the lidar odometer factor, the camera odometer factor and the IMU pre-integration after factor graph optimization Factor, by using the map_server server to configure the rgbdslam package in the ros system and combining all the data after the optimization of the graph to perform three-dimensional map modeling, it is characterized in that it includes: A point cloud information acquisition module (100), the point cloud information acquisition module (100) is used to obtain the point cloud information of the current laser frame of the lidar; Feature extraction module (200), described feature extraction module (200) is used for calculating the curvature of each point in the laser radar current laser frame point cloud information, and extracts the corner feature of each point and the plane according to the curvature of each point point feature; Corner feature judgment module (300), described corner point feature judgment module (300) is used for extracting the information of current frame by camera, adopts FAST algorithm to detect the corner point of current frame according to the information of current frame, and judges whether it is a corner Feature points; Feature point tracking module (400), said feature point tracking module (400) is used to adopt the KLT optical flow method tracking algorithm to track the feature point of the current sliding window key frame according to the information of the current frame; The pose estimation update module (500), the pose estimation update module (500) is used to update the position of the current state according to the reprojection error between the pose estimation of the IMU priori and the feature point of the tracking current sliding window key frame attitude estimation; A visual feature deriving module (600), the visual feature deriving module (600) is used to update the state of the pose estimation of the current state by ESIKF to obtain the latest visual features; The odometer factor obtains the module (700), and the odometer factor obtains the module (700) for performing feature matching on the point cloud information of the current laser frame of the lidar with the corner feature point cloud data and the latest visual feature , get the lidar odometry factor and the camera odometry factor; A factor graph optimization module (800), the factor graph optimization module (800) is used to add the lidar odometer factor, the camera odometer factor and the IMU pre-integration factor into the factor graph optimization to perform three-dimensional map modeling.

Images