A global positioning method and system for indoor robots based on cylindrical features

Abstract

This invention proposes a global positioning method and system for indoor robots based on cylindrical features. The method involves stitching a reference point cloud map of the robot's operating scene from multiple frames of offline LiDAR point clouds using a laser odometry method. The method then extracts cylindrical point clouds from the map based on a geometric model, constructs a rasterized map using an affine transformation matrix, and digitally represents key map information and saves it offline to construct a parametric map. Cylinders are extracted from real-time, single-frame, multi-line LiDAR point clouds based on the symmetry, salience, and outliers of indoor cylindrical structures. When only a single cylinder is detected in the real-time data, the robot's global pose is calculated using a heuristic search in conjunction with the raster map data. When multiple cylinders are detected in the real-time data, the robot's global pose is calculated using an optimization solution in conjunction with the raster map data. This method overcomes the difficulty of positioning in indoor environments lacking external signals.

Description

Technical Field

[0001] The present invention belongs to the field of surveying, mapping, remote sensing and unmanned driving technology, and in particular relates to a global positioning method and system for an indoor robot based on cylindrical features. Background Art

[0002] With the continuous development of social productivity and innovations in artificial intelligence, unmanned driving and robotics technologies are gradually entering numerous fields, including logistics and distribution, security patrol, mining, sanitation, and public transportation. Indoor mobile robots are the most representative of these products, with applications in homes, hotels, restaurants, hospitals, office buildings, and indoor parking lots. A complete robotic system typically includes tasks such as fixed-point grasping, dynamic perception, autonomous navigation and positioning, obstacle avoidance, and path planning. In the autonomous navigation and positioning module, the robot collects, processes, and calculates sensor data to calculate real-time pose and correct accumulated errors, which is a prerequisite for the effective execution of subsequent tasks. Global positioning, as a novel positioning method, uses sensor data to calculate the robot's absolute pose at specific coordinates, effectively solving the problems of initial positioning and loss of the robot during positioning. Furthermore, the ability to obtain the global pose makes it possible to determine whether the robot's trajectory is closed (loop detection), facilitating pose optimization.

[0003] Among existing robot positioning methods, Global Positioning Systems (GPS) and Inertial Navigation Systems (INS) are commonly used in outdoor environments to calculate position and orientation. However, due to the requirement for stable satellite signals, they perform poorly in environments such as elevated highways and alleyways. Multipath effects can also lead to significant positioning errors within densely populated buildings. Indoor positioning, some solutions require the pre-installation of sensor devices or artificial landmarks in specific locations. These include methods based on WiFi positioning, Bluetooth positioning, ultra-wideband positioning (UWB), and radio frequency identification (RFID). Bluetooth and WiFi positioning calculate position information based on the signal return strength from multiple receiving devices. While these methods are relatively low-cost, they suffer from poor positioning accuracy and signal penetration, making it impossible to calculate the robot's orientation. UWB positioning technology determines position based on the time difference between transmitted and received signals. However, this method is limited by the number of base stations. As the number of indoor rooms increases or the number of dynamic targets becomes complex, the number of base stations must be multiplied, increasing system costs. Radio frequency identification (RFID)-based positioning methods use artificial landmarks placed in the scene to achieve positioning, but this method is susceptible to motion. Some methods often utilize various measurement sensors to assist in positioning. Among visual positioning methods, cameras are low-cost and rich in information, but they have a narrow field of view and are susceptible to lighting effects. Among laser positioning methods, LiDAR (Light Detection and Ranging) offers a wide range of measurement, is unaffected by lighting, and has high measurement accuracy, making it more conducive to the deployment and promotion of robotic products. While multi-sensor fusion positioning methods can improve positioning accuracy, they increase system cost, and the correlation and synchronization of multi-source data remain technical challenges. Compared to 2D LiDAR, multi-line LiDAR can perceive three-dimensional target information, which is more conducive to maintaining the stability of the positioning system. However, the increase in data volume also poses greater challenges to the algorithm. Among global positioning methods, a mainstream approach relies on prior maps for positioning. However, as the scene scale increases, the massive amount of map data leads to a sharp decline in map management and positioning efficiency.

[0004] To this end, the present invention proposes a global positioning method for indoor robots based on cylindrical features. The proposed method overcomes the problem that pose calculation in traditional positioning methods requires exogenous signals, artificial markers or relies on initial values, and can be effectively applied to a large range of indoor environments containing cylinders. Since cylindrical buildings are common artificial structures indoors, this method has broad application prospects. Using only lidar as a measurement sensor can effectively control the overall cost of robot products. In addition, the method can effectively overcome the sparsity of low-line-count lidar point clouds and accurately segment cylindrical features. While providing basic elements for the positioning system, it can also provide three-dimensional target information for the perception module. As a novel robot repositioning solution, it can effectively solve the problem of robot kidnapping and improve the stability of the system. Summary of the Invention

