A mobile robot 3D laser SLAM fusion retro-reflective column positioning method

Abstract

The application discloses a mobile robot 3D laser SLAM fusion reflective column positioning method, and relates to the technical field of measurement and positioning, which comprises the following steps: establishing an ordinary point cloud layer and a reflective column positioning layer of an environment point cloud map; extracting local geometric distribution of the ordinary point cloud layer, center point sequence and adjacent distance relationship of the reflective column positioning layer, and generating a real-time positioning reference set; adopting the real-time positioning reference set to perform reflective column positioning preprocessing on a current frame three-dimensional laser point cloud, extracting a reflective column center point track and dividing ordinary environment point clouds, and constructing current frame double-layer point cloud data and a reflective column topology graph. According to the application, the reflective column is converted from a single high reflection point into a positioning reference with adjacent distance relationship and continuous frame appearance sequence, so that the interference of isolated high reflection noise, misrecognized reflective points and incorrect corresponding relationship on the positioning result is effectively reduced.

Description

Technical Field

[0001] This invention relates to the field of measurement and positioning technology, and in particular to a method for positioning mobile robots using 3D laser SLAM fusion reflective pillars. Background Technology

[0002] With the expanding applications of mobile robots in warehousing and logistics, intelligent manufacturing, inspection and maintenance, and indoor-outdoor composite scenarios, 3D laser SLAM is gradually becoming an important technical route for autonomous localization and mapping of mobile robots. Current 3D laser localization typically relies on lidar to collect environmental point clouds. Through point cloud feature extraction, rasterized map representation, normal distribution transformation registration, or iterative nearest-point registration, the current frame's point cloud is matched with the environmental point cloud map to obtain the robot's position and orientation in the map coordinate system. For scenarios with rich structural features, fixed structures such as walls, floors, columns, and equipment outlines can provide relatively stable geometric constraints for point cloud registration, enabling the mobile robot to continuously obtain real-time poses that can be used for navigation and control.

[0003] However, in scenarios with long corridors, open areas, repetitive structures, or insufficient local environmental geometry, ordinary environmental point clouds often only provide geometric constraints in limited directions. Especially in parallel walls, continuous planes, or areas with repetitive structures, point cloud registration is prone to problems such as drift along the direction of travel, difficulty in lateral offset convergence, and unstable attitude constraints. Existing schemes that use highly reflective markers to assist localization often add highly reflective points as ordinary feature points to the matching process, failing to fully distinguish between stable constraint directions and insufficient constraint directions of ordinary point clouds. They also fail to utilize the sequence of reflective pillar center points, adjacent distance relationships, and continuous frame trajectory relationships to form topologically consistent localization constraints. As a result, reflective pillar features are easily affected by misidentified points, isolated highly reflective noise, or incorrect correspondences in complex scenes, making it difficult to stably improve the reliability of 3D laser SLAM localization. Summary of the Invention

[0004] In view of the aforementioned existing problems, the present invention is proposed.

[0005] Therefore, this invention provides a 3D laser SLAM fusion reflective column localization method for mobile robots to solve the problem of 3D laser SLAM localization drift of mobile robots when the geometric constraints of ordinary point clouds are insufficient.

[0006] To solve the above-mentioned technical problems, the present invention provides the following technical solution:

[0007] This invention provides a 3D laser SLAM fusion reflective column localization method for mobile robots, comprising,

[0008] Establish a general point cloud layer and a reflective column positioning layer for the environmental point cloud map. Extract the local geometric distribution of the general point cloud layer, the center point sequence and adjacent distance relationship of the reflective column positioning layer, and generate a real-time positioning reference set.

[0009] The real-time positioning reference set is used to perform reflector column localization preprocessing on the current frame's 3D laser point cloud, extract the trajectory of the center point of the reflector column and divide the ordinary environment point cloud, and construct the current frame's two-layer point cloud data and reflector column topology map;

[0010] The normal distribution transformation registration is performed on the ordinary environment point cloud and the ordinary point cloud layer in the current frame dual-layer point cloud data. Then, the pose information matrix feature decomposition, maximum consistent subgraph matching and topological residual orientation embedding are combined with the reflection column topology map to generate the pose correction amount of the current frame.

[0011] Based on the pose correction amount of the current frame, convert the coordinates of the current frame's two-layer point cloud data, and output the robot's real-time pose and the pose reference for the next frame;

[0012] The trajectory of the center point of the reflector is corrected by the pose reference of the next frame, and the trajectory of the center point of the reflector that satisfies the adjacent distance relationship is constructed as the key frame index of the reflector to generate the localization result of the mobile robot.

[0013] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflective column localization method described in this invention, wherein: the generation of the real-time localization reference set specifically includes:

[0014] Load the environmental point cloud map, and divide the fixed structure point cloud of the environment into the ordinary point cloud layer, and divide the high reflectivity points of the reflective pillars into the reflective pillar positioning layer;

[0015] The ordinary point cloud layer is processed according to the grid scale of the normal distribution transformation registration, and the local geometric distribution of the point cloud in each grid is calculated to obtain the local geometric distribution set of the ordinary point cloud layer.

[0016] Extract the center point of each reflector in the reflector positioning layer, arrange the center points according to the spatial order in the map coordinate system, and calculate the distance between adjacent center points to obtain the reflector center point sequence and adjacent distance relationship;

[0017] The initial frame 3D laser point cloud is registered with the local geometric distribution set of the ordinary point cloud layer by performing a normal distribution transformation to obtain the initial pose.

[0018] By unifying the local geometric distribution set, the sequence of reflective pillar center points, the adjacent distance relationship, and the initial pose into the same map coordinate system, a real-time positioning reference set is generated.

[0019] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflector positioning method of the present invention, the reflector positioning preprocessing specifically includes:

[0020] The initial pose and ordinary point cloud layer in the real-time positioning reference set are used to perform pose pre-alignment of the current frame's 3D laser point cloud to obtain the pre-aligned 3D laser point cloud.

[0021] The continuity of reflection intensity, spatial morphology of point clusters, and beam angle distribution of pre-aligned 3D laser point clouds are detected to obtain reflective column point clusters;

[0022] Extract the center point of the reflective column cluster and match the center point with the trajectory of the center point in the previous frame to obtain the trajectory of the reflective column center point;

[0023] The reflective column clusters are separated from the pre-aligned 3D laser point cloud, and the separated point cloud is retained as a normal environmental point cloud.

[0024] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflector localization method described in this invention, the construction of the current frame's dual-layer point cloud data and reflector topology map specifically includes:

[0025] The ordinary environment point cloud, the trajectory of the center point of the reflector, and the initial pose are unified to the current frame point cloud coordinate system to obtain the current frame double-layer point cloud data with pose reference.

[0026] The center point in the trajectory of the center point of the reflective column is recorded as the topological node. The adjacent nodes are determined according to the spatial proximity order between the center points. The distance between the adjacent nodes is calculated, and the order in which the adjacent nodes appear in consecutive frames is recorded.

[0027] The topology map of reflective pillars in the current frame is constructed by topological nodes, adjacent nodes, adjacent distances, and the order of occurrence of consecutive frames.

[0028] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflective column localization method described in this invention, the normal distribution transformation registration specifically includes:

[0029] Select the ordinary environment point cloud in the current frame of dual-layer point cloud data, and transform the ordinary environment point cloud to the corresponding raster of the ordinary point cloud layer according to the pose reference in the current frame of dual-layer point cloud data.

[0030] Local probability distribution parameters are established using the mean and covariance matrix of the point cloud within the corresponding grid, and the geometric matching residual between the ordinary environment point cloud and the local geometric distribution within the corresponding grid is calculated based on the local probability distribution parameters.

