Global positioning method suitable for well mining environment
Patent Information
- Application Number
- CN202410947079.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-16
- Publication Date
- 2024-11-01
AI Technical Summary
相似环境下的关键帧不仅会造成冗余,还容易造成误匹配
[0023]该适用于井工矿环境的全局定位方法,首先通过激光雷达数据融合扫描环境得到3D点云集合,利用该历史3D点云进行SLAM建图;同时得到关键帧并提取其拓扑特征。定位时通过当前帧点云从开始节点开始搜索,通过提取得到的关键帧描述符,与上述历史特征描述符进行匹配与计算得到当前无人驾驶汽车的大致位姿。进一步的,利用大致位姿加载当前位置的分块地图,通过实时点云匹配局部地图得到更精确的位姿。
Smart Images

Figure CN118882633A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the field of positioning technology, and in particular to a global positioning method suitable for underground mining environments. Background Art
[0002] Accurate and stable positioning is the premise of autonomous navigation. In unmanned driving, global positioning generally refers to positioning on a built map after booting up or positioning after the robot is kidnapped. At this time, the position of the current frame may appear in any position in the existing map, or even outside the existing map. Generally speaking, in the absence of prior information, the robot relies solely on its own sensors to estimate its position in a known global map. There are three main traditional methods: one is to use rtk (carrier phase difference technology) as global constraint information to add to positioning; the second is to manually give the initial position directly on rviz; the third is to directly use all the point clouds of the current frame to perform brute force search and matching with the map.
[0003] The difficulties and challenges of global positioning in underground mining come from several aspects: (1) There is no GPS signal and it is impossible to obtain an accurate prior position; (2) There is a lack of prior knowledge, that is, the robot's position at the previous moment is unknown; (3) During re-positioning and closed-loop detection, that is, returning to a previously visited location, various changes will occur, including changes in lighting and viewing angle, as well as changes caused by some dynamic objects.
[0004] The current mainstream SLAM positioning methods include gmaping, NDT, and cartographer. During navigation, the robot's posture suddenly changes due to some external factors (such as human removal, external collision, etc.), which causes the original positioning algorithm that relies on continuous changes in posture to fail. In indoor environments, the combined navigation loses the GNSS signal and cannot establish global constraints. There are certain limitations in manually obtaining the initial posture directly on rviz, which is an unscientific method in autonomous driving. Key frames in similar environments not only cause redundancy, but also easily cause mismatching. The method of directly matching with lasers is slow and is not suitable for global positioning in larger scenes. To this end, the present invention provides a global positioning method suitable for underground mining environments. Summary of the invention
[0005] In view of the above-mentioned deficiencies in the prior art, the present invention provides a global positioning method suitable for underground mining environments.
[0006] The present invention provides the following technical solution: a global positioning method applicable to underground mining environments, comprising the following steps:
[0007] S1. The autonomous driving vehicle is equipped with IMU and LiDAR to scan the underground environment, and record and save the topic data of the sensor. The Cartographer algorithm is adapted and mapped according to the parameters of LiDAR and IMU to obtain a high-precision point cloud map without ghosting.
[0008] S2. Extract edge feature points and calculate the curvature according to the formula as follows:
[0009]
[0010] Define i as the lidar point cloud P k A point in P, i∈P k , let S be the continuous point set of i returned by the laser scanner in the same scan, j be the point set around i, the laser radar coordinate system L is a three-dimensional coordinate system, its origin is located at the geometric center of the laser radar, the x-axis points to the left, the y-axis points upward, and the z-axis points forward. The laser point i, i∈P k In L k The coordinate system is represented as X L (k,j) ;
[0011] S3. In the process of SLAM mapping of a specific environment, the edge point features are matched and combined with it for positioning. After the mapping is completed, each frame of laser point cloud used for mapping is first encoded into a corresponding set of edge point descriptors. And record its pose set Assume that the matching symbol corresponding to the currently collected laser point cloud data is I q , find the frame with the smallest distance function in I through feature matching, and its number is k. If the distance function is less than the threshold, the match is successful, that is, the two frames of point clouds are very similar. It can be considered that the positions of the two frames of point clouds are roughly the same, and the postures are only different in the heading angle. The current posture T q The expression can be written as:
[0012] Where R is the rotation matrix and t is the translation matrix. The expressions of the two are:
[0013]
[0014] t=[0 0 0] T .
[0015] Where θ is the heading angle offset obtained from edge feature point matching;
[0016] S4, loading the block map according to the global pose obtained by coarse matching;
[0017] S5. First, convert the map into a point cloud (soUrCe p'), then convert the laser of the current frame into a point cloud (targe P), align the source with the targe, that is, find the rotation and translation between the two; finally, apply it to the base_to_map coordinate system of the robot at the previous moment.
[0018] Preferably, in S5, p' is rotated by R and translated by t so that p' coincides with p, that is, the following formula:
[0019] e i =p i -(Rp′ i +t).;
[0020] Find R and t that can minimize the error;
[0021]
[0022] Compared with the prior art, the present invention has the following beneficial effects:
[0023] This global positioning method, which is applicable to underground mining environments, first obtains a 3D point cloud set by fusing laser radar data to scan the environment, and uses the historical 3D point cloud for SLAM mapping; at the same time, key frames are obtained and their topological features are extracted. During positioning, the current frame point cloud is searched from the starting node, and the extracted key frame descriptor is matched and calculated with the above historical feature descriptor to obtain the approximate position and posture of the current driverless car. Furthermore, the block map of the current position is loaded using the approximate position and a more accurate position and posture is obtained by matching the local map with the real-time point cloud.
[0024] This system can realize the re-positioning of the unmanned vehicle after it is powered on again or its posture changes suddenly in the underground mining environment, effectively solving the re-positioning problem in the absence of prior knowledge and the loop detection and global positioning problem when the robot returns to the location it has visited before due to environmental changes, effectively improving the stability and robustness of positioning. BRIEF DESCRIPTION OF THE DRAWINGS
[0025] Figure 1 This is a schematic diagram of the positioning and matching process of the present invention;
[0026] Figure 2 Schematic diagram of the tree search pruning algorithm of the present invention. DETAILED DESCRIPTION
[0027] In order to make the objectives, technical solutions and advantages of the embodiments of the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings of the embodiments of the present disclosure. In order to keep the following description of the embodiments of the present disclosure clear and concise, the present disclosure omits detailed descriptions of known functions and known components to avoid unnecessary confusion of the concepts of the present invention.
[0028] See also Figure 1 A global positioning method suitable for underground mining environments comprises the following steps:
[0029] S1 and the autonomous driving vehicle are equipped with IMU and lidar to scan the underground environment, record and save the topic data of the sensors, adapt the Cartographer algorithm according to the parameters of the lidar and IMU, and obtain a high-precision point cloud map without ghosting.
[0030] S2. Extract edge feature points and calculate the curvature according to the formula as follows:
[0031]
[0032] Define i as the lidar point cloud P k A point in P, i∈P k , let S be the continuous point set of i returned by the laser scanner in the same scan, j be the point set around i, the laser radar coordinate system L is a three-dimensional coordinate system, its origin is located at the geometric center of the laser radar, the x-axis points to the left, the y-axis points upward, and the z-axis points forward. The laser point i, i∈P k In L k The coordinate system is represented as X L (k,j) .
[0033] S3. In the process of SLAM mapping of a specific environment, the edge point features are matched and combined with it for positioning. After the mapping is completed, each frame of laser point cloud used for mapping is first encoded into a corresponding set of edge point descriptors. And record its pose set Assume that the matching symbol corresponding to the currently collected laser point cloud data is I q , find the frame with the smallest distance function in I through feature matching, and its number is k. If the distance function is less than the threshold, the match is successful, that is, the two frames of point clouds are very similar. It can be considered that the positions of the two frames of point clouds are roughly the same, and the postures are only different in the heading angle. The current posture T q The expression can be written as:
[0034] Where R is the rotation matrix and t is the translation matrix. The expressions of the two are:
[0035]
[0036] t=[0 0 0] T ;
[0037] Where θ is the heading angle offset obtained from edge feature point matching.
[0038] S4. Load the block map according to the global pose obtained by coarse matching, accelerate through branch and bound, obtain the accurate pose, and use the tree search pruning algorithm to solve the integer programming problem. Convert the optimal solution problem into a tree search problem. The root node represents the entire space, the leaf node represents the optimal solution, and the intermediate node represents a part of the solution space. The algorithm idea is as follows: Figure 2 .
[0039] S5. First, convert the map into a point cloud (source p'), then convert the laser of the current frame into a point cloud (target P), align the source with the target, that is, find the rotation and translation between the two; finally, apply it to the base_to_map coordinate system of the robot at the previous moment.
[0040] After rotation R and translation t, p' is made to coincide with p, that is, the following formula:
[0041] e i =p i -(Rp′ i +t).;
[0042] Find R and t that can minimize the error;
[0043]
[0044] The source and target are registered as follows:
[0045] (1) Find the nearest point in p' corresponding to each point cloud in p to form a point cloud set C.
[0046] (2) Find the center points of p' and p respectively (the calculation method is to sum the horizontal and vertical coordinates and divide by the number of point clouds)
[0047] (3) Calculate R by SVD first to calculate (rotation amount)
[0048] (4) Based on the calculated R, find t, according to the formula: t' = p-Rp'
[0049] (5) Start iteration, obtain the new point cloud A1 through R×p'+t, and repeat steps 1-4. This time, find the nearest point in p' corresponding to each point cloud in A1.
[0050] (6) Calculate the average distance dst between the point cloud An and its nearest point set Cn. When dst is less than the set threshold, exit the loop.
[0051] The above embodiments are only exemplary embodiments of the present invention and are not intended to limit the present invention. The protection scope of the present invention is defined by the claims. Those skilled in the art may make various modifications or equivalent substitutions to the present invention within the essence and protection scope of the present invention, and such modifications or equivalent substitutions shall also be deemed to fall within the protection scope of the present invention.
Claims
1. A global positioning method suitable for underground mining environments, characterized in that: The steps include: S1. The autonomous driving vehicle is equipped with IMU and LiDAR to scan the underground environment, and record and save the topic data of the sensor. The Cartographer algorithm is adapted and mapped according to the parameters of LiDAR and IMU to obtain a high-precision point cloud map without ghosting. S2. Extract edge feature points and calculate the curvature according to the formula as follows: Define i as the lidar point cloud P k A point in P, i∈P k , let S be the continuous point set of i returned by the laser scanner in the same scan, j be the point set around i, the laser radar coordinate system L is a three-dimensional coordinate system, its origin is located at the geometric center of the laser radar, the x-axis points to the left, the y-axis points upward, and the z-axis points forward. The laser point i, i∈P k In L k The coordinate system is represented as X L (k,j) ; S3. In the process of SLAM mapping of a specific environment, the edge point features are matched and combined with it for positioning. After the mapping is completed, each frame of laser point cloud used for mapping is first encoded into a corresponding set of edge point descriptors. And record its pose set Assume that the matching symbol corresponding to the currently collected laser point cloud data is I q , find the frame with the smallest distance function in I through feature matching, and its number is k. If the distance function is less than the threshold, the match is successful, that is, the two frames of point clouds are very similar. It can be considered that the positions of the two frames of point clouds are roughly the same, and the postures are only different in the heading angle. The current posture T q The expression can be written as: Where R is the rotation matrix and t is the translation matrix. The expressions of the two are: t=[000] T ; Where θ is the heading angle offset obtained from edge feature point matching; S4, loading the block map according to the global pose obtained by coarse matching; S5. First, convert the map into a point cloud (source p'), then convert the laser of the current frame into a point cloud (target P), align the source with the target, that is, find the rotation and translation between the two; finally, apply it to the base_to_map coordinate system of the robot at the previous moment.
2. A global positioning method suitable for underground mining environment according to claim 1, characterized in that: In S5, p' is rotated by R and translated by t so that p' coincides with p, that is, the following formula: e i =p i -(Rp′ x +t).; Find R and t that can minimize the error;