[0005] This invention aims to address the problems of high system cost, loss of abduction, and low relocation success rate faced by mobile robots in large-scale indoor environments. It proposes a method that uses a multi-line laser radar mounted on a mobile robot to achieve rapid global positioning using an indoor cylindrical structure.

[0006] To achieve the above objectives, the present invention provides a global positioning method for indoor robots based on cylindrical features, comprising the following steps:

[0007] Step 1: Based on the multi-frame lidar point cloud collected offline, a reference point cloud map of the robot operation scene is stitched based on the laser odometry method;

[0008] Step 2: Extract the cylindrical point cloud in the map based on the geometric model, construct a raster map using the affine transformation matrix, digitally express the key information of the map and save it offline to construct a parametric map;

[0009] Step 3: Based on the symmetry, saliency, and outlier properties of indoor cylindrical structures, cylinders are extracted from the real-time single-frame multi-line lidar point cloud.

[0010] Step 4: When only a single cylinder is detected in the real-time data, the global pose of the robot is calculated based on heuristic search in combination with the grid map data.

[0011] Step 5: When multiple cylinders are detected in the real-time data, the global position of the robot is calculated based on the optimization solution in combination with the grid map data.

[0012] Moreover, the implementation process of step 1 includes the following sub-steps,

[0013] Step 1.1: Use an indoor mobile robot equipped with a multi-line lidar to collect data around a specific scene to ensure that the main features of the scene are fully recorded;

[0014] Step 1.2: Use the laser odometry method (LOAM) to calculate the relative pose between two adjacent point cloud frames, and select key frames to stitch the point cloud map in the same coordinate system.

[0015] In step 1.3, the lidar is set up with the X-axis pointing forward, the Y-axis pointing left, and the Z-axis pointing vertically upward. Based on the relative height of the sensor platform and the room, the height is used to filter out the ground and ceiling 3D points in the map for subsequent parameterized map construction.

[0016] Moreover, the implementation process of step 2 includes the following sub-steps:

[0017] Step 2.1: Based on the cylindrical geometric model, the cylindrical point cloud is segmented cyclically within the prior map.

[0018] Step 2.2, correcting the point cloud map filtered in step 1.3 based on principal component analysis, and generating a rasterized map according to the affine transformation matrix;

[0019] In step 2.3, the map cylindrical geometric model parameters and the position index occupied in the grid map in step 2.2 are expressed in binary.

[0020] Moreover, the implementation process of step 3 includes the following sub-steps:

[0021] Step 3.1: Use height information to approximately remove the ground and roof 3D points in the real-time lidar point cloud, and use principal component analysis to correct the real-time point cloud;

[0022] Step 3.2: Project the corrected real-time point cloud onto the two-dimensional XOY plane while retaining the height information of the corresponding points before correction. Use Euclidean clustering to divide the projected point cloud into multiple point cloud clusters.

[0023] Step 3.3, calculate the height representative information of each cluster and use the height value to eliminate some non-cylindrical clusters;

[0024] In step 3.4, in each of the remaining clusters, a random sampling consensus algorithm is used to fit the arcs, and symmetry constraints and radius thresholds are used to exclude some erroneous arcs.

[0025] Moreover, the implementation process of step 4 includes the following sub-steps:

[0026] Step 4.1: traverse all cylinders in the map in turn, build candidate matches with the real-time single cylinder, use the radius as a constraint, and eliminate some incorrect cylinder matching pairs;

[0027] In step 4.2, in each cylindrical matching pair, the angular resolution is set, and the matching rate between the real-time data and the grid map is used as the objective function to heuristically search for the optimal pose.

[0028] In step 4.3, among all the pose results, find the pose corresponding to the maximum value of the objective function, which is the final result. Set the scoring threshold to determine whether the current positioning result is reliable.

[0029] Moreover, the implementation process of step 5 includes the following sub-steps:

[0030] Step 5.1: traverse all cylinders in the map and real-time data in turn, construct multiple candidate cylinder matching subsets, and use topological and geometric relationships as constraints to eliminate some incorrect cylinder matching pairs;

[0031] Step 5.2: In each cylindrical matching subset, construct an error equation and use the LM algorithm combined with QR decomposition to calculate the pose. The error equation includes point-to-point error and point-to-line error.