[0031] By using the normal distribution transformation evaluation function of the ordinary environment point cloud, the coordinate transformation relationship of the ordinary environment point cloud is iteratively adjusted along the translation and attitude components of the robot pose to obtain the ordinary point cloud registration result.

[0032] The pose increment and geometric matching residual are extracted from the ordinary point cloud registration results to generate an ordinary point cloud registration constraint set.

[0033] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflector positioning method of the present invention, wherein: the pose information matrix feature decomposition specifically includes:

[0034] The influence relationship between each translation component and attitude component on the geometric matching residual is extracted using a common point cloud registration constraint set, and the pose information matrix of the current frame is constructed.

[0035] Perform eigenvalue decomposition on the pose information matrix of the current frame to obtain the feature direction and feature intensity corresponding to each pose component;

[0036] Arrange the feature directions in descending order of feature intensity, calculate the difference between adjacent feature intensities, and use the adjacent position with the largest difference as the position of the pose component.

[0037] The feature directions before the pose components are divided into positions are labeled as pose components that can be stably constrained by ordinary point clouds, and the feature directions after the pose components are divided into positions are labeled as pose components that are not sufficiently constrained by ordinary point clouds.

[0038] Establish component correspondences between the pose components that can be stably constrained by ordinary point clouds, the pose components that are not sufficiently constrained by ordinary point clouds, the registration results of ordinary point clouds, and the topology map of the reflector pillars in the current frame, and generate pose component segmentation results.

[0039] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflective column localization method described in this invention, wherein: the maximum consistent subgraph matching specifically includes:

[0040] Extract the topological nodes, adjacent distance relationships, and consecutive frame occurrence order from the current frame's reflective column topology map, and extract the center point sequence and adjacent distance relationships from the reflective column positioning layer;

[0041] By mapping the topological nodes in the current frame's reflective column topology map to the sequence of center points in the reflective column positioning layer, candidate reflective column corresponding point pairs are obtained.

[0042] Compare the adjacent distance relationship, arrangement order and continuous frame trajectory relationship between the corresponding point pairs of candidate reflective pillars, and delete the corresponding point pairs of candidate reflective pillars that do not meet the consistency relationship;

[0043] A consistent reflection column correspondence graph is constructed using the retained candidate reflection column correspondence point pairs. The subgraph with the largest number of nodes and consistent adjacency relationship is selected from the consistent reflection column correspondence graph to obtain the maximum reflection column correspondence subgraph.

[0044] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflective column localization method of the present invention, wherein: the topological residual orientation embedding specifically includes:

[0045] The correspondence between the center point of the reflective column in the current frame and the center point of the reflective column positioning layer is determined by using the sub-map corresponding to the maximum reflective column, and the positional deviation of the corresponding center point in the map coordinate system is calculated to obtain the topological residual of the reflective column.

[0046] The center point of the reflection column in the sub-image corresponding to the maximum reflection column is used as a high reflection registration term and incorporated into the normal distribution transformation evaluation function of the ordinary environmental point cloud to obtain the normal distribution transformation evaluation function of the fused reflection column.

[0047] The topological residual of the reflector is mapped to the pose component of the ordinary point cloud with insufficient constraints according to the result of the pose component partitioning, so as to obtain the under-constrained direction reflector residual.

[0048] The pose correction components that can be stably constrained by ordinary point cloud registration results are extracted, and the pose correction components that are not constrained by ordinary point cloud are extracted by under-constrained directional reflector residuals, thus obtaining the quantized pose correction relationship.

[0049] By utilizing the fractional pose correction relationship and the normal distribution transformation evaluation function of the fused reflector, the geometric matching residual of the ordinary environmental point cloud and the topological residual of the reflector center point are corrected simultaneously, and the pose change of the current frame is determined to generate the pose correction amount of the current frame.

[0050] As a preferred embodiment of the 3D laser SLAM fusion reflector positioning method for mobile robots described in this invention, the output of the robot's real-time pose and next-frame pose reference specifically includes:

[0051] The coordinates of the current frame's two-layer point cloud data are transformed using the current frame pose correction value to obtain the current frame aligned point cloud in the map coordinate system;

[0052] The robot's position and pose in the map coordinate system are extracted from the point cloud aligned with the current frame to obtain the robot's real-time pose.

[0053] By mapping the robot's real-time pose to the trajectory of the center point of the reflector in the current frame, the positioning trajectory record of the current frame is obtained, and the pose reference for the next frame is determined by the positioning trajectory record of the current frame.

[0054] As a preferred embodiment of the mobile robot 3D laser SLAM fusion reflective column localization method of the present invention, wherein: generating the mobile robot localization result specifically includes:

[0055] The position of the center point of the reflective column in the next frame of the 3D laser point cloud is corrected by using the pose reference of the next frame, and the corrected trajectory of the center point of the reflective column is obtained.

[0056] Compare the corrected trajectory of the center point of the reflective column with the adjacent distance relationship in the reflective column positioning layer, and filter the center point trajectories of the reflective columns that satisfy the adjacent distance relationship;

[0057] An index is created based on the correspondence between frame number, robot real-time pose, reflector center point sequence, and adjacent distance relationship to obtain the reflector keyframe index;

[0058] From the keyframe index of reflectors, select the index item that has the smallest position distance to the pose reference of the next frame in the map coordinate system and contains the trajectory of the center point of the continuous reflectors. Extract the robot's real-time pose and the sequence of center points of the reflectors corresponding to the index item to obtain the subsequent local registration reference frame.

[0059] The subsequent local registration reference frames are used to continue participating in the eigenvalue decomposition of the pose information matrix and the underconstrained directional projection normal distribution transformation registration of the reflector pillars to generate the mobile robot localization results.

[0060] The beneficial effects of this invention are as follows: By unifying the local geometric distribution of ordinary point cloud layers with the center point sequence and adjacent distance relationships of reflective pillar positioning layers into the same map coordinate system, the positioning of mobile robots no longer relies solely on ordinary environmental point clouds such as wall surfaces, ground surfaces, and pillar outlines. Instead, it forms a real-time positioning reference involving both ordinary structural constraints and high-reflectivity reflective pillar constraints, improving the reliability of the positioning reference in repetitive structures, open areas, and long corridor scenarios. Through reflective pillar cluster identification, center point trajectory correspondence, and reflective pillar topology map construction, reflective pillars are transformed from discrete high-reflectivity points into stable positioning references with spatial order, adjacent distances, and continuous frame trajectory relationships, reducing the impact of isolated high-reflectivity noise and erroneous reflective points on pose solving. Furthermore, by differentiating features through pose information matrix eigenvalue decomposition... This approach combines stable and constrained pose components from ordinary point clouds with those that are insufficiently constrained. It integrates the high-reflectivity registration term from the subgraph corresponding to the largest reflector into the normal distribution transformation evaluation function, enabling ordinary environmental point cloud registration and reflector topological residuals to work synergistically in the same pose solution process, rather than simply superimposing two types of features. Through topological residual orientation embedding, the deviation of the reflector center point can effectively compensate for pose directions that are difficult to constrain with ordinary point clouds, reducing the risk of robot drift along the travel direction and lateral offset accumulation. Furthermore, by using reflector keyframe indexing to provide continuous reference for subsequent local registration, the localization process can remain continuous even when reflectors are temporarily invisible or environmental geometric features are insufficient, thereby improving the stability, accuracy, and engineering applicability of the mobile robot's 3D laser SLAM localization results. Attached Figure Description

[0061] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0062] Figure 1 A flowchart for a 3D laser SLAM fusion reflective column localization method for mobile robots.

[0063] Figure 2 A schematic diagram illustrating the construction of a real-time positioning reference set and a two-layer point cloud data of the current frame.