[0032] In step 5.3, for each calculated pose, calculate the updated objective function value. Among all positioning results, find the pose corresponding to the maximum value of the objective function as the final result, set the scoring threshold, and judge whether the current positioning result is reliable.

[0033] Moreover, in step 1.3, the positive direction of the laser radar coordinate system is: the X axis is the robot's forward direction, the Y axis is to the left of the forward direction, and the Z axis is vertically upward.

[0034] On the other hand, the present invention also provides an indoor robot global positioning system based on cylindrical features, which is used to implement the indoor robot global positioning method based on cylindrical features as described above.

[0035] Furthermore, the method comprises a processor and a memory, wherein the memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute the above-mentioned global positioning method of an indoor robot based on cylindrical features.

[0036] Alternatively, the method comprises a readable storage medium having a computer program stored thereon, and when the computer program is executed, the method for global positioning of an indoor robot based on cylindrical features as described above is implemented.

[0037] The present invention has the following positive effects:

[0038] 1) This invention addresses the core function of robotic systems: positioning. It utilizes common indoor artificial structures—cylinders—to calculate the position of a robot equipped with a multi-line LiDAR within a global map, addressing the difficulty of indoor robot positioning. It also serves as a repositioning solution, effectively resolving the issue of robot abduction and loss.

[0039] 2) This invention does not rely on artificially erected landmarks or external signals, significantly reducing the cost of the unmanned system. It uses a low-beam multi-line LiDAR sensor, which provides richer three-dimensional information than traditional single-line LiDAR. It also proposes a method for segmenting cylinders in real-time point clouds, improving feature extraction accuracy and ensuring the safety of the robotic system.

[0040] 3) The principle of this invention is simple. Cylinders are common artificial structures and can be used in large, open indoor environments, overcoming the localization challenge in scenes lacking texture features. This invention proposes a novel method for robust localization using structures, a concept easily extendable to other features such as walls, windows, columns, and streetlights. This has significant implications for advancing research in robot localization.

[0041] The present invention can effectively achieve global positioning of robots in large-scale, open indoor scenes, with a maximum speed of less than 100ms, without relying on external signals or artificial markers, etc., reducing the cost of the robot positioning system. It effectively identifies cylindrical features and improves the accuracy of feature extraction, which is of great significance to the perception of robots. This invention solves the positioning problem in open indoor scenes with a lack of feature information. As a repositioning solution, it can solve the problem of robot kidnapping and loss. Therefore, the present invention is of great significance to the research in the field of robotics, ensures the stability of robot products, and promotes the commercialization of the unmanned driving industry. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Flowchart of an embodiment of the present invention.

[0043] Figure 2 Schematic diagram of the cylindrical segmentation result of the global map in an embodiment of the present invention.

[0044] Figure 3 Schematic diagram of symmetry constraints during real-time cylinder segmentation according to an embodiment of the present invention.

[0045] Figure 4 This is a schematic diagram of the results of single-cylinder positioning according to an embodiment of the present invention, wherein part (a) is a schematic diagram of the results of scene #1, part (b) is a schematic diagram of the results of scene #2, and part (c) is a schematic diagram of the results of scene #3.

[0046] Figure 5 This is a schematic diagram of the results of multi-cylinder positioning according to an embodiment of the present invention, wherein part (a) is a schematic diagram of the results of scene #1, part (b) is a schematic diagram of the results of scene #2, and part (c) is a schematic diagram of the results of scene #3. DETAILED DESCRIPTION

[0047] The following provides a detailed description of the technical solution of the present invention in conjunction with the accompanying drawings and embodiments.

[0048] The present invention proposes that, first, complete data is collected in advance around a specific scene, and a global point cloud map is preliminarily spliced. Then, a cylindrical point cloud is segmented within the map based on a geometric model and expressed using simple geometry. A raster map is constructed within the map, and the positions of stable structures in the scene are represented by raster indexes. Furthermore, map-related information is saved as an offline binary file to form a parameterized map. Secondly, the outliers, symmetry, and significance of indoor cylinders are utilized to segment cylinders from sparse real-time lidar point clouds. Finally, two pose calculation strategies are formulated based on the difference in the number of cylinders. The first is a heuristic search based on a single cylinder, and the second is an optimization solution based on multiple cylinders.

[0049] The present invention does not require the use of artificial markers or initial values ​​of posture and can be effectively applied to a wide range of indoor environments including cylinders. Using only lidar as a measurement sensor can effectively control the cost of robot products. By utilizing the characteristics of cylindrical buildings such as significance, symmetry and outliers, it is possible to overcome the sparsity of data and accurately segment cylindrical features. While providing basic elements for the positioning system, it can also provide feature information for the perception system. The positioning results of this invention will not be affected by factors such as trajectory length and running time, effectively ensuring the stability of the robot system. As a novel robot repositioning solution, it can effectively solve the kidnapping problem of indoor robots.