[0064] Figure 3 This is a schematic diagram of pose correction by fusing ordinary point cloud constraints with topological residuals of reflective pillars.

[0065] Figure 4 This is a schematic diagram of the keyframe index and continuous positioning update of the reflective column. Detailed Implementation

[0066] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0067] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0068] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0069] Reference Figures 1-4 As one embodiment of the present invention, this embodiment provides a 3D laser SLAM fusion reflective column localization method for mobile robots, including the following steps:

[0070] S1. Establish the ordinary point cloud layer and reflective column positioning layer of the environmental point cloud map, extract the local geometric distribution of the ordinary point cloud layer, the center point sequence and adjacent distance relationship of the reflective column positioning layer, and generate a real-time positioning reference set.

[0071] S1.1. After the robot initiates 3D laser localization, it loads the environmental point cloud map. Wall points, ground points, and pillar outline points in the environmental point cloud map are assigned to the ordinary point cloud layer, while the high-reflectivity points corresponding to the reflective pillars are assigned to the reflective pillar localization layer, resulting in the ordinary point cloud layer and the reflective pillar localization layer. Both the ordinary point cloud layer and the reflective pillar localization layer are maintained in the same map coordinate system. The ordinary point cloud layer is used for normal distribution transformation registration, while the reflective pillar localization layer provides the reflective pillar center point sequence and adjacent distance relationships.

[0072] Among them, the ordinary point cloud layer corresponds to the ordinary fixed structure points in the environmental point cloud map, and the reflective column positioning layer corresponds to the high reflectivity points in the environmental point cloud map. The ordinary fixed structure points and high reflectivity points are classified and marked after the environmental point cloud map is loaded, so that the 3D laser point cloud of the current frame can establish a correspondence with the ordinary point cloud layer and the reflective column positioning layer respectively.

[0073] The ordinary point cloud layer is processed according to the raster scale of normal distribution transformation registration. The spatial extent of the ordinary point cloud layer is determined in the map coordinate system, and the minimum and maximum coordinate values ​​in the horizontal, vertical, and height directions are obtained. Using the raster side length adopted in normal distribution transformation registration as the 3D raster scale, the ordinary point cloud layer is divided into continuously arranged 3D rasters along the horizontal, vertical, and height directions, and a raster number is assigned to each 3D raster. For each point in the ordinary point cloud layer, the 3D raster to which the point belongs is determined based on its horizontal, vertical, and height coordinates in the map coordinate system, and the corresponding point is assigned to the corresponding 3D raster. For points falling on the boundary of two adjacent 3D rasteres, a unique 3D raster is determined according to the actual orientation of the point's coordinates in the map coordinate system to avoid the same point repeatedly participating in local geometric distribution calculations. The 3D raster scale corresponds to the target point cloud voxel grid scale in normal distribution transformation registration, with the ordinary point cloud layer serving as the target point cloud distribution, and the current frame's 3D laser point cloud serving as the source point cloud for subsequent registration.

[0074] After assigning points to grid cells, the number of points in each 3D grid is counted. For 3D grids with enough points to support covariance matrix calculation, the point cloud mean and covariance matrix are calculated. For 3D grids with insufficient points to support covariance matrix calculation, their point clouds are merged into spatially adjacent 3D grids belonging to the same ordinary fixed structure for calculation. If the calculation is still insufficient after merging, the corresponding 3D grid does not participate in the local geometric distribution set. The grid number, point cloud mean, covariance matrix, and number of points of each 3D grid participating in the calculation are saved together to obtain the local geometric distribution set of the ordinary point cloud layer.

[0075] For any 3D raster, normal distribution transformation registration uses the point cloud mean to represent the center of the point cloud within the 3D raster and the covariance matrix to represent the distribution direction of the point cloud within the 3D raster. The expression is:

[0076] ;

[0077] ;

[0078] in, Indicates the first A set of ordinary points within a three-dimensional grid Indicates the first Within the nth three-dimensional grid A regular point, Indicates the first The number of ordinary points within a 3D grid Indicates the first Mean value of point cloud within a 3D raster Indicates the first The covariance matrix of point clouds within a 3D raster This indicates the transpose operation, used to convert the point cloud deviation vector into the matrix product form required for covariance matrix calculation.

[0079] The point cloud mean and covariance matrix correspond to the multidimensional normal distribution parameters within each 3D raster in the normal distribution transformation registration. The point cloud mean is used to determine the local probability distribution center, and the covariance matrix is ​​used to determine the local point cloud surface morphology.

[0080] When the number of points in a 3D raster is insufficient to calculate the covariance matrix, the points in the 3D raster are incorporated into adjacent 3D rasters to participate in the local geometric distribution calculation; if the conditions for calculating the covariance matrix are still not met, the corresponding 3D raster is not included in the local geometric distribution set of the ordinary point cloud layer.

[0081] S1.2. Extract the high-reflectivity point clusters of each reflector in the reflector positioning layer, calculate the center point of each high-reflectivity point cluster, and arrange them according to the spatial order of the center points in the map coordinate system to obtain the reflector center point sequence. The high-reflectivity point clusters in the reflector positioning layer are derived from the high-reflectivity points that have been classified and marked in the environmental point cloud map, and the high-reflectivity point clusters corresponding to the same reflector are used as a positioning reference unit.

[0082] The center point of the reflector column is determined by the average coordinates of all points within the same cluster of highly reflective points on the reflector column, expressed as:

[0083] ;

[0084] in, Indicates the first A cluster of highly reflective points on the reflective pillars Indicates the first The first high-reflectivity point cluster within the first reflective column A high-reflectivity point, Indicates the first The number of highly reflective points within a cluster of highly reflective points on a reflective column. Indicates the first The center points of each reflector pillar. The adjacent center points in the reflector pillar center point sequence are determined by their spatial distance in the map coordinate system, expressed as:

[0085] ;

[0086] in, Represents the first in the sequence of center points of the reflector pillars One central point, Represents the first in the sequence of center points of the reflector pillars One central point, Indicates the first The center point and the first The distance between the center points.

[0087] S1.3. The robot acquires an initial frame of 3D laser point cloud at locations where it can scan non-coplanar fixed structures within the ordinary point cloud layer. The initial frame of 3D laser point cloud is then registered with the local geometric distribution set of the ordinary point cloud layer using a normal distribution transformation to obtain the initial pose of the initial frame of 3D laser point cloud relative to the environment point cloud map. Non-coplanar fixed structures include ground points, wall points, and column contour points. When the initial frame of 3D laser point cloud can cover these non-coplanar fixed structures, the normal distribution transformation registration can provide stable geometric constraints for the robot's initial pose.

[0088] Normal distribution transformation registration uses the initial frame 3D laser point cloud as the source point cloud and the local geometric distribution set of the ordinary point cloud layer as the target point cloud distribution. It adjusts the translation and attitude of the initial frame 3D laser point cloud in the map coordinate system so that the initial frame 3D laser point cloud and the local geometric distribution of the ordinary point cloud layer reach the registration state and obtain the initial pose.

[0089] When the initial frame 3D laser point cloud only scans parallel wall points and causes the registration result to be unstable, the robot continues to collect the initial frame 3D laser point cloud at positions where ground points, wall points, and column outline points can be scanned, until the initial frame 3D laser point cloud can complete normal distribution transformation registration with the ordinary point cloud layer.

[0090] The local geometric distribution set of the ordinary point cloud layer, the sequence of reflective column center points, adjacent distance relationships and initial poses of the reflective column positioning layer are unified into the same map coordinate system to generate a real-time positioning reference set.

[0091] S2. Use a real-time positioning reference set to perform reflector column positioning preprocessing on the current frame's 3D laser point cloud, extract the trajectory of the reflector column's center point, divide the ordinary environment point cloud, and construct the current frame's two-layer point cloud data and reflector column topology map.

[0092] S2.1. During the robot's movement, it collects the current frame's 3D laser point cloud, calls the initial pose and ordinary point cloud layer from the real-time positioning reference set, and performs pose pre-alignment between the current frame's 3D laser point cloud and the ordinary point cloud layer to obtain the pre-aligned 3D laser point cloud.

[0093] Pose pre-alignment is the process of converting the current frame's 3D laser point cloud to the coordinate range corresponding to the environmental point cloud map according to the initial pose, so that the ordinary fixed structure points in the current frame's 3D laser point cloud and the local geometric distribution in the ordinary point cloud layer are located in a registerable spatial position.

[0094] The continuity of reflection intensity is detected for the pre-aligned 3D laser point cloud. Specifically, the laser points in the pre-aligned 3D laser point cloud are arranged according to the same scanning direction of the lidar to form a reflection intensity sequence corresponding to each scanning direction. In each reflection intensity sequence, continuous point segments are found where the reflection intensity increases from the level of ordinary environment points, maintains a continuous high reflection state, and then falls back to the level of ordinary environment points. Laser points with adjacent spatial coordinates, continuous reflection intensity changes, and no breaks in the reflection intensity of the continuous point segments are grouped into the same high reflection continuous point segment. High reflection continuous point segments with adjacent spatial positions in different scanning directions are merged to obtain candidate high reflection point clusters.

[0095] Among them, the continuity detection of reflection intensity is consistent with the classification label of high reflectivity points in the environmental point cloud map, and the candidate high reflectivity point clusters correspond to the set of high reflectivity points in the current frame's 3D laser point cloud that may belong to the reflective pillar.

[0096] The candidate high-reflectivity point clusters are examined for spatial morphology. Specifically, the horizontal span, vertical span, and height span of the candidate high-reflectivity point clusters in the current frame point cloud coordinate system are calculated, and these spans are compared with the corresponding spans of the high-reflectivity point clusters in the reflective pillar positioning layer. When the candidate high-reflectivity point clusters have a point cloud distribution that extends continuously along the height direction, and the horizontal and vertical spans are consistent with those of the high-reflectivity point clusters in the reflective pillar positioning layer, the candidate high-reflectivity point clusters are retained. When the candidate high-reflectivity point clusters exhibit sheet-like reflections, isolated point reflections, or spatial morphology inconsistent with those of the high-reflectivity point clusters in the reflective pillar positioning layer, the candidate high-reflectivity point clusters are deleted.

[0097] The retained candidate high-reflectivity point clusters are subjected to beam angle distribution verification. Specifically, the beam number and beam angle corresponding to each laser point in the retained candidate high-reflectivity point clusters are extracted. The candidate high-reflectivity point clusters are checked according to the beam number order to see if they are formed by continuous scanning of adjacent beams. When the candidate high-reflectivity point clusters are continuously distributed on adjacent beam numbers and the laser points corresponding to adjacent beams fall within the same outer contour range of the reflective column in space, the candidate high-reflectivity point clusters are confirmed as reflective column point clusters. When the candidate high-reflectivity point clusters are only distributed on a single beam number, or the laser points corresponding to adjacent beams cannot fall within the same outer contour range of the reflective column in space, the candidate high-reflectivity point clusters are deleted.

[0098] After continuous reflection intensity detection, spatial morphology verification of point clusters, and beam angle distribution verification, the retained candidate high reflectivity point clusters are used as reflective pillar point clusters. When the pre-aligned 3D laser point cloud does not form reflective pillar point clusters, all laser points in the pre-aligned 3D laser point cloud are used as ordinary environmental point clouds to participate in subsequent normal distribution transformation registration, and the trajectory of the center point of the reflective pillar maintains the existing result of the center point trajectory of the previous frame in the current frame.

[0099] S2.2. Extract the center point of the reflective column cluster and match it with the trajectory of the center point in the previous frame to obtain the reflective column center point trajectory. The center point of the reflective column cluster is determined by the average of the three-dimensional coordinates of all laser points within the same reflective column cluster. When the current frame is the first frame after initial positioning, the center point of the reflective column cluster is used as the starting point of the trajectory of the center point in the previous frame. When the current frame has the trajectory of the center point in the previous frame, the center point of the reflective column cluster in the current frame is matched with the trajectory of the center point in the previous frame according to the spatial position of the center point in the current frame point cloud coordinate system, the order of appearance of the center point in consecutive frames, and the distance relationship between adjacent center points to obtain the reflective column center point trajectory. The reflective column center point trajectory includes the record of the position change and the record of the order of appearance of the same reflective column center point in consecutive frames.

[0100] Separate the reflective column point clusters from the pre-aligned 3D laser point cloud, and retain the separated point cloud as the ordinary environment point cloud. Specifically, remove the laser points that have been included in the reflective column point clusters from the pre-aligned 3D laser point cloud, and retain the wall points, ground points, column outline points and other ordinary fixed structure points to obtain the ordinary environment point cloud. The ordinary environment point cloud and the trajectory of the center point of the reflective column are kept in the same current frame point cloud coordinate system.

[0101] S2.3. Unify the ordinary environment point cloud, the trajectory of the center point of the reflector, and the initial pose to the current frame point cloud coordinate system to obtain the current frame double-layer point cloud data.

[0102] The center point in the trajectory of the reflective column is recorded as the topological node. The adjacent nodes are determined according to the spatial proximity between the center points. The distance between the adjacent nodes is calculated, and the order in which the adjacent nodes appear in consecutive frames is recorded.

[0103] Among them, the topology node is the center point of the reflector that has formed the trajectory of the center point of the reflector in the current frame, the adjacent node is the topology node that is adjacent in spatial order, the adjacent distance is the spatial distance between adjacent nodes in the point cloud coordinate system of the current frame, and the order of appearance of consecutive frames is the order in which the same topology node appears in the current frame and the previous frame.

[0104] The current frame's reflector topology map is constructed by using topological nodes to represent the center point of the reflector, the connection relationship between adjacent nodes to represent the spatial proximity relationship between the center points of the reflector, the adjacent distance to represent the geometric constraints between the center points of the reflector, and the order of appearance of consecutive frames to represent the temporal continuity of the trajectory of the center point of the reflector.

[0105] S3. Perform normal distribution transformation and registration on the ordinary environment point cloud and the ordinary point cloud layer in the current frame's dual-layer point cloud data, and combine the reflection column topology map to perform pose information matrix feature decomposition, maximum consistent subgraph matching and topological residual orientation embedding to generate the pose correction amount for the current frame.

[0106] S3.1. Select ordinary environment point clouds from the current frame's two-layer point cloud data. Transform the ordinary environment point clouds to the map coordinate system of the environment point cloud map according to the pose reference in the current frame's two-layer point cloud data. Then, assign each point of the transformed ordinary environment point cloud to the corresponding grid in the ordinary point cloud layer. The corresponding grid is the 3D grid in which the laser points in the ordinary environment point cloud fall in the map coordinate system. The mean and covariance matrix of the point cloud are stored in the corresponding grid. For laser points that do not fall into the effective 3D grid of the ordinary point cloud layer, the ordinary environment point cloud retains the coordinates of the corresponding laser points, but the corresponding laser points do not participate in the normal distribution transformation registration of the current frame.

[0107] After laser points in the ordinary environment point cloud are assigned to their corresponding grids, the mean and covariance matrix of the points within the corresponding grids in the ordinary point cloud layer are used as local probability distribution parameters for normal distribution transformation and registration in the current frame. Based on these local probability distribution parameters, the local geometric distribution within the corresponding grid is represented as a multidimensional normal distribution. The probability density of the transformed ordinary environment point cloud in the multidimensional normal distribution is used to describe the degree of matching between the ordinary environment point cloud and the ordinary point cloud layer. For any position within the corresponding grid... The probability density function is expressed as:

[0108] ;

[0109] in, Indicates the first A three-dimensional grid at position The probability density at that location, This represents any three-dimensional position within the corresponding grid. Indicates the first Mean value of point cloud within a 3D raster Indicates the first The covariance matrix of point clouds within a 3D raster Indicates the first The determinant of the covariance matrix of a point cloud within a 3D raster. Indicates the first The inverse matrix of the covariance matrix of point clouds within a 3D raster. Represents pi (π). This represents the natural exponential function.

[0110] After obtaining the probability density function of the corresponding grid, the deviation of the laser points in the ordinary environment point cloud from the geometric distribution of the corresponding grid is further calculated to obtain the geometric matching residual. The geometric matching residual is calculated using the Mahalanobis distance residual in normal distribution transformation registration, and the expression is:

[0111] ;

[0112] in, Indicating the first point cloud in a normal environment The laser point's pose vector in the current frame Geometric matching residuals under the following conditions This indicates the index of the laser point in the ordinary environmental point cloud that participated in the normal distribution transformation registration. This represents the pose vector of the current frame, which includes the robot's translation and attitude components in the map coordinate system. Indicates the first Each laser point is positioned according to the pose vector of the current frame. Transformed map coordinates Indicates the first The corresponding grid number to which each laser point belongs. This represents the mean value of the point cloud within the corresponding raster. This represents the covariance matrix of the point cloud within the corresponding raster. This represents the inverse matrix of the covariance matrix of the point cloud within the corresponding raster.

[0113] S3.2. Adjust the coordinate transformation relationship of the ordinary environment point cloud along the translation and attitude components of the robot pose, and continuously calculate the geometric matching residuals to make the wall points, ground points, and column contour points in the ordinary environment point cloud reach a normal distribution transformation registration state with the local geometric distribution of the ordinary point cloud layer, thus obtaining the ordinary point cloud registration result. The ordinary point cloud registration result includes the pose increment of the ordinary environment point cloud in the current frame relative to the ordinary point cloud layer and the geometric matching residuals of each laser point involved in the registration. The pose increment and the geometric matching residuals together form the ordinary point cloud registration constraint set.

[0114] After the geometric matching residual calculation is completed for the ordinary environmental point cloud, the normal distribution transformation registration obtains the ordinary point cloud registration result by evaluating the score of the ordinary environmental point cloud in the probability density field of the ordinary point cloud layer under the rigid body transformation.

[0115] For point cloud assemblies in ordinary environments The normal distribution transformation registration evaluation function is:

[0116] ;

[0117] in, Represents rigid body transformation The corresponding normal distribution transformation registration score, This represents the rigid body transformation of the current frame's normal environment point cloud relative to the normal point cloud layer. This represents the set of point clouds in the normal environment of the current frame. This indicates the first element in the current frame's ordinary environment point cloud set. A typical environmental point. Indicates the first A normal environment point undergoes rigid body transformation The subsequent 3D raster number, Indicates the first The probability density function corresponding to a 3D grid.

[0118] To facilitate iterative solutions using the Gauss-Newton method, the normal distribution transformation registration score is converted into a negative log-likelihood form, and constant terms irrelevant to pose determination are omitted, resulting in the NDT evaluation function for ordinary environmental point clouds:

[0119] ;

[0120] in, This represents the NDT evaluation function for point clouds in a normal environment. Indicates the first The average point cloud value within a 3D raster of a typical environmental point. Indicates the first The inverse matrix of the covariance matrix of the point cloud within the 3D raster to which each ordinary environmental point belongs.

[0121] After the NDT evaluation function of the ordinary environment point cloud achieves convergence, the pose increment and geometric matching residual of the ordinary environment point cloud in the current frame relative to the ordinary point cloud layer are extracted to generate the ordinary point cloud registration constraint set.

[0122] The influence of each translation and pose component on the geometric matching residual is extracted using a standard point cloud registration constraint set, and the current frame pose information matrix is ​​constructed. This matrix is ​​derived from the Gauss-Newton normal equations in the least squares localization solution. The current frame pose information matrix represents the strength of the constraints imposed by the standard environment point cloud on each pose component, and its expression is:

[0123] ;

[0124] in, This represents the pose information matrix for the current frame. This represents the set of ordinary environmental point clouds in the current frame that participate in the normal distribution transformation registration. This represents the index of the laser point in a typical environmental point cloud set. Indicates the first The map coordinates of each laser point correspond to the pose vector of the current frame. The derivative matrix. The current frame pose information matrix is ​​obtained by accumulating the relationship between the geometric matching residual and the changes in the translation and attitude components. The elements in the current frame pose information matrix reflect the degree of constraint of the ordinary environment point cloud on the current frame pose vector.

[0125] S3.3. Perform eigenvalue decomposition on the pose information matrix of the current frame to obtain the feature direction and feature intensity corresponding to each pose component.

[0126] The eigenvalue decomposition is performed using a symmetric matrix eigenvalue decomposition method, and the expression is:

[0127] ;

[0128] in, This represents the pose information matrix for the current frame. This represents the index of the pose component obtained from the eigenvalue decomposition. Indicates the first The feature orientation corresponding to each pose component Indicates the first The feature intensities corresponding to each pose component are then arranged in descending order of feature intensity to obtain the feature intensity sequence. And calculate the difference between adjacent feature intensities, expressed as:

[0129] ;

[0130] in, Indicates the first The feature intensity corresponding to the pose component and the first The difference between the feature intensities corresponding to each pose component.

[0131] The adjacent positions with the largest difference are selected as the pose component division positions. The feature directions before the pose component division positions are labeled as pose components that can be stably constrained by ordinary point clouds, and the feature directions after the pose component division positions are labeled as pose components that are not sufficiently constrained by ordinary point clouds. When all adjacent feature intensity differences are zero, all pose components are determined by ordinary point cloud registration results, and topological residual orientation embedding of reflector pillars is not performed.

[0132] The pose components that can be stably constrained by ordinary point clouds correspond to the ordinary point cloud registration results, while the pose components that are not sufficiently constrained by ordinary point clouds correspond to the topology map of the reflector pillars in the current frame, thus obtaining the pose component segmentation results.

[0133] Extract the topological nodes, adjacent distance relationships, and consecutive frame order from the current frame's reflector topology map, and extract the center point sequence and adjacent distance relationships from the reflector positioning layer. Map the topological nodes in the current frame's reflector topology map to the center point sequence in the reflector positioning layer to form candidate reflector-corresponding point pairs. The candidate reflector-corresponding point pairs are determined by the proximity relationships of the topological nodes, the spatial order of the center point sequence in the reflector positioning layer, and the adjacent distance relationships.

[0134] S3.4. Compare the adjacent distance relationships, arrangement order, and continuous frame trajectory relationships among the candidate reflector point pairs, and delete candidate reflector point pairs that do not meet the consistency requirements. The retained candidate reflector point pairs form a consistent reflector correspondence graph. The nodes in the consistent reflector correspondence graph represent the retained candidate reflector point pairs, and the connection relationships in the consistent reflector correspondence graph indicate that the adjacent distance relationships and arrangement order among the candidate reflector point pairs are consistent. Select the subgraph with the largest number of nodes and consistent adjacent relationships from the consistent reflector correspondence graph to obtain the maximum reflector correspondence subgraph. When there are no available topological nodes in the reflector topology graph of the current frame, the maximum reflector correspondence subgraph is empty, and the pose correction amount of the current frame is determined by the ordinary point cloud registration result.