[0050] like Figure 1 As shown, the embodiment of the present invention provides a global positioning method for multiple indoor robots based on cylindrical features, which mainly includes the following steps:

[0051] Step 1: Based on the multi-frame lidar point cloud collected offline, a reference point cloud map of the robot operation scene is constructed based on the laser odometry calculation method.

[0052] Furthermore, the present invention proposes the following implementation method: First, data is collected around the specific scene in which the robot operates, fully recording the key characteristics of the indoor environment. Using the laser odometry method (LOAM), the relative pose between two adjacent point cloud frames is calculated. A key frame is selected every 10 frames, and the relative poses are used to stitch the point cloud map together in the same coordinate system.

[0053] Next, by measuring the height of the robot platform and the center of the LiDAR from the ground and estimating the height of the LiDAR from the ceiling, we set two height thresholds, Z1 and Z2, and retained points whose Z coordinates fell within these thresholds. In this experiment, the LiDAR coordinate system was oriented with the X axis pointing forward, the Y axis pointing left, and the Z axis pointing upward. For other setups, the thresholds can be set based on the vertical coordinate axes.

[0054] In this embodiment, the specific implementation of step 1 is as follows:

[0055] In step 1.1, use an indoor mobile robot equipped with a multi-line lidar to collect data around a specific scene to ensure that the main features of the scene are fully recorded.

[0056] In step 1.2, the Lidar Odometry and Mapping in Real-time (LOAM) method is used to calculate the relative pose between two adjacent point cloud frames. Key frames are selected to create a point cloud map in the same coordinate system. LOAM divides each point cloud frame into line bundles and extracts two types of feature points (edge ​​points and plane points) based on surface smoothness. The smoothness c is calculated as follows:

[0057]

[0058] Where S is the point cloud set, is the coordinates of point i and j in the LiDAR coordinate system {L}, k is the scan number, || is the number of point clouds, and || || is the modulus of the coordinate difference. Then, the pose parameters are calculated based on the least squares method by minimizing the sum of the distances between the two types of feature points, including (a) the point-to-line distance d E :

[0059]

[0060] In the formula and are the coordinates of points i, j, and l in the lidar coordinate system {L}, and k is the scan number.

[0061] (b) Distance d from point to surface P :

[0062]

[0063] Where (j, l, m) is the plane formed by the jth, lth, and mth points, and is the plane segment corresponding to the i-th point.

[0064] In step 1.3, the lidar is typically set up with the X-axis pointing forward, the Y-axis pointing left, and the Z-axis pointing vertically upward. Based on the relative height of the sensor platform and the room, the height is used to filter out 3D ground and ceiling points in the map to facilitate subsequent parametric map construction.

[0065] Preferably, the positive direction of the laser radar coordinate system is: the X axis is the forward direction of the robot, the Y axis is to the left of the forward direction, and the Z axis is vertically upward.

[0066] Step 2: Extract the cylindrical point cloud and rasterized map from the reference point cloud map, and save the geometric model of the cylinder and the index of the position occupied in the raster map as an offline binary file.

[0067] Furthermore, the present invention proposes the following implementation:

[0068] First, based on the geometric model of the cylinder, the RANSAC algorithm is used to segment the cylinder from the current map point cloud. After each segmentation, the segmented cylinder point cloud is removed from the current map. Figure 2 is the cylindrical point cloud segmented from the map, c is the center of the 2D cylinder in the horizontal plane, and p1 and p2 are the two endpoints of the arc corresponding to the real-time 2D cylinder.

[0069] Then, the point cloud map filtered in step 1 is corrected using principal component analysis, and a rasterized map is generated in the current map according to the affine transformation matrix.

[0070] Finally, the cylinder's geometric model (x, y, r) and the index number (row, col) of the grid map's location, along with the number of rows and columns on the grid map, are saved as two binary offline files. (x, y) are the coordinates of the cylinder's center projected onto the horizontal plane, and r is the corresponding radius.

[0071] In this embodiment, the specific implementation of step 2 is as follows:

[0072] Step 2.1: Based on the cylindrical geometric model, segment the cylindrical point cloud in a loop within the prior map. The cylindrical fitting formula is as follows:

[0073] Where k is the number of iterative splits, q k It is a point on the centerline of the cylinder. i is the split cylinder point, × represents the cross product operation, is the direction vector of the cylinder center axis, r k is the radius of the column.