[0135] The correspondence between the center point of the current frame's reflector and the center point of the reflector positioning layer is determined using the sub-image corresponding to the maximum reflector. The positional deviation of the corresponding center point in the map coordinate system is then calculated to obtain the reflector topological residual. The calculation object of the reflector topological residual is the corresponding center point in the sub-image corresponding to the maximum reflector. The center point of the current frame's reflector is transformed to the map coordinate system according to the ordinary point cloud registration result, and then the positional deviation is calculated between it and the corresponding center point in the reflector positioning layer. The expression is:

[0136] ;

[0137] in, This indicates the number of reflection pillars in the subgraph corresponding to the largest reflection pillar. Each corresponding center point number Indicates the first Topological residuals of the reflective columns corresponding to the center points This indicates the coordinates of the center point of the reflector in the current frame after transformation to the map coordinate system according to the ordinary point cloud registration result. Indicates the reflective column positioning layer and The corresponding center point coordinates.

[0138] After obtaining the topological residual of the reflector, the center point of the reflector in the subgraph corresponding to the largest reflector has a definite map correspondence. Based on the definite map correspondence, the center point of the reflector is incorporated as a high-reflectivity registration term into the NDT evaluation function of the ordinary environment point cloud, so that the ordinary environment point cloud registration term and the reflector center point registration term jointly participate in the pose solution under the same rigid body transformation.

[0139] For the set of center points of the reflectors in the current frame The NDT evaluation function for the fused reflective column is:

[0140] ;

[0141] in, This represents the NDT evaluation function after incorporating the reflective columns. This represents the set of center points of the reflector pillars in the current frame. Represents the first point in the set of center points of the reflector pillars in the current frame. The center point of each reflective column This indicates the index of the center point in the set of center points of the reflector pillars in the current frame. Indicates the first The center point of each reflective column is numbered according to the center point number in the reflective column positioning layer. Indicates the reflective column positioning layer and The corresponding center point coordinates Indicates the reflective column positioning layer and The covariance matrix of the high-reflectivity point cluster corresponding to the center point, The matrix representing the inverse of the covariance matrix of the high-reflectivity point cluster is... This represents the residual coefficient at the center point of the reflector column.

[0142] The registration term for the center point of the reflective column is defined by the corresponding sub-image of the largest reflective column, while the registration term for the ordinary environment point cloud is defined by the local geometric distribution of the ordinary point cloud layer. The NDT evaluation function after fusing the reflective columns is constrained by the same rigid body transformation by both types of registration terms.

[0143] S3.5. Map the topological residual of the reflector pillar to the pose components with insufficient constraints in the ordinary point cloud according to the pose component partitioning results, to obtain the under-constrained reflector pillar residual. For the translation components with insufficient constraints in the ordinary point cloud, map the topological residual of the reflector pillar... The components in the same coordinate direction are used as the under-constrained directional reflector residuals; for attitude components with insufficient constraints in ordinary point clouds, the directional deviation between the direction of the line connecting the center points of adjacent reflectors in the sub-image corresponding to the maximum reflector and the direction of the line connecting the corresponding center points in the reflector positioning layer is used as the under-constrained directional reflector residual.

[0144] The pose correction components that can be stably constrained in ordinary point clouds are extracted using the registration results of ordinary point clouds, and the pose correction components that are insufficiently constrained in ordinary point clouds are extracted using the under-constrained directional reflector residuals. Specifically, the pose vector of the current frame is... The pose components are divided into stable and constrained pose components and unconstrained pose components based on the pose component segmentation results of the ordinary point cloud. The pose increment in the ordinary point cloud registration result is projected onto the stable and constrained pose components of the ordinary point cloud to obtain the stable and constrained pose correction components. The unconstrained directional reflector residual is projected onto the unconstrained pose components of the ordinary point cloud to obtain the unconstrained pose correction components. The stable and constrained pose correction components and the unconstrained pose correction components of the ordinary point cloud are then combined according to the pose vector of the current frame. The components are arranged in order to obtain the componentized pose correction relationship.

[0145] The geometric matching residuals and topological residuals of the center points of reflector pillars in ordinary environment point clouds are simultaneously corrected using the fractional pose correction relation, and the pose change of the current frame is determined. Specifically, the pose vector of the current frame is updated using the fractional pose correction relation. The geometric matching residual of the ordinary environment point cloud and the topological residual of the reflector center point are recalculated. When the geometric matching residual of the ordinary environment point cloud no longer decreases with the pose correction component that can be stably constrained by the ordinary point cloud, and the topological residual of the reflector center point no longer decreases with the pose correction component that is not constrained by the ordinary point cloud, the fractionalized pose correction relationship is determined as the pose change of the current frame. The pose change of the current frame includes translational change and attitude change, and the pose change of the current frame is used as the pose correction amount of the current frame.

[0146] S4. Based on the pose correction amount of the current frame, convert the coordinates of the current frame's two-layer point cloud data, and output the robot's real-time pose and the pose reference for the next frame.

[0147] S4.1. Superimpose the current frame pose correction amount with the current frame pose vector to obtain the robot's real-time pose. The robot's real-time pose includes the robot's position and orientation in the map coordinate system. The position corresponds to the translation amount in the map coordinate system, and the orientation corresponds to the rotation amount in the map coordinate system.

[0148] The pose correction amount in the current frame comes from the pose correction component that can be stably constrained by the ordinary point cloud and the pose correction component that is not constrained by the ordinary point cloud. The robot's real-time pose retains both the registration result of the ordinary environment point cloud and the orientation embedding result of the topological residual of the reflector.

[0149] The expression for calculating the robot's real-time pose using the current frame pose vector and the current frame pose correction is as follows:

[0150] ;

[0151] in, This indicates the robot's real-time pose. This represents the pose vector of the current frame. This indicates the pose correction amount for the current frame.

[0152] S4.2. The robot's real-time pose transformation is used to convert the coordinates of the current frame's two-layer point cloud data to obtain the current frame's aligned point cloud in the map coordinate system. The current frame's two-layer point cloud data includes the ordinary environment point cloud and the trajectory of the reflector center point. The laser points in the ordinary environment point cloud are transformed to the map coordinate system according to the robot's real-time pose, and the center point in the reflector center point trajectory is also transformed to the map coordinate system according to the robot's real-time pose. The transformed ordinary environment point cloud and the transformed reflector center point trajectory together serve as the current frame's aligned point cloud.

[0153] The coordinate transformation of the current frame's two-layer point cloud data uses rigid body coordinate transformation, and the expression is:

[0154] ;

[0155] in, This indicates the first [item] in the current frame's two-layer point cloud data. The coordinates of a point in the current frame's point cloud coordinate system Indicates the first The coordinates of each point after transformation to the map coordinate system This indicates the point number in the current frame's two-layer point cloud data. Indicates the real-time pose of the robot The rotation matrix obtained from the attitude components, Indicates the real-time pose of the robot The translation vector is obtained from the translation components in the vector.

[0156] S4.3. Align the current frame's point cloud with the robot's real-time pose in the map coordinate system to generate a current frame localization trajectory record. The next frame's pose reference is determined from this current frame's localization trajectory record. The next frame's pose reference uses the robot's real-time pose from the current frame's localization trajectory record as the starting pose for pose pre-alignment of the next frame's 3D laser point cloud, and uses the trajectory of the current frame's reflector center point from the current frame's localization trajectory record as the trajectory of the center point of the preceding frame when extracting the center point trajectory of the reflector from the next frame's 3D laser point cloud. If the current frame's localization trajectory record does not contain the current frame's reflector center point trajectory, the next frame's pose reference uses both the robot's real-time pose and the center point trajectory of the preceding frame; if the current frame's localization trajectory record contains the current frame's reflector center point trajectory, the next frame's pose reference uses both the robot's real-time pose and the current frame's reflector center point trajectory.