[0074] Step 2.2, based on principal component analysis, correct the point cloud map filtered in step 1.3, according to the affine transformation matrix T f Generate a rasterized map. The principal component analysis formula is as follows:

[0075]

[0076] Where T m is the transformation matrix, R m is the rotation matrix, t m is the translation vector, det() is the determinant, R m(3,3) It is R m The element in row 3 and column 3. m =-R m t c , t c =[x c ,yc ,z c ] T is the centroid of the filtered point cloud map. m =[η1η2η3]∈SO(3), SO(3) represents the Lie group rotation space, η1, η2, η3 are the three eigenvectors obtained by eigenvalue decomposition after constructing the covariance matrix of the filter map.

[0077] Specifically, T m is a 4*4 transformation matrix, R m is a 3*3 rotation matrix, t m is a 3*1 translation vector, det() represents the calculation of the determinant, R m(3,3) Represents the matrix R m The element in row 3 and column 3.

[0078] The affine transformation matrix formula is as follows:

[0079]

[0080] Where I 33 is the 3×3 identity matrix, O 13 is a 1×3 matrix of all zeros, t 31 is the translation component, and Represents the minimum value of the x and y coordinates respectively. is the coordinate of the 3D point of the map after height filtering, g is the grid map resolution, T f is the affine transformation matrix.

[0081] Step 2.3: Express the cylindrical geometric model parameters in step 2.1 and the position index (row and column number) in the grid map in step 2.2 in binary format and save them as an offline file.

[0082] Step 3: Based on the unique symmetry, saliency and outlier properties of cylindrical buildings, cylinders are extracted from the real-time multi-line lidar point cloud.

[0083] Preferably, the RANSAC fitting error is 0.03 meters.

[0084] Preferably, the symmetry constraint threshold is 0.3.

[0085] Preferably, the radius threshold in step 3.4 is 0.2 meters (minimum value) and 2 meters (maximum value).

[0086] Furthermore, the present invention proposes the following implementation:

[0087] First, use height information to initially filter out the ground and ceiling 3D points in the real-time data. To avoid significant data discrepancies affecting the matching results, it is recommended to use the same height threshold parameters as in step 2 whenever possible. Principal component analysis is used to correct the real-time point cloud.

[0088] Secondly, the corrected point cloud data is projected onto the XOY plane while retaining the corresponding height value before correction. The projected point cloud is divided into multiple point cloud clusters using Euclidean clustering, where the point cloud containing the cylinder appears in an arc shape on the plane.

[0089] Then, the representative height of each cluster is calculated, and the height value is used to filter out some non-cylindrical clusters.

[0090] Finally, in each of the remaining clusters, RANSAC is used to fit the arcs, the model fitting error is set to 0.03m, the symmetry constraint threshold is set to 0.3, and some erroneous arcs are excluded by using the symmetry constraint. The minimum radius is set to 0.2m and the maximum radius is set to 2m to filter out some arcs that do not conform to the geometric shape. Figure 3 This is a schematic diagram of symmetry constraints. In the figure, p0 represents the laser radar, p1 and p2 are the endpoints of the arc, p3 is the midpoint of the arc, and c is the center of the arc.

[0091] In this embodiment, the specific implementation of step 3 is as follows:

[0092] In step 3.1, the height information is used to approximately remove the ground and roof 3D points in the real-time lidar point cloud, and the principal component analysis method is used to correct the real-time point cloud.

[0093] In step 3.2, the real-time point cloud after correction is projected onto the two-dimensional XOY plane, while retaining the height information of the corresponding points before correction. The projected point cloud is divided into multiple point cloud clusters (outliers) using Euclidean clustering.

[0094] Step 3.3: Calculate the height representative information of each cluster and use the height value to eliminate some non-cylindrical clusters (significance). The formula for calculating the height of each cluster is as follows:

[0095] h i =max(z i ), j∈C i , p j ∈S h (7)

[0096] Where h i is the i-th cluster C in the original three-dimensional space i Height, z i is the 3D point p in the filtered point cloud j Height, S h It is a highly filtered real-time lidar point cloud.

[0097] In step 3.4, in each of the remaining clusters, the Random Sample Consensus algorithm (RANSAC) is used to fit arcs, and symmetry constraints and radius thresholds are used to exclude some incorrect arcs (symmetry). The symmetry constraint formula is as follows:

[0098]

[0099] Where ω is a symmetric metric, · is the dot product operation, and λ ω is a threshold parameter, p1 and p2 are the two endpoints on both sides of the arc, and → represents the vector formed by the two points.

[0100] In step 4, when only a single cylinder is detected in the real-time data, the global position of the robot is calculated based on a heuristic search using the map cylinder set, the real-time single cylinder, and the rasterized map; otherwise, if multiple cylinders are detected, proceed to step 5.

[0101] Preferably, the radius difference threshold is 1 meter.

[0102] Preferably, the angular resolution is 3 degrees.

[0103] Preferably, the scoring threshold is 0.3.

[0104] Furthermore, the present invention proposes the following implementation method: first, all cylinders in the map are traversed in sequence, and candidate matching pairs are constructed with real-time cylinders. If the radius difference of a cylinder matching pair is greater than 1m, it is considered to be an incorrect matching pair.

[0105] Then, the angular resolution is set to 3 degrees. In each cylindrical matching pair, the matching rate between the filtered real-time point cloud and the grid map is used as the target evaluation function to search for the pose with the highest current score.

[0106] Finally, the pose with the largest score among all pose scores is selected as the final calculation result, and the score threshold is set to 0.3. If the optimal score exceeds the threshold, the current result is considered reliable, otherwise the positioning fails. Figure 4 The positioning results based on a single cylinder are shown in the figure. The positioning results of four random positions are shown in the figure, where (a) is a schematic diagram of the results of scene #1, (b) is a schematic diagram of the results of scene #2, and (c) is a schematic diagram of the results of scene #3.

[0107] In this embodiment, the specific implementation of step 4 is as follows:

[0108] In step 4.1, all cylinders in the map are traversed in turn, candidate matches are constructed with the real-time single cylinder, and the radius is used as a constraint to eliminate some incorrect cylinder matching pairs.

[0109] In step 4.2, in each cylindrical matching pair, set the angle resolution, use the matching rate between real-time data and grid map as the objective function, and heuristically search for the optimal pose. The pose calculation for each cylindrical matching pair and each search angle is as follows:

[0110] T k =T rot T tra (9)

[0111]

[0112]

[0113] Where k is the cylinder matching pair index, T rot represents the rotation transformation, T tra represents the translation transformation, T k is the final transformation matrix, I 2×2 is the 2×2 identity matrix, O 2×2 is a 2×2 matrix of all zeros, is the translation component of the translation transformation, is the translation component of the rotation transformation, is the center coordinate of the real-time cylinder, θ is the coordinate of the center of the map cylinder. j =j*ε,θ j is the angle of the j-th search, and ε is the angular resolution.

[0114] The objective function L is calculated as follows:

[0115]

[0116] In the formula, K1 and K2 are two small positive constants, which are used to ensure that both the numerator and the denominator are greater than 0. m is the map correction matrix, is a rotation transformation, is the translation transformation, M G is a grid map. ⊕ is a counting operation, A⊕B means counting the number of points in B that hit grid A. S h It is a real-time point cloud that filters out the roof and ground. is the number of points it contains.

[0117] Step 4.3, in all pose results, find the objective function L(k,θ j The pose corresponding to the maximum value of ) is the final result. Set the scoring threshold to determine whether the current positioning result is reliable.

[0118] Step 5: When multiple cylinders are detected in the real-time data, the global position of the robot is calculated based on the optimization solution using the map cylinder set and the real-time cylinder set.

[0119] Preferably, the radius difference threshold is 1 meter.

[0120] Preferably, the cylinder center distance difference threshold is 3 meters.

[0121] Furthermore, the present invention proposes the following implementation:

[0122] First, we traverse all cylinders in the map and real-time data, constructing multiple matching subsets. If the radius of a cylinder on the map differs from that in the real-time data by more than 1 meter, it cannot be considered a candidate pair. If the center distance between two cylinders on the map differs from the center distance of a real-time cylinder by more than 3 meters, it cannot be considered a candidate pair. This process then eliminates any incorrect cylinder correspondences.

[0123] Then, in each pair of candidate cylinder subset matching pairs, two types of error equations, point-to-point and point-to-line, are constructed. Since the error equations are nonlinear, the LM algorithm is used to linearize the equation group, and the QR decomposition is used to calculate the pose variable increments. The optimal pose parameters are gradually calculated in a loop, and the corresponding pose score is calculated.

[0124] Finally, all candidate matching subsets are traversed, and the pose with the largest score is selected as the final calculation result among all pose scores. At the same time, the score threshold is set to 0.3. If the optimal score exceeds the threshold, the current result is considered reliable, otherwise the positioning fails. Figure 5 The figure shows the results of multi-cylinder positioning, with data from the same locations as single-cylinder positioning. (a) shows the results for scenario #1, (b) shows the results for scenario #2, and (c) shows the results for scenario #3.