[0157] S5. Correct the trajectory of the center point of the reflector using the pose reference of the next frame, and construct the trajectory of the center point of the reflector that satisfies the adjacent distance relationship as the key frame index of the reflector to generate the positioning result of the mobile robot.

[0158] S5.1. The pose reference of the next frame is used to perform pose pre-alignment of the 3D laser point cloud of the next frame, and the position of the center point of the reflector in the 3D laser point cloud of the next frame is extracted according to the reflector positioning preprocessing method in S2. The real-time pose of the robot in the pose reference of the next frame is used to transform the 3D laser point cloud of the next frame to the map coordinate system. The center point trajectory of the previous frame in the pose reference of the next frame is used to determine whether the center point of the reflector in the 3D laser point cloud of the next frame continues the same center point trajectory of the reflector. The center point position of the reflector that continues the same center point trajectory of the reflector is added to the center point trajectory of the previous frame to obtain the corrected center point trajectory of the reflector.

[0159] The corrected trajectory of the reflector center point is compared with the adjacent distance relationship in the reflector positioning layer, and the reflector center point trajectories that satisfy the adjacent distance relationship are selected. In the comparison process, the distance between adjacent center points in the corrected reflector center point trajectory is taken as the current trajectory distance, and the distance between adjacent center points in the reflector positioning layer is taken as the map reference distance. When the current trajectory distance is consistent with the map reference distance, and the order of appearance of adjacent center points in consecutive frames is consistent with the sequence of center points in the reflector positioning layer, the corrected reflector center point trajectory is retained. When the current trajectory distance is inconsistent with the map reference distance, or the order of appearance of adjacent center points in consecutive frames is inconsistent with the sequence of center points in the reflector positioning layer, the corresponding corrected reflector center point trajectory is deleted, thus obtaining the reflector center point trajectories that satisfy the adjacent distance relationship.

[0160] S5.2. Establish index items according to the correspondence between frame number, robot real-time pose, reflector center point sequence, and adjacent distance relationship to obtain reflector keyframe index. Specifically, the frame number of the reflector center point trajectory that satisfies the adjacent distance relationship is used as the time identifier of the index item, the robot real-time pose under the same frame number is used as the pose content of the index item, the reflector center point sequence under the same frame number is used as the reflector position content of the index item, and the adjacent distance relationship under the same frame number is used as the geometric constraint content of the index item. The index item is formed by the time identifier, pose content, reflector position content, and geometric constraint content. The index items are arranged in the order of frame number to obtain reflector keyframe index.

[0161] From the reflector keyframe index, select the index item that has the smallest positional distance to the next frame pose reference in the map coordinate system and contains the trajectory of continuous reflector center points. Extract the robot's real-time pose and reflector center point sequence corresponding to the index item to obtain the subsequent local registration reference frame. The smallest positional distance means that the robot's real-time pose in the index item is closest to the robot's real-time pose in the next frame pose reference in the map coordinate system. The continuous reflector center point trajectory means that the reflector center point sequence in the index item can maintain the same center point appearance order and adjacent distance relationship in adjacent frames. When there is no index item containing the trajectory of continuous reflector center points in the reflector keyframe index, the subsequent local registration reference frame is determined by the robot's real-time pose in the next frame pose reference and the local geometric distribution set of the ordinary point cloud layer.

[0162] Subsequent local registration reference frames are used to continue participating in the eigenvalue decomposition of the pose information matrix and the underconstrained directional projection normal distribution transformation registration of the reflector pillars to generate the mobile robot localization result. The real-time robot pose in the subsequent local registration reference frames is used as the pose reference of the 3D laser point cloud in the subsequent frames, and the sequence of reflector pillar center points in the subsequent local registration reference frames is used as the reference for the corresponding trajectory of the reflector pillar center points in the subsequent frames. The 3D laser point cloud in the subsequent frames continues to generate the pose correction amount, the real-time robot pose, and the localization trajectory record of the subsequent frames according to S2 to S4. The continuously obtained real-time robot pose, real-time robot pose in the subsequent frames, and the corresponding reflector pillar center point trajectory together form the mobile robot localization result.

[0163] In summary, this invention unifies the local geometric distribution of ordinary point cloud layers with the center point sequence and adjacent distance relationships of reflective pillar positioning layers into the same map coordinate system. This allows mobile robot positioning to no longer rely solely on ordinary environmental point clouds such as wall surfaces, ground surfaces, and pillar outlines, but instead forms a real-time positioning reference involving both ordinary structural constraints and high-reflectivity reflective pillar constraints. This improves the reliability of the positioning reference in repetitive structures, open areas, and long corridor scenarios. Through reflective pillar point cluster identification, center point trajectory correspondence, and reflective pillar topology map construction, reflective pillars are transformed from discrete high-reflectivity points into stable positioning references with spatial order, adjacent distances, and continuous frame trajectory relationships. This reduces the impact of isolated high-reflectivity noise and erroneous reflective points on pose solving. Furthermore, by using pose information matrix eigenvalue decomposition to distinguish ordinary... The system combines stable point cloud pose components with those that are insufficiently constrained by ordinary point clouds. It integrates the high-reflectivity registration term from the subgraph corresponding to the largest reflector into the normal distribution transformation evaluation function. This allows ordinary environmental point cloud registration and reflector topological residuals to work synergistically in the same pose solution process, rather than simply superimposing two types of features. Through topological residual orientation embedding, the deviation of the reflector center point can effectively compensate for pose directions that are difficult to constrain by ordinary point clouds, reducing the risk of robot drift along the travel direction and lateral offset accumulation. The reflector keyframe index provides continuous reference for subsequent local registration, maintaining the continuity of the localization process even when the reflector is temporarily invisible or environmental geometric features are insufficient. This improves the stability, accuracy, and engineering applicability of the mobile robot's 3D laser SLAM localization results.

[0164] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for 3D laser SLAM fusion and reflective column localization of a mobile robot, characterized in that: include, Establish a general point cloud layer and a reflective column positioning layer for the environmental point cloud map. Extract the local geometric distribution of the general point cloud layer, the center point sequence and adjacent distance relationship of the reflective column positioning layer, and generate a real-time positioning reference set. The real-time positioning reference set is used to perform reflector column localization preprocessing on the current frame's 3D laser point cloud, extract the trajectory of the center point of the reflector column and divide the ordinary environment point cloud, and construct the current frame's two-layer point cloud data and reflector column topology map; The normal distribution transformation registration is performed on the ordinary environment point cloud and the ordinary point cloud layer in the current frame dual-layer point cloud data. Then, the pose information matrix feature decomposition, maximum consistent subgraph matching and topological residual orientation embedding are combined with the reflection column topology map to generate the pose correction amount of the current frame. Based on the pose correction amount of the current frame, convert the coordinates of the current frame's two-layer point cloud data, and output the robot's real-time pose and the pose reference for the next frame; The trajectory of the center point of the reflector is corrected by the pose reference of the next frame, and the trajectory of the center point of the reflector that satisfies the adjacent distance relationship is constructed as the key frame index of the reflector to generate the localization result of the mobile robot.

2. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 1, characterized in that: The generation of the real-time positioning reference set specifically includes: Load the environmental point cloud map, and divide the fixed structure point cloud of the environment into the ordinary point cloud layer, and divide the high reflectivity points of the reflective pillars into the reflective pillar positioning layer; The ordinary point cloud layer is processed according to the grid scale of the normal distribution transformation registration, and the local geometric distribution of the point cloud in each grid is calculated to obtain the local geometric distribution set of the ordinary point cloud layer. Extract the center point of each reflector in the reflector positioning layer, arrange the center points according to the spatial order in the map coordinate system, and calculate the distance between adjacent center points to obtain the reflector center point sequence and adjacent distance relationship; The initial frame 3D laser point cloud is registered with the local geometric distribution set of the ordinary point cloud layer by performing a normal distribution transformation to obtain the initial pose. By unifying the local geometric distribution set, the sequence of reflective pillar center points, the adjacent distance relationship, and the initial pose into the same map coordinate system, a real-time positioning reference set is generated.

3. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 2, characterized in that: The preprocessing for positioning the reflective column specifically includes: The initial pose and ordinary point cloud layer in the real-time positioning reference set are used to perform pose pre-alignment of the current frame's 3D laser point cloud to obtain the pre-aligned 3D laser point cloud. The continuity of reflection intensity, spatial morphology of point clusters, and beam angle distribution of pre-aligned 3D laser point clouds are detected to obtain reflective column point clusters; Extract the center point of the reflective column cluster and match the center point with the trajectory of the center point in the previous frame to obtain the trajectory of the reflective column center point; The reflective column clusters are separated from the pre-aligned 3D laser point cloud, and the separated point cloud is retained as a normal environmental point cloud.

4. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 3, characterized in that: The construction of the current frame's two-layer point cloud data and reflective column topology map specifically includes: The ordinary environment point cloud, the trajectory of the center point of the reflector, and the initial pose are unified to the current frame point cloud coordinate system to obtain the current frame double-layer point cloud data with pose reference. The center point in the trajectory of the center point of the reflective column is recorded as the topological node. The adjacent nodes are determined according to the spatial proximity order between the center points. The distance between the adjacent nodes is calculated, and the order in which the adjacent nodes appear in consecutive frames is recorded. The topology map of reflective pillars in the current frame is constructed by topological nodes, adjacent nodes, adjacent distances, and the order of occurrence of consecutive frames.

5. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 4, characterized in that: The normal distribution transformation registration specifically includes: Select the ordinary environment point cloud in the current frame of dual-layer point cloud data, and transform the ordinary environment point cloud to the corresponding raster of the ordinary point cloud layer according to the pose reference in the current frame of dual-layer point cloud data. Local probability distribution parameters are established using the mean and covariance matrix of the point cloud within the corresponding grid, and the geometric matching residual between the ordinary environment point cloud and the local geometric distribution within the corresponding grid is calculated based on the local probability distribution parameters. By using the normal distribution transformation evaluation function of the ordinary environment point cloud, the coordinate transformation relationship of the ordinary environment point cloud is iteratively adjusted along the translation and attitude components of the robot pose to obtain the ordinary point cloud registration result. The pose increment and geometric matching residual are extracted from the ordinary point cloud registration results to generate an ordinary point cloud registration constraint set.

6. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 5, characterized in that: The feature decomposition of the pose information matrix specifically includes: The influence relationship between each translation component and attitude component on the geometric matching residual is extracted using a common point cloud registration constraint set, and the pose information matrix of the current frame is constructed. Perform eigenvalue decomposition on the pose information matrix of the current frame to obtain the feature direction and feature intensity corresponding to each pose component; Arrange the feature directions in descending order of feature intensity, calculate the difference between adjacent feature intensities, and use the adjacent position with the largest difference as the position of the pose component. The feature directions before the pose components are divided into positions are labeled as pose components that can be stably constrained by ordinary point clouds, and the feature directions after the pose components are divided into positions are labeled as pose components that are not sufficiently constrained by ordinary point clouds. Establish component correspondences between the pose components that can be stably constrained by ordinary point clouds, the pose components that are not sufficiently constrained by ordinary point clouds, the registration results of ordinary point clouds, and the topology map of the reflector pillars in the current frame, and generate pose component segmentation results.

7. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 6, characterized in that: The maximum consistent subgraph matching specifically includes: Extract the topological nodes, adjacent distance relationships, and consecutive frame occurrence order from the current frame's reflective column topology map, and extract the center point sequence and adjacent distance relationships from the reflective column positioning layer; By mapping the topological nodes in the current frame's reflective column topology map to the sequence of center points in the reflective column positioning layer, candidate reflective column corresponding point pairs are obtained. Compare the adjacent distance relationship, arrangement order and continuous frame trajectory relationship between the corresponding point pairs of candidate reflective pillars, and delete the corresponding point pairs of candidate reflective pillars that do not meet the consistency relationship; A consistent reflection column correspondence graph is constructed using the retained candidate reflection column correspondence point pairs. The subgraph with the largest number of nodes and consistent adjacency relationship is selected from the consistent reflection column correspondence graph to obtain the maximum reflection column correspondence subgraph.

8. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 7, characterized in that: The topological residual oriented embedding specifically includes: The correspondence between the center point of the reflective column in the current frame and the center point of the reflective column positioning layer is determined by using the sub-map corresponding to the maximum reflective column, and the positional deviation of the corresponding center point in the map coordinate system is calculated to obtain the topological residual of the reflective column. The center point of the reflection column in the sub-image corresponding to the maximum reflection column is used as a high reflection registration term and incorporated into the normal distribution transformation evaluation function of the ordinary environmental point cloud to obtain the normal distribution transformation evaluation function of the fused reflection column. The topological residual of the reflector is mapped to the pose component of the ordinary point cloud with insufficient constraints according to the result of the pose component partitioning, so as to obtain the under-constrained direction reflector residual. The pose correction components that can be stably constrained by ordinary point cloud registration results are extracted, and the pose correction components that are not constrained by ordinary point cloud are extracted by under-constrained directional reflector residuals, thus obtaining the quantized pose correction relationship. By utilizing the fractional pose correction relationship and the normal distribution transformation evaluation function of the fused reflector, the geometric matching residual of the ordinary environmental point cloud and the topological residual of the reflector center point are corrected simultaneously, and the pose change of the current frame is determined to generate the pose correction amount of the current frame.

9. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 8, characterized in that: The output robot real-time pose and next frame pose reference specifically include: The coordinates of the current frame's two-layer point cloud data are transformed using the current frame pose correction value to obtain the current frame aligned point cloud in the map coordinate system; The robot's position and pose in the map coordinate system are extracted from the point cloud aligned with the current frame to obtain the robot's real-time pose. By mapping the robot's real-time pose to the trajectory of the center point of the reflector in the current frame, the positioning trajectory record of the current frame is obtained, and the pose reference for the next frame is determined by the positioning trajectory record of the current frame.

10. The mobile robot 3D laser SLAM fusion reflector positioning method as described in claim 9, characterized in that: The generation of mobile robot positioning results specifically includes: The position of the center point of the reflective column in the next frame of the 3D laser point cloud is corrected by using the pose reference of the next frame, and the corrected trajectory of the center point of the reflective column is obtained. Compare the corrected trajectory of the center point of the reflective column with the adjacent distance relationship in the reflective column positioning layer, and filter the center point trajectories of the reflective columns that satisfy the adjacent distance relationship; An index is created based on the correspondence between frame number, robot real-time pose, reflector center point sequence, and adjacent distance relationship to obtain the reflector keyframe index; From the keyframe index of reflectors, select the index item that has the smallest position distance to the pose reference of the next frame in the map coordinate system and contains the trajectory of the center point of the continuous reflectors. Extract the robot's real-time pose and the sequence of center points of the reflectors corresponding to the index item to obtain the subsequent local registration reference frame. The subsequent local registration reference frames are used to continue participating in the eigenvalue decomposition of the pose information matrix and the underconstrained directional projection normal distribution transformation registration of the reflector pillars to generate the mobile robot localization results.

Images