[0125] In this embodiment, the specific implementation of step 5 is as follows:

[0126] In step 5.1, all cylinders in the map and real-time data are traversed in turn to construct multiple candidate cylinder matching subsets. The topological and geometric relationships are used as constraints to eliminate some incorrect cylinder matching pairs.

[0127] In step 5.2, two error equations are constructed for each cylindrical matching subset. The pose is calculated using the Levenberg-Marquardt (LM) algorithm combined with orthogonal triangular decomposition (QR decomposition). The LM algorithm is a conventional gradient descent solution method, and the QR decomposition is a conventional matrix decomposition method, which will not be described in detail in this invention.

[0128] The error equation includes (a) point-to-point error:

[0129]

[0130] (b) Point-to-line error:

[0131]

[0132] Where i, i', i" represent the map cylinder index, j represents the real-time cylinder index, Indicates the coordinates of the center of the map cylinder. Indicates the center coordinates of the real-time cylinder. k' is the cylinder matching index. is the transformation parameter.

[0133] In step 5.3, for each calculated pose, calculate the updated objective function value. Among all positioning results, find the pose corresponding to the maximum value of the objective function L(k') and this is the final result. Set a scoring threshold to determine whether the current positioning result is reliable.

[0134]

[0135] In the formula Replace the formula (12)

[0136] The embodiment selects the indoor first floor lobby (#1), the fourth floor corridor (#2), and the first floor mixed scene (#3) as experimental scenes. A Turtlebot robot equipped with a 16-line laser radar is used to collect 548 frames (#1), 4529 frames (#2), and 1103 frames (#3) of point clouds in the three scenes to test the positioning performance. The experimental results are shown in Tables 1 and 2. In the lobby scene with an average area of ​​about 1500 square meters, the single-cylinder positioning success rate can reach more than 90%, and the average error is about 0.1 meters, which has a good positioning success rate. The positioning error of multi-cylinder positioning in the three scenes is less than 0.1 meters, and the positioning speed is within 200ms, with good positioning speed and positioning accuracy. This method can provide an effective and reliable repositioning solution for indoor robot navigation and positioning.

[0137] Table 1 Single cylinder positioning test performance in different scenarios

[0138]

[0139] Table 2 Multi-cylinder positioning test performance in different scenarios

[0140]

[0141] In specific implementation, the method proposed in the technical solution of the present invention can be automatically run by those skilled in the art using computer software technology. System devices that implement the method, such as computer-readable storage media that store the corresponding computer program of the technical solution of the present invention and computer equipment that runs the corresponding computer program, should also be within the scope of protection of the present invention.

[0142] In some possible embodiments, a global positioning system for an indoor robot based on cylindrical features is provided, comprising a processor and a memory, wherein the memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute the above-described global positioning method for an indoor robot based on cylindrical features.

[0143] In some possible embodiments, a global positioning system for an indoor robot based on cylindrical features is provided, comprising a readable storage medium having a computer program stored thereon. When the computer program is executed, a global positioning method for an indoor robot based on cylindrical features as described above is implemented.

[0144] The specific embodiments described herein are merely illustrative of the spirit of the present invention. Persons skilled in the art may make various modifications, additions, or substitutions to the described specific embodiments without departing from the spirit of the present invention or exceeding the scope of the appended claims.

Claims

1. A global positioning method for indoor robots based on cylindrical features, characterized in that: The following steps are involved: Step 1: Based on the multi-frame lidar point cloud collected offline, a reference point cloud map of the robot operation scene is stitched based on the laser odometry method; Step 2: Extract the cylindrical point cloud in the map based on the geometric model, construct a raster map using the affine transformation matrix, digitally express the key information of the map and save it offline to construct a parametric map; Step 3: Based on the symmetry, saliency, and outlier properties of indoor cylindrical structures, cylinders are extracted from the real-time single-frame multi-line lidar point cloud. Symmetry constraints based on dot product operations and radius thresholds are used to eliminate some erroneous arcs. Step 4: When only a single cylinder is detected in the real-time data, the robot's global pose is calculated based on a heuristic search in combination with the grid map data. This involves setting the angular resolution for each cylinder matching pair and using the matching rate between the real-time data and the grid map as the objective function to heuristically search for the optimal pose. Step 5: When multiple cylinders are detected in the real-time data, the robot's global pose is calculated based on the optimization solution in combination with the grid map data. This includes using topology and geometric relationships as constraints to eliminate some erroneous cylinder matching pairs. For each calculated pose, the updated objective function value is calculated. Among all positioning results, the pose corresponding to the maximum value of the objective function is the final result.

2. The method for global positioning of an indoor robot based on cylindrical features according to claim 1, characterized in that: The implementation process of step 1 includes the following sub-steps: Step 1.1: Use an indoor mobile robot equipped with a multi-line lidar to collect data around a specific scene to ensure that the main features of the scene are fully recorded; Step 1.2: Use the laser odometry method (LOAM) to calculate the relative pose between two adjacent point cloud frames, and select key frames to stitch the point cloud map in the same coordinate system. In step 1.3, the lidar is set up with the X-axis pointing forward, the Y-axis pointing left, and the Z-axis pointing vertically upward. Based on the relative height of the sensor platform and the room, the height is used to filter out the ground and ceiling 3D points in the map for subsequent parameterized map construction.

3. The method for global positioning of an indoor robot based on cylindrical features according to claim 2, characterized in that: The implementation process of step 2 includes the following sub-steps: Step 2.1: Based on the cylindrical geometric model, the cylindrical point cloud is segmented cyclically within the prior map. Step 2.2, correcting the point cloud map obtained by removing the ground and roof three-dimensional points in the map by height filtering in step 1.3 based on principal component analysis, and generating a rasterized map according to the affine transformation matrix; In step 2.3, the map cylindrical geometric model parameters and the position index occupied in the grid map in step 2.2 are expressed in binary.

4. The method for global positioning of an indoor robot based on cylindrical features according to claim 1, characterized in that: The implementation process of step 3 includes the following sub-steps: Step 3.1: Use height information to approximately remove the ground and roof 3D points in the real-time lidar point cloud, and use principal component analysis to correct the real-time point cloud; Step 3.2: Project the corrected real-time point cloud onto the two-dimensional XOY plane while retaining the height information of the corresponding points before correction. Use Euclidean clustering to divide the projected point cloud into multiple point cloud clusters. Step 3.3, calculate the height representative information of each cluster and use the height value to eliminate some non-cylindrical clusters; In step 3.4, in each of the remaining clusters, a random sampling consensus algorithm is used to fit the arcs, and symmetry constraints and radius thresholds are used to exclude some erroneous arcs.

5. The method for global positioning of an indoor robot based on cylindrical features according to claim 1, characterized in that: The implementation process of step 4 includes the following sub-steps: Step 4.1: traverse all cylinders in the map in turn, build candidate matches with the real-time single cylinder, use the radius as a constraint, and eliminate some incorrect cylinder matching pairs; In step 4.2, in each cylindrical matching pair, the angular resolution is set, and the matching rate between the real-time data and the grid map is used as the objective function to heuristically search for the optimal pose. In step 4.3, among all the pose results, find the pose corresponding to the maximum value of the objective function, which is the final result. Set the scoring threshold to determine whether the current positioning result is reliable.

6. The method for global positioning of an indoor robot based on cylindrical features according to claim 1, characterized in that: The implementation process of step 5 includes the following sub-steps: Step 5.1: traverse all cylinders in the map and real-time data in turn, construct multiple candidate cylinder matching subsets, and use topological and geometric relationships as constraints to eliminate some incorrect cylinder matching pairs; Step 5.2: In each cylindrical matching subset, construct an error equation and use the LM algorithm combined with QR decomposition to calculate the pose. The error equation includes point-to-point error and point-to-line error. In step 5.3, for each calculated pose, calculate the updated objective function value. Among all positioning results, find the pose corresponding to the maximum value of the objective function as the final result, set the scoring threshold, and judge whether the current positioning result is reliable.

7. The method for global positioning of an indoor robot based on cylindrical features according to claim 2, characterized in that: In step 1.3, the positive direction of the laser radar coordinate system is: the X axis is the robot's forward direction, the Y axis is to the left of the forward direction, and the Z axis is vertically upward.

8. A global positioning system for indoor robots based on cylindrical features, characterized by: Used to implement a global positioning method for an indoor robot based on cylindrical features as described in any one of claims 1-7.

9. The cylindrical feature-based indoor robot global positioning system according to claim 8, characterized in that: The invention comprises a processor and a memory, wherein the memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute the global positioning method of an indoor robot based on cylindrical features as described in any one of claims 1 to 7.

10. The cylindrical feature-based indoor robot global positioning system according to claim 8, characterized in that: It includes a readable storage medium, on which a computer program is stored. When the computer program is executed, the global positioning method of an indoor robot based on cylindrical features as described in any one of claims 1 to 7 is implemented.

Images