Map construction method and device, robot, storage medium and program product

Abstract

This application discloses a map construction method, apparatus, robot, storage medium, and program product, belonging to the field of image processing technology. The map construction method includes: acquiring point cloud data representing the robot's environment; performing multi-resolution scanning matching on the point cloud data to obtain local poses, and generating front-end constraints representing the relative transformation relationship between adjacent local poses to construct a pose graph; performing similarity retrieval and geometric consistency verification on the point cloud data, and generating closed-loop constraints based on the verification results and adding them to the pose graph; the closed-loop constraints representing the relative transformation relationship between local poses and historical local poses; performing global consistency correction on the pose graph using the front-end constraints and closed-loop constraints to obtain a global pose; and projecting the point cloud data onto a global coordinate system based on the global pose to construct and output a map. The map construction method of this application improves the global consistency and real-time performance of map construction.

Description

Technical Field

[0001] This application belongs to the field of image processing technology, and in particular relates to a map building method, apparatus, robot, storage medium and program product. Background Technology

[0002] Two-dimensional simultaneous localization and mapping (SLT / MR) technology is applicable to service robots, automated guided vehicles (AGVs), and navigation systems. In practical applications such as warehousing, supermarkets, and hospitals, while improving mapping accuracy, the requirements for map repeatability and maintainability are also gradually increasing. Related technologies typically utilize scan matching to obtain relative poses and construct a pose graph, while loop closure detection generates loop closure constraints. Finally, graph optimization is used to globally correct the pose graph to output the map. However, due to the significant impact of noise and occlusion on low-cost 2D LiDAR, related technologies face insufficient reliability issues in registration and loop closure in large-scale or complex environments, easily leading to map drift and closure errors. Furthermore, loop closure detection, when dealing with environments with similar local structures, is prone to introducing false detection constraints due to feature confusion and a lack of rigorous geometric verification. This results in global map distortion during global correction, affecting the global consistency of the system during long-path operation. Summary of the Invention

[0003] This application aims to address at least one of the technical problems existing in the prior art. To this end, this application proposes a map building method, apparatus, robot, storage medium, and program product, which improves the global consistency and real-time performance of map building.

[0004] Firstly, this application provides a map construction method, which includes: Acquire point cloud data representing the robot's environment; Multi-resolution scanning matching is performed on the point cloud data to obtain local poses, and front-end constraints representing the relative transformation relationship between adjacent local poses are generated to construct a pose graph. The point cloud data is subjected to similarity retrieval and geometric consistency verification, and closed-loop constraints are generated and added to the pose graph based on the verification results; wherein, the closed-loop constraints represent the relative transformation relationship between the local pose and the historical local pose. The pose graph is subjected to global consistency correction using the front-end constraints and the closed-loop constraints to obtain the global pose. The point cloud data is projected onto the global coordinate system based on the global pose to construct and output a map.

[0005] According to the map construction method of this application, multi-resolution scanning matching is performed on point cloud data to generate front-end constraints, which improves the robustness of local registration in complex scenes, effectively reduces the cumulative error of pose estimation, and alleviates map drift. Furthermore, by combining similarity retrieval and geometric consistency verification to generate closed-loop constraints, reliable verification of candidate loop closures is achieved, reducing the risk of erroneous constraints introduced by feature confusion. Then, global consistency correction is performed on the pose graph, realizing the integrated collaboration of front-end matching, loop closure detection, and back-end optimization, and constructing a data exchange and constraint management mechanism between the front-end, back-end, and closed loops. Through the collaborative strategy of multi-resolution scanning matching, closed-loop geometric consistency verification, and global consistency correction, registration errors are effectively controlled in the early stages, achieving timely error constraints and gradual convergence, thereby improving the global consistency and real-time performance of map construction.

[0006] According to one embodiment of this application, the step of performing multi-resolution scan matching on the point cloud data to obtain local poses and generating front-end constraints characterizing the relative transformation relationship between adjacent local poses to construct a pose graph includes: The first estimated pose of the point cloud data is obtained based on the motion model; Based on the first estimated pose, multi-resolution scanning matching is performed on the point cloud data to obtain the local pose; The local pose is added to the pose graph as a pose node, and front-end constraints are generated based on the relative transformation relationship between the pose nodes.

[0007] According to one embodiment of this application, the step of performing multi-resolution scan matching on the point cloud data based on the first estimated pose to obtain the local pose includes: Based on the first estimated pose, the point cloud data and the constructed local map are registered at the first resolution to obtain the first matching pose; Based on the first matched pose, dynamic point culling processing is performed on the point cloud data to obtain the culled point cloud data. Using the first matched pose as a reference, a second registration is performed on the removed point cloud data and the constructed local map at a second resolution to obtain the local pose; wherein, the first resolution is lower than the second resolution.

[0008] According to one embodiment of this application, the step of performing similarity retrieval and geometric consistency verification on the point cloud data, and generating closed-loop constraints based on the verification results and adding them to the pose graph, includes: When the robot's motion state reaches a motion state threshold, a subgraph descriptor retrieval and filtering process is performed on the subgraph to obtain closed-loop candidate positions; wherein, the subgraph is obtained by aggregating the pose nodes and their corresponding point cloud data; Perform the geometric consistency check on the candidate closed-loop positions, and generate the closed-loop constraints based on the check results, adding them as constraint edges to the pose graph.

[0009] According to one embodiment of this application, performing the geometric consistency check on the candidate closed-loop positions and generating the closed-loop constraints based on the check results, and adding them as constraint edges to the pose graph, includes: Obtain the relative pose transformation relationship and matching residual between the point cloud data and the historical point cloud data corresponding to the closed-loop candidate position; The geometric consistency verification is performed on the relative pose transformation relationship based on the matching residual. If the geometric consistency check passes, the closed-loop constraint generated based on the relative pose transformation relationship will be added as a constraint edge to the pose graph.

[0010] According to one embodiment of this application, the step of performing global consistency correction on the pose graph using the front-end constraint and the closed-loop constraint to obtain a global pose includes: Based on the residual terms corresponding to the front-end constraints and the closed-loop constraints, a global objective function is constructed. The residual terms in the global objective function are weighted to obtain the weighted global objective function; The weighted global objective function is iteratively solved to obtain the pose increment of the pose node; The pose of the pose nodes is updated by superimposing the pose increment, and the updated pose nodes are combined into the global pose.

[0011] According to one embodiment of this application, the step of projecting the point cloud data onto a global coordinate system based on the global pose to construct and output a map includes: Extract the local coordinates of each data point in the point cloud data; Based on the global pose, determine the corresponding coordinate transformation matrix; The local coordinates of each data point are transformed and calculated using the coordinate transformation matrix to obtain the global coordinates of each data point in the global coordinate system. The map is constructed and output by stitching together data points containing the global coordinates of each data point.

[0012] Secondly, this application provides a map building apparatus, which includes: The first processing module is used to acquire point cloud data that characterizes the environment in which the robot is located; The second processing module is used to perform multi-resolution scanning matching on the point cloud data to obtain local poses and generate front-end constraints that characterize the relative transformation relationship between adjacent local poses in order to construct a pose graph. The third processing module is used to perform similarity retrieval and geometric consistency verification on the point cloud data, and generate closed-loop constraints based on the verification results and add them to the pose graph; wherein, the closed-loop constraints represent the relative transformation relationship between the local pose and the historical local pose. The fourth processing module is used to perform global consistency correction on the pose graph using the front-end constraints and the closed-loop constraints to obtain the global pose. The fifth processing module is used to project the point cloud data onto the global coordinate system based on the global pose in order to construct and output a map.

[0013] According to the map building apparatus of this application, multi-resolution scanning matching is performed on point cloud data to generate front-end constraints, which improves the robustness of local registration in complex scenes, effectively reduces the cumulative error of pose estimation, and alleviates map drift. Furthermore, by combining similarity retrieval and geometric consistency verification to generate closed-loop constraints, reliable verification of candidate loop closures is achieved, reducing the risk of erroneous constraints introduced by feature confusion. Then, global consistency correction is performed on the pose graph, realizing the integrated collaboration of front-end matching, loop closure detection, and back-end optimization, and constructing a data exchange and constraint management mechanism between the front-end, back-end, and closed loop. Through the collaborative strategy of multi-resolution scanning matching, closed-loop geometric consistency verification, and global consistency correction, registration errors are effectively controlled in the early stages, achieving timely error constraint and gradual convergence, thereby improving the global consistency and real-time performance of map building.

[0014] Thirdly, this application provides a robot, comprising: Environmental ranging sensors are used to acquire point cloud data characterizing the robot's environment; The map building apparatus as described in the second aspect above; the environmental ranging sensor is electrically connected to the map building apparatus.

[0015] Fourthly, this application provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the map construction method as described in the first aspect above.

[0016] Fifthly, this application provides a computer program product, including a computer program that, when executed by a processor, implements the map construction method as described in the first aspect above.

[0017] The above-described one or more technical solutions in the embodiments of this application have at least one of the following technical effects: By performing multi-resolution scanning matching on point cloud data to generate front-end constraints, the robustness of local registration in complex scenes is improved, effectively reducing the cumulative error of pose estimation and alleviating map drift. Furthermore, by combining similarity retrieval and geometric consistency verification to generate closed-loop constraints, reliable verification of candidate loop closures is achieved, reducing the risk of erroneous constraints introduced by feature confusion. Finally, global consistency correction is performed on the pose graph, realizing integrated collaboration between front-end matching, loop closure detection, and back-end optimization, and constructing a data exchange and constraint management mechanism between the front-end, back-end, and closed-loop systems. Through the coordinated strategy of multi-resolution scanning matching, closed-loop geometric consistency verification, and global consistency correction, registration errors are effectively controlled in the early stages, enabling timely error constraints and gradual convergence, thereby improving the global consistency and real-time performance of the mapping.

[0018] Furthermore, by introducing motion priors for joint coarse registration at a lower first resolution, the convergence boundary is expanded and the risk of computation getting trapped in local extrema is reduced, improving convergence stability in weak feature environments. Based on this, dynamic point culling is performed using the coarse registration pose to effectively identify and filter moving objects, reducing interference from dynamic obstacles. Then, using the coarse registration pose as the initial value, fine registration is performed on the culled point cloud at a higher second resolution, achieving accurate local pose solving. This deep synergy between the multi-resolution mechanism and dynamic filtering significantly enhances the robustness of local registration in complex dynamic and sparse scenes, effectively controlling registration errors in the early stages, thereby improving the global consistency and real-time performance of the mapping.

[0019] Furthermore, by setting motion state thresholds, loop closure detection is triggered on demand, effectively balancing environmental coverage and computational efficiency. During the detection process, subgraph descriptor retrieval is first used for coarse screening to improve the recall rate of candidate positions. Then, geometric consistency verification is used to filter out false detections caused by feature confusion, achieving loop closure screening that balances real-time performance and accuracy. Finally, the verified loop closure constraints are added to the pose graph as global topological constraint edges, effectively suppressing the cumulative drift under long-term system operation, significantly reducing map closure error and improving loop closure stability, thereby enhancing the global consistency of map construction and real-time performance.

[0020] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0021] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, in which: Figure 1 This is one of the flowcharts illustrating the map construction method provided in the embodiments of this application; Figure 2 This is a second schematic flowchart of the map construction method provided in the embodiments of this application; Figure 3 This is the third flowchart illustrating the map construction method provided in this application embodiment; Figure 4 This is a schematic diagram of the structure of the map building apparatus provided in the embodiments of this application; Figure 5 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0022] The technical solutions of the embodiments of this application will be clearly described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application are within the scope of protection of this application.

[0023] The terms "first," "second," etc., used in the specification and claims of this application are used to distinguish similar objects and not to describe a specific order or sequence. It should be understood that such use of data can be interchanged where appropriate so that embodiments of this application can be implemented in orders other than those illustrated or described herein, and the objects distinguished by "first," "second," etc., are generally of the same class and the number of objects is not limited; for example, a first object can be one or more. Furthermore, in the specification and claims, "and / or" indicates at least one of the connected objects, and the character " / " generally indicates that the preceding and following objects are in an "or" relationship.

[0024] The map building method, map building device, robot, electronic device, and readable storage medium provided in this application will be described in detail below with reference to the accompanying drawings and through specific embodiments and application scenarios.

[0025] The map construction method can be applied to the terminal, and can be executed by the hardware or software in the terminal.

[0026] The map building method provided in this application embodiment can be executed by an electronic device or a functional module or entity in an electronic device that can implement the map building method. The following uses a robot as the execution subject to illustrate the map building method provided in this application embodiment.

[0027] During the research and development process, the inventors discovered that 2D simultaneous localization and mapping (SLAM) technology is applicable to service robots, automated guided vehicles (AGVs), and navigation systems. In practical applications such as warehousing, supermarkets, and hospitals, while improving mapping accuracy, the requirements for map repeatability and maintainability are also gradually increasing. Related technologies typically use algorithms such as Iterative Closest Point (ICP), Normal Distribution Transform (NDT), or correlation matching to estimate adjacent poses and construct a pose graph. The front-end can be implemented using grid-based correlation matching (CSM) or NDT, while loop closure constraints are generated based on geometric consistency or global matching. Scene descriptors (such as Scan Context) are often relied upon for candidate retrieval, and finally, back-end optimization is performed using tools such as the graph optimization library g2o or the nonlinear least squares solver Ceres to obtain a grid map. However, because low-cost 2D LiDAR is significantly affected by noise and occlusion, related technologies face insufficient reliability issues in registration and loop closure in large-scale or complex environments, easily leading to map drift and closure errors.

[0028] For example, front-end matching in related technologies is prone to inaccuracy in weak feature or dynamic environments, and its insufficient handling of dynamic targets can easily lead to short-term local map instability. The resulting error accumulation can cause map structure drift. Simultaneously, loop closure detection in related technologies is prone to confusion in similar scenarios. When dealing with environments with similar local structures, feature confusion and a lack of rigorous geometric verification can easily introduce erroneous constraints such as false positives or false negatives. Furthermore, front-end matching and back-end optimization in related technologies often lack unified constraint management, resulting in unstable loop closure triggering. When false loop closures occur, back-end optimization may introduce erroneous constraints, easily causing map distortion that is difficult to recover. Without corresponding robust mechanisms or loop closure constraints, it is also difficult to maintain good global consistency and real-time performance during long-path, long-distance operation.

[0029] To address the issues of poor global consistency in mapping and insufficient robustness in local structural registration, the inventors, through in-depth research, designed a map construction method, including: acquiring point cloud data representing the robot's environment; performing multi-resolution scanning matching on the point cloud data to obtain local poses and generating front-end constraints representing the relative transformation relationship between adjacent local poses to construct a pose graph; performing similarity retrieval and geometric consistency verification on the point cloud data, and generating closed-loop constraints based on the verification results and adding them to the pose graph; wherein, the closed-loop constraints represent the relative transformation relationship between local poses and historical local poses; performing global consistency correction on the pose graph using front-end constraints and closed-loop constraints to obtain global poses; and projecting the point cloud data onto a global coordinate system based on the global poses to construct and output a map.

[0030] According to the map construction method provided in this application, multi-resolution scanning matching is performed on point cloud data to generate front-end constraints, which improves the robustness of local registration in complex scenes, effectively reduces the cumulative error of pose estimation, and alleviates map drift. Furthermore, combining similarity retrieval and geometric consistency verification to generate closed-loop constraints enables reliable verification of candidate loop closures, reducing the risk of erroneous constraints introduced by feature confusion. Then, global consistency correction is performed on the pose graph, realizing the integrated collaboration of front-end matching, loop closure detection, and back-end optimization, and constructing a data exchange and constraint management mechanism between the front-end, back-end, and closed loop. Through the collaborative strategy of multi-resolution scanning matching, closed-loop geometric consistency verification, and global consistency correction, registration errors are effectively controlled in the early stages, achieving timely error constraint and gradual convergence, thereby improving the global consistency and real-time performance of map construction.

[0031] like Figure 1 As shown, the map construction method includes steps 110, 120, 130, 140, and 150.

[0032] Step 110: Obtain point cloud data representing the environment in which the robot is located; In this step, point cloud data is a set of scattered points that characterize the geometric contours of the physical environment around the robot. It is used as the observation input stream for the subsequent scanning and matching process. The local pose of the robot is calculated by registering it with the reference data.

[0033] In some embodiments, point cloud data is acquired based on a lidar device installed on the robot or a detection data acquisition device that is communicatively connected to the robot.

[0034] In actual implementation, refer to Figure 2 In the data acquisition process of S1, the robot uses detection devices (such as 2D LiDAR) installed on its mobile chassis to scan the surrounding environment in real time, detect the geometric information of the robot's surrounding environment, and generate point cloud data that reflects the environmental detection information.

[0035] In some embodiments, after step 110, the map construction method further includes: preprocessing the point cloud data (e.g., ... Figure 2 Point cloud filtering in the data is used to improve data consistency and adjust the scale of subsequent calculations.

[0036] In this embodiment, preprocessing may include, but is not limited to: time synchronization processing, distortion correction processing, downsampling processing, and outlier removal processing.

[0037] In actual implementation, considering that the raw environmental point cloud data collected by the robot via LiDAR may contain noise or erroneous points, therefore: By performing time synchronization processing, the correspondence between point cloud data and robot motion state is improved, making the data acquisition time more consistent with the motion state. By performing distortion correction processing, the coordinates of the scan points in the point cloud data are compensated based on the robot's displacement information during the scanning cycle, so as to suppress the distortion of the scanning trajectory caused by the robot's movement. By performing downsampling, the number of points in the point cloud is reduced to adjust the processing efficiency of subsequent calculations. Furthermore, by performing outlier removal, isolated noise points or anomalous data points in the point cloud are identified and reduced, thereby reducing the interference of sensor noise on the registration process.

[0038] Step 120: Perform multi-resolution scanning matching on the point cloud data to obtain local poses, and generate front-end constraints that characterize the relative transformation relationship between adjacent local poses to construct a pose graph. In this step, multi-resolution scanning involves aligning point cloud data with the constructed local map using different grid density levels to calculate the robot's local pose.

[0039] Local pose is used to describe the spatial state information of a robot in a reference coordinate system, including but not limited to pose parameters in dimensions such as position and orientation.

[0040] Front-end constraints are used to characterize the relative transformation relationship between adjacent local poses, serving as constraint edges connecting adjacent pose nodes in the pose graph.

[0041] A pose graph is a topological structure that records the robot's motion trajectory and the constraints between frames. A pose graph contains pose nodes composed of local poses, and constraint edges composed of front-end constraints and closed-loop constraints generated later.

[0042] In some embodiments, scanning and matching processing of point cloud data can be triggered frame by frame based on the acquisition frequency of point cloud data (i.e., step 120).

[0043] In actual execution, the initial estimated pose is first determined by combining motion model or odometry data, and this is used as the reference search point for multi-resolution scan matching, providing motion priors for the registration process. Based on this, a coarse-to-fine hierarchical registration process aligns the point cloud data of the current frame with reference data (such as a pre-constructed local map), thereby calculating the local pose reflecting the robot's position and orientation in the reference coordinate system. During the scan matching process, point-to-line matching targets or the Normal Distribution Transform (NDT) algorithm can be used as matching targets to enhance the algorithm's robustness to registration under diverse environmental characteristics. This coarse-to-fine matching approach helps reduce the probability of getting trapped in local extrema during calculation, thus improving the accuracy of pose calculation.

[0044] Furthermore, refer to, for example Figure 3 In step S3, after acquiring the local pose, the system performs the process of constructing a pose graph and generating front-end constraints. During this process, the local pose is added to the pose graph as a pose node. The constraint edges in the pose graph include adjacent frame constraints, subgraph constraints, and loop closure constraints. Specifically, the system generates front-end constraints based on the relative transformation relationship between adjacent pose nodes to serve as adjacent frame constraints. Subgraph constraints are used to characterize the relative pose transformation relationship between the current pose node and the local subgraph constructed from historical multi-frame point cloud data, thereby controlling the cumulative error within the local region.

[0045] The system combines adjacent frame constraints, subgraph constraints, and closed-loop constraints generated in subsequent detection as constraint edges connecting each pose node. This constructs and dynamically updates the topology that records the robot's motion trajectory and observation constraint relationships, providing a data foundation for subsequent global consistency correction.

[0046] For reference Figure 2 In the multi-resolution scan matching and motion prior, in some embodiments, step 120 includes: The first estimated pose of point cloud data is obtained based on the motion model; Based on the first estimated pose, multi-resolution scanning matching is performed on the point cloud data to obtain the local pose. Local poses are added to the pose graph as pose nodes, and front-end constraints are generated based on the relative transformation relationships between pose nodes.

[0047] In this step, the first estimated pose is represented by a prediction of the spatial state at the current moment based on the robot's motion laws, which is used as the initial search point for performing Scan Matching.

[0048] In some embodiments, the first estimated pose is determined based on a motion model (such as an odometry model), which can be expressed by the formula:

[0049] in, This represents the first estimated pose at the current moment. This indicates the pose state at the previous moment. Indicates control input, This represents noise that characterizes uncertainty. This represents the process noise covariance. This motion model is used to map the pose and control input from the previous time step to the current time step, thereby providing initial values ​​for the subsequent registration process.

[0050] In some embodiments, the above motion model The form can be determined based on the robot's physical driving characteristics. For example, for a robot using differential drive, the motion model can be further expressed by the formula:

[0051] in, Linear velocity, Angular velocity, The sampling period is This represents the first estimated pose at the current moment. This represents the pose state at the previous moment. The formula describes the displacement and rotation of the robot from the previous moment to the current moment. By accumulating linear and angular velocities, the auxiliary system obtains a reasonable initial matching value (i.e., the first estimated pose) in environments with insignificant geometric features.

[0052] Furthermore, refer to, for example Figure 2 In S2, the first estimated pose obtained based on motion models such as odometry essentially provides reliable motion prior information for the point cloud registration process.

[0053] In the process of obtaining local pose, the local pose can be expressed by the formula:

[0054] in, This represents the robot's position components in the reference coordinate system at the current moment. This represents the position and orientation components of the robot in the reference coordinate system at the current moment. Local pose defines the state vector used to describe the robot in space and is a core variable for front-end matching and subsequent back-end optimization.

[0055] Furthermore, refer to, for example Figure 2 In step S3, after acquiring the local pose, the process of constructing the pose graph and generating front-end constraints is performed. During this process, the local pose is added as a pose node to the pose graph, and front-end constraints are generated based on the relative transformation relationships between adjacent pose nodes. These front-end constraints serve as constraint edges connecting adjacent pose nodes, thereby constructing and dynamically updating the topology that records the robot's motion trajectory and observation constraint relationships.

[0056] According to the map construction method provided in the embodiments of this application, the first estimated pose of point cloud data obtained based on the motion model provides a reasonable motion prior initial value for subsequent scanning registration, which can improve the convergence stability of the solution in weak feature or high dynamic scene. Furthermore, multi-resolution scanning matching is performed based on the first estimated pose, and front-end constraints are generated based on the obtained local pose to construct the pose graph, which can further enhance the robustness of local registration and reduce the risk of the matching solution getting trapped in local extrema due to inaccurate initial values. This achieves a reliable topological representation of the robot motion trajectory nodes in the pose graph, providing a high-reliability local geometric basis for subsequent global consistency correction.

[0057] In some embodiments, multi-resolution scan matching is performed on point cloud data based on a first estimated pose to obtain a local pose, including: Based on the first estimated pose, perform the first registration of point cloud data and the constructed local map at the first resolution to obtain the first matching pose; Dynamic point culling is performed on the point cloud data based on the first matched pose to obtain the culled point cloud data. Using the first matched pose as a reference, a second registration is performed on the removed point cloud data and the constructed local map at a second resolution to obtain the local pose.

[0058] In this step, the first resolution is lower than the second resolution.

[0059] The first registration is a coarse registration performed at the first resolution. For example, the first estimated pose is used as the initial search point and prior constraints, and algorithms such as Normal Distribution Transform (NDT) are used to perform point cloud alignment; at the same time, a motion prior fusion strategy is combined to balance the odometry prior and the coarse matching error, thereby stably solving the first matching pose.

[0060] Dynamic point culling refers to using the initial matching pose of the preliminary alignment to compare the geometric differences between the current point cloud and the reference map, identify and filter dynamic point clouds generated by moving objects in the environment, so as to improve the robustness of subsequent high-precision registration.

[0061] The second registration is a fine registration performed at the second resolution. In this stage, the system uses the first matched pose as the precise initial value for iteration. By solving for matching objective functions such as point-to-point or point-to-line, it performs high-precision rigid body registration between the point cloud data after removing dynamic points and the constructed local map. The pose result output after optimization is the robot's final local pose at the current moment.

[0062] The first matching pose is the preliminary alignment result of the first registration output, which is used to provide an initial reference for dynamic point culling and the second registration; the second matching pose is the high-precision alignment result of the second registration output, which is the local pose mentioned above.

[0063] In some embodiments, as the mathematical basis for the above-described scanning matching process, multi-resolution scanning matching is constructed based on an observation model, which can be expressed by the following formula:

[0064] in, This represents the point cloud measurement data at the current moment. This represents the robot's local pose. This indicates the corresponding map features in the constructed local map. The observation function is used to describe the theoretical observation projection relationship between a given local pose and map features. Measurement noise, representing the uncertainty of the sensor, This model is used to measure the covariance of noise. It provides a theoretical basis for calculating registration errors at subsequent resolution levels.

[0065] In actual execution, when performing the first registration, considering that the point cloud features are relatively sparse at the first resolution, the Normal Distribution Transform (NDT) algorithm can be used to calculate the matching residuals, and the Mahalanobis distance can be constructed using the mean and covariance of the grid distribution to measure the registration error.

[0066] In some embodiments, the matching objective function constructed based on the Normal Distribution Transform (NDT) algorithm can be expressed by the following formula:

[0067] in, This represents the transformation matrix obtained after optimization, used to characterize the transformation relationship corresponding to the first matched pose; Represents the spatial transformation matrix to be optimized; This represents the i-th data point in the point cloud data; and These represent the data points in the constructed local map. The probability distribution mean and covariance matrix of the matched target raster.

[0068] This formula constitutes the matching objective of the Normal Distribution Transform (NDT) algorithm. It uses Mahalanobis distance to measure the matching error, and based on the probabilistic statistical characteristics of raster data, it is suitable for processing dense point clouds. The spatial transformation matrix that minimizes the sum of Mahalanobis distances is solved through optimization. It can accurately calculate the spatial transformation relationship required for point cloud data to be aligned with a local map, thereby achieving stable registration of point clouds.

[0069] Meanwhile, to reduce the probability of the algorithm getting stuck in local optima in the initial stage, in some embodiments, the first registration process may include a strategy of jointly optimizing motion priors and scan matching residuals; for example, the matching objective function including prior constraints can be expressed by the formula:

[0070] in, This represents the first estimated pose, which is used as prior information. Represents the spatial transformation matrix to be optimized; Represents the prior information matrix; This represents the i-th data point in the point cloud data; This indicates a reference point in the local map that matches the data points.

[0071] This formula effectively integrates motion priors and scan matching residuals. It dynamically balances the weights of odometry priors and laser matching observations through a prior information matrix. This joint solution of odometry predictions and laser matching results significantly improves the algorithm's robustness in environments with insignificant geometric features, thus reliably calculating the first matching pose.

[0072] After obtaining the first matching pose, dynamic point culling is performed on the point cloud data based on this pose. By comparing the geometric differences between the point cloud data and the reference map, dynamic point clouds generated by moving objects in the environment are identified and filtered out, thus obtaining the culled point cloud data. This culling process effectively reduces observation interference caused by dynamic obstacles and improves the robustness of subsequent high-precision registration.

[0073] Subsequently, using the first matched pose as the accurate initial reference for iteration, a second registration is performed on the removed point cloud data and the constructed local map at a second resolution higher than the first resolution to obtain the robot's final local pose at the current moment.

[0074] In some embodiments, the high-precision rigid body registration objective function solved when performing the second registration may include, but is not limited to: point-to-point matching objective and point-to-line matching objective.

[0075] In this embodiment, the point-to-point matching target can be expressed by the formula:

[0076] in, This represents the high-precision transformation matrix obtained after optimization in the second registration stage. Represents the spatial transformation matrix to be optimized; This indicates the total number of corresponding point pairs participating in the registration; This represents the i-th data point in the point cloud data after removing dynamic points; This indicates that a local map has been constructed with data points. The reference point for matching.

[0077] This formula constitutes the objective function for point-to-point scan matching. In this formula, the summation term calculates the sum of squared Euclidean distances between the spatially transformed point cloud data and the corresponding reference points in the local map. The spatial transformation matrix is ​​then solved to minimize the sum of these squared errors (i.e., matching errors). It can accurately describe the high-precision alignment relationship between the current point cloud and the local map, thereby completing the rigid body registration calculation in the second registration process.

[0078] The point-to-line matching target can be expressed by the formula:

[0079] in, This represents the high-precision transformation matrix obtained after optimization in the second registration stage. Represents the spatial transformation matrix to be optimized; This indicates the total number of corresponding point pairs participating in the registration; This represents the i-th data point in the point cloud data after removing dynamic points; This indicates that a local map has been constructed with data points. The reference point for matching; The normal vector representing the local line characteristics.

[0080] In the objective function, the sum of squared projected distances of the point cloud data onto the corresponding line feature normal after spatial transformation is calculated, and this is used as a standard to measure the matching error. This calculation process involves solving for the spatial transformation matrix that minimizes the sum of squared projected distances. It accurately describes the alignment relationship between point cloud data and local map, and then completes the rigid body registration calculation in the second registration stage, thereby effectively improving the robustness of the map construction method to measurement noise and sparse feature environment.

[0081] It should be noted that the above registration process includes, but is not limited to, a first resolution and a second resolution. In some embodiments, it may also include a multi-resolution structure with more layers, such as a third resolution. This disclosure does not limit the number of layers used for multi-resolution scanning matching, the scaling ratio between each resolution layer, or the matching objective function used for each layer.

[0082] In practical applications, the first estimated pose can be obtained by a wheeled odometer, a low-precision IMU, or a general kinematic recursive model. Because this embodiment jointly optimizes the motion prior and scan matching residuals during the coarse registration stage, expanding the convergence boundary, it has a high tolerance for the absolute accuracy of the prior information. This allows the system to operate stably without relying on high-precision inertial navigation, helping to reduce system hardware costs.

[0083] According to the map construction method provided in this application, by introducing motion priors for joint coarse registration at a lower first resolution, the convergence boundary is expanded and the risk of computation getting trapped in local extrema is reduced, thus improving the convergence stability in weak feature environments. Based on this, dynamic point culling is performed using the coarse registration pose to effectively identify and filter moving objects, reducing interference from dynamic obstacles. Then, using the coarse registration pose as the initial value, fine registration is performed on the culled point cloud at a higher second resolution, achieving accurate local pose solving. This deep synergy between the multi-resolution mechanism and dynamic filtering significantly enhances the robustness of local registration in complex dynamic and sparse scenes, effectively controlling registration errors in the early stages, thereby improving the global consistency and real-time performance of the map construction.

[0084] In some alternative embodiments, the front end may also employ a feature-point-based scanning matching algorithm, or fuse visual odometry to enhance pose estimation. Furthermore, a dynamic object removal strategy based on semantic segmentation may also be employed.

[0085] It should be noted that although these alternative solutions can provide information completion from different dimensions, they usually require higher computing resources and complex sensor calibration costs in practical applications. The front-end processing mechanism of the present disclosure can achieve better real-time performance and stability while maintaining lower computing resource consumption.

[0086] Step 130: Perform similarity retrieval and geometric consistency verification on the point cloud data, and generate closed-loop constraints based on the verification results and add them to the pose graph; In this step, the closed-loop constraint characterizes the relative transformation relationship between the local pose and the historical local pose.

[0087] For reference Figure 3In S4 of the algorithm, loop closure detection and verification are performed during actual execution to determine whether the robot has returned to the previously visited area. Loop closure detection and verification first extracts feature descriptors from the current frame's point cloud data for similarity retrieval, thereby filtering potential historical candidate pose regions. Then, a precise registration algorithm is used to perform geometric consistency verification between the current frame's point cloud data and the historical point cloud data corresponding to the historical candidate pose regions. If the geometric consistency verification is passed, loop closure is confirmed, and loop closure constraints are generated based on the registration verification results and added to the pose graph as loop constraint edges. Loop closure constraints characterize the relative transformation relationship between the current local pose and the historical local pose, thereby introducing global topological constraints between non-adjacent pose nodes in the pose graph to suppress the cumulative drift risk caused by long-term system operation.

[0088] In some embodiments, step 130 includes: When the robot's motion state reaches the motion state threshold, subgraph descriptor retrieval is performed on the subgraph to obtain closed loop candidate positions. Perform geometric consistency checks on the candidate locations for closed loops, and generate closed loop constraints based on the check results, adding them as constraint edges to the pose graph.

[0089] In this embodiment, the subgraph is obtained by aggregating pose nodes and their corresponding point cloud data.

[0090] The robot's motion state reaches the motion state threshold, which means that the robot's cumulative motion index meets the preset conditions, and is used to trigger closed-loop detection on demand.

[0091] In some embodiments, the robot's motion state reaching a motion state threshold may include at least one of the following situations: The robot's cumulative running time has reached the preset time threshold; The robot's cumulative driving distance has reached the preset mileage threshold.

[0092] It should be noted that the statistical starting point and clearing mechanism for the above-mentioned cumulative running time or cumulative driving distance include, but are not limited to: resetting to zero after each closed-loop detection is triggered, accumulating according to a fixed periodic window, or performing phased zeroing based on a specific subgraph update event. This disclosure does not specifically limit these aspects.

[0093] In actual execution, the robot's current motion state is continuously monitored. When the current motion state meets the preset motion state threshold, the construction of the current subgraph is triggered and the spatial features of the point cloud data contained in the current subgraph are aggregated to calculate a compact vector that represents the global environmental features of the subgraph, i.e., the corresponding current subgraph descriptor is generated. This process provides a low-dimensional feature retrieval benchmark for subsequent large-scale closed-loop matching.

[0094] In some embodiments, subgraph descriptor retrieval is performed on the subgraph obtained by aggregating pose nodes and their corresponding point cloud data to obtain candidate locations for closed loops, including: Calculate the cosine similarity between the subgraph descriptor of the current subgraph and the subgraph descriptors of the historical subgraphs; The positions corresponding to historical subgraphs whose cosine similarity meets the preset similarity conditions are determined as candidate positions for closed loops.

[0095] In this embodiment, the cosine similarity can be expressed by the formula:

[0096] in, Let represent the cosine similarity between the i-th subgraph and the j-th subgraph; This represents the subgraph descriptor of the i-th subgraph; The subgraph descriptor represents the j-th subgraph; and These represent the norms of the corresponding subgraph descriptors.

[0097] In some embodiments, geometric consistency checks are performed on candidate closed-loop locations, and closed-loop constraints are generated based on the check results and added to the pose graph as constraint edges, including: Obtain the relative pose transformation relationship and matching residual between point cloud data and historical point cloud data corresponding to closed-loop candidate positions; Geometric consistency verification of relative pose transformation relationship is performed based on matching residuals; If the geometric consistency check passes, the closed-loop constraints generated based on the relative pose transformation relationship will be added to the pose graph as constraint edges.

[0098] In this embodiment, geometric consistency verification refers to the calculation process of using the overlap or error index of spatial geometric relationships to accurately verify the rigid body transformation matrix of potential closed-loop regions screened out by descriptor similarity, in order to evaluate the alignment reliability of current point cloud observations and historical point cloud observations in three-dimensional space.

[0099] In some embodiments, the relative pose transformation relationship and matching residual between point cloud data and historical point cloud data corresponding to closed-loop candidate positions can be expressed by the following formula:

[0100] The above formula constitutes the objective function for the Iterative Closest Point (ICP) geometric consistency check, where, and Let represent the rotation matrix and translation vector to be solved, respectively; This represents the k-th data point in the point cloud data; This represents the corresponding matching point in the historical point cloud data. In solving the objective function of spatial transformation, the system minimizes the spatial registration error between corresponding point pairs by solving the rotation matrix R and the translation vector t, thereby confirming whether the loop closure holds and establishing the basis for solving the rigid body transformation matrix.

[0101] In some embodiments, the matching residual can be obtained by formulating:

[0102] The above formula constitutes the weighted calculation model for the matching residuals, where, Indicates the introduction of an information matrix The weighted error norm; Represents the spatial registration error vector; The information matrix is ​​represented by this formula. This formula defines the weighted error norm, a common mathematical representation in graph optimization and filtering. In the weighted calculation model, the system uses the information matrix to assign different weights to different directions of the spatial registration error, and performs weighted projection calculations on the spatial registration error based on the calculated optimal pose. This objectively quantifies and obtains the matching residuals that reflect the final registration quality.

[0103] In some embodiments, geometric consistency verification of the relative pose transformation relationship based on the matching residual can be expressed by the following formula:

[0104] The above formula constitutes the judgment model for geometric consistency verification. This formula is the chi-square test threshold. If the matching residual exceeds the chi-square test threshold, the corresponding relative pose transformation relationship is regarded as an unreliable constraint and is closed-loop filtered out. This represents the matching residual calculated under the optimal pose; This represents the preset chi-square test threshold. In the decision model, the system performs geometric consistency checks by comparing whether the matching residuals are within the chi-square test threshold. If the matching residuals are less than the chi-square test threshold, the check is deemed successful, thereby identifying and filtering unreliable observations.

[0105] In some embodiments, if the geometric consistency check passes, the closed-loop constraints generated based on the relative pose transformation relationship are added as constraint edges to the pose graph, wherein the closed-loop constraints can be expressed by the formula:

[0106] The above formulas constitute the generative model of closed-loop constraints, where, This represents the generated closed-loop constraints; and These represent the historical pose node and the current pose node in the pose graph, respectively. This represents measurement noise. In the generative model, after the geometric consistency check passes, the system transforms the calculated relative pose transformation relationship into a spatial closed-loop edge with topological constraints and adds it to the pose graph. This introduces a spatial constraint structure between non-adjacent nodes, thereby reducing the risk of false loop closures caused by local environmental similarity.

[0107] According to the map construction method provided in this application, on-demand triggering of loop closure detection is achieved by setting a motion state threshold, effectively balancing environmental coverage and computational efficiency. During the detection process, a coarse screening is first performed using subgraph descriptor retrieval to improve the recall rate of candidate positions. Then, a geometric consistency check is used to filter out false detections caused by feature confusion, achieving a loop closure screening that balances real-time performance and accuracy. Finally, the verified loop closure constraints are added to the pose graph as global topological constraint edges, effectively suppressing the cumulative drift under long-term system operation, significantly reducing map closure error and improving loop closure stability, thereby enhancing the global consistency and real-time performance of map construction.

[0108] In some alternative embodiments, the above-described loop closure detection mechanism can also be replaced by a loop closure detection method based on semantic information or pure visual features to provide loop closure constraints in specific scenarios rich in visual features. This disclosure does not specifically limit this.

[0109] Step 140: Perform global consistency correction on the pose graph using front-end constraints and closed-loop constraints to obtain the global pose; In actual implementation, refer to, for example Figure 2 The S5 algorithm performs robust graph optimization. Global consistency correction is achieved by performing back-end robust graph optimization on the entire pose graph. Based on a nonlinear least squares optimization method, it aims to globally correct deviations in the topological positional relationships calculated in the early stages of the system. When the robot's trajectory drifts due to prolonged operation, preventing the path from completely closing, back-end robust graph optimization uniformly adjusts the intermediate pose nodes in the pose graph to ensure the entire motion path meets the global topological closure requirement, thereby reducing the system's cumulative error.

[0110] In this process, a robust kernel function structure can be introduced into the optimization calculation to penalize and reduce the weight of excessive constraint errors when there are abnormal pseudo-closed-loop observations, thereby reducing the risk of global topology map distortion caused by the introduction of erroneous closed-loop data.

[0111] In some embodiments, step 140 includes: A global objective function is constructed based on the residual terms corresponding to the front-end constraints and the closed-loop constraints, respectively. The residuals in the global objective function are weighted to obtain the weighted global objective function. The weighted global objective function is solved iteratively to obtain the pose increment of the pose nodes; The pose of the pose nodes is updated by overlaying based on the pose increment, and the updated pose nodes are combined into a global pose.

[0112] In this embodiment, the global objective function is used to perform a joint topological representation of the front-end scan matching results and the loop closure detection results with multi-source constraints; Weighting is used to adjust the actual contribution weights of constraints of different properties to the global optimization results and to suppress anomalous outliers. Pose increment is used to quantize the spatial adjustment vector of each pose node in nonlinear iteration; Superposition updates are used to achieve lossless composite of pose states in special Euclidean group manifold spaces.

[0113] In some embodiments, a global objective function is constructed based on the residual terms corresponding to the front-end constraints and the closed-loop constraints, which can be expressed by the following formula:

[0114] The above formula constitutes a collaborative optimization model for the global objective function. This formula represents the overall objective of pose graph optimization and is implemented using a robust kernel. Suppressing abnormal constraints allows the global pose to achieve optimal consistency under multiple constraints; among which, This represents the global pose state variable to be optimized. This represents the set of all constraint edges in the pose graph, which includes front constraint edges and closed-loop constraint edges. This represents the observation constraint relationship between node i and node j; This represents the robust kernel function. In the collaborative optimization model, the system constructs an overall optimization framework by minimizing the residuals corresponding to all front-end constraints and closed-loop constraints, which serves as the mathematical benchmark for global consistency correction.

[0115] In some embodiments, the residual terms in the global objective function are weighted to obtain a weighted global objective function, wherein the error weighting norm and robust kernel weighting function involved can be expressed by the following formula:

[0116]

[0117] The above formula constitutes the weighted calculation model for the matching residuals, where the piecewise function is defined by the Huber robust kernel. As a threshold for change, when the error becomes too large, the penalty shifts from quadratic to linear to reduce the impact of outliers; simultaneously, in the weighted calculation model, Indicates the introduction of an information matrix The weighted error norm is used to assign corresponding weights to different error directions; Represents the topological error vector for spatial registration; Represents the information matrix. This represents the square of the weighted error. This represents the inflection threshold of the robust kernel. In the weighted calculation model, the system uses the information matrix to reasonably scale the anisotropic error, and combines it with the robust kernel function to penalize and reduce the weight of abnormally large errors that exceed the inflection threshold, thereby reducing the risk of global topology map distortion caused by the introduction of erroneous closed-loop data.

[0118] In some embodiments, the weighted global objective function is iteratively solved to obtain the pose increment of the pose nodes. The process of linearizing the solution using the normal equation can be expressed by the following formula:

[0119] The above formula constitutes the iterative solution model for optimizing pose increments, and this formula is the linearized normal equation. The Jacobian matrix representing the registration error with respect to pose; An optimization information matrix used to characterize constraint weights; This represents the pose increment during the iterative solution process, used for Gauss-Newton or Levenberg-Marquardt (LM) solutions; This represents the spatial registration error vector. In the nonlinear iterative optimization, the system uses normal equations to calculate the convergent optimal pose increment. During this process, the termination condition for the iterative solution is set as either the residual decrease is lower than a preset residual threshold, or the number of iterations reaches a preset iteration limit. When the termination condition is met, the system stops iterating and outputs the final pose increment, which serves as a quantitative basis for the unified adjustment of the positions of each pose node.

[0120] In some embodiments, the pose of the pose nodes is updated by overlay based on the pose increment, and the updated pose nodes are combined into a global pose. The overlay update can be expressed by the formula:

[0121] The above formula constitutes the manifold update model for pose state, and this formula is the pose update rule. The composite operation on SE(2) ensures that the update is performed in the correct geometric space. At the same time, in the manifold update model, the system superimposes the calculated pose increment onto the current pose node through the manifold composite operation on a special Euclidean group surface to maintain the update calculation in the accurate geometric space, and dynamically generates a high-precision global pose by combining all the updated pose nodes.

[0122] According to the map construction method provided in this application, during the process of performing global consistency correction on the pose graph using front-end constraints and closed-loop constraints, the influence of abnormal constraints can be effectively weakened and the risk of map distortion can be reduced by constructing a global objective function containing a robust kernel function and weighting the residuals. Furthermore, by iteratively solving and optimizing the pose state of each node, global correction of the cumulative deviation of the motion trajectory is achieved. The pose nodes are uniformly adjusted to meet the topology closure requirements, which can gradually alleviate the cumulative drift and improve the map closure, thereby improving the global consistency and real-time performance of the map construction.

[0123] In some alternative embodiments, a combination of switchable constraints and graph optimization can be used in the back-end optimization stage to replace the robust kernel function structure described above, in order to identify and remove abnormal constraints. However, compared to these alternatives, the robust kernel function used in the embodiments of this disclosure offers a more balanced performance in terms of computational real-time performance and convergence stability when dealing with high-frequency dynamic noise.

[0124] By abstracting both front-end and closed-loop constraints into topological constraint edges with a unified data structure, the system establishes a modular subgraph structure. This structure is easily extensible at the software level and decoupled from specific sensor underlying hardware and mechanical chassis, facilitating flexible deployment on different hardware platforms and in different scenarios.

[0125] Step 150: Project the point cloud data onto the global coordinate system based on the global pose to construct and output a map.

[0126] In this step, refer to, for example Figure 2 The S6 system, as shown, performs occupancy map processing. Based on the optimized global pose, it dynamically projects the point cloud data from each frame onto a unified global coordinate system to achieve the fusion of all point cloud data into the raster map. In the raster map, each grid cell represents the probability that the corresponding spatial area is occupied by an obstacle. By combining the global pose with the current laser observation, the system calculates and updates the occupancy state of each grid cell, thereby generating a structurally stable and globally consistent raster map for output.

[0127] In some embodiments, step 150 includes: Extract the local coordinates of each data point in the point cloud data; Based on the global pose, determine the corresponding coordinate transformation matrix; The local coordinates of each data point are transformed and calculated using a coordinate transformation matrix to obtain the global coordinates of each data point in the global coordinate system. A map is constructed and output by stitching together data points containing global coordinates.

[0128] In this embodiment, local coordinates are used to characterize the original spatial position in the sensor coordinate system; The coordinate transformation matrix is ​​used as a rotation and translation mathematical operator that carries global pose information; Stitching is used to achieve spatial state fusion and accumulation of multiple frames of independent point cloud data under a unified global reference frame.

[0129] In some embodiments, the occupancy state of the raster map is updated and calculated during the construction and output of the map, which can be expressed by the formula:

[0130] The above formula constitutes the log-odds update model for raster maps. This formula is the log-odds update formula for raster maps, and the system incorporates the current observation probability. with prior probability To continuously update the occupancy status of each grid cell; among which, This represents the logarithmic occupancy probability of grid cell m at the current moment after its update; This represents the historical logarithmic occupancy probability of grid cell m at the previous time step; Indicates combining current observation data The current observation occupancy probability is obtained; This represents the prior logarithmic occupancy probability of grid cell m. In the update model, the system uses the logarithmic ratio to transform complex probability multiplication into addition. By adding the information gain from the current observation to the historical state and subtracting the prior influence, it achieves dynamic recursion of the occupancy state of each grid cell, thereby generating a structurally stable and globally consistent raster map.

[0131] During the raster update process, the complex probability multiplication is transformed into efficient scalar addition operations using a logarithmic ratio. Combined with the aforementioned multi-resolution hierarchical matching in the front end and the closed-loop on-demand triggering mechanism, the overall computational load of the system is significantly reduced. This allows the algorithm to maintain real-time operation on medium-power platforms, making it suitable for engineering implementation and large-scale deployment.

[0132] According to the map construction method provided in the embodiments of this application, based on obtaining high-precision global pose through front-end multi-resolution matching, closed-loop geometric verification and global correction, the local coordinates of each data point are dynamically projected to the global coordinate system using a coordinate transformation matrix for spatial stitching. Combined with the logarithmic ratio update model, the occupancy state of each grid cell is recursively deduced, realizing the spatial fusion and state accumulation of multi-frame point cloud data. This can effectively suppress the influence of laser observation noise, accurately characterize the obstacle distribution in the environment, and achieve stable and high-quality two-dimensional grid map update output, thereby improving the global consistency and real-time performance of map construction.

[0133] The following is for reference. Figure 2 The overall SLAM flow shown illustrates the sequential execution relationship of each step in the method provided in this embodiment of the invention.

[0134] The system first executes step S1 for data acquisition and preprocessing (laser / point cloud filtering). During this process, the acquired laser scanning data undergoes preprocessing operations, which may include, but are not limited to, downsampling the original laser point cloud, removing outliers, and synchronizing time between multiple sensors to provide a regular and high-quality data foundation for subsequent registration calculations. The system then proceeds to step S2 to perform multi-resolution scan matching + motion prior (scanmatching + odometry) processing. This involves combining multi-resolution scan matching with motion constraints to obtain the robot's initial pose estimate. In the matching solution, the system can selectively use a point-to-line registration model or a normal distribution transform (NDT) registration model as the matching objective function to enhance the robustness of pose estimation to irregular or sparse environments. Next, the system enters step S3 to perform pose graph construction and front-end constraint (pose graph) processing. Based on the calculated motion trajectory, a topological pose graph structure is constructed and front-end constraints are dynamically added. In the topological pose graph, the pose graph edges contained therein are mainly composed of odometry constraints between adjacent frames, subgraph constraints between the current frame and the subgraph, and subsequently detected loop closure constraints.

[0135] During the basic forward main flow of the system consisting of steps S1, S2 to S3, the loop closure detection and verification process performed in step S4 runs in parallel with the front-end main flow and is triggered periodically. In actual engineering execution, the triggering timing of step S4 can be conditionally set based on the cumulative mileage threshold or the threshold for the number of newly generated subgraphs set internally by the system. This reduces the computational resource consumption caused by uninterrupted matching and reduces the computational burden caused by excessively frequent triggering. After step S4 is triggered, the system internally performs subgraph retrieval and closed-loop geometric consistency verification. If the closed-loop matching is confirmed, a highly deterministic closed-loop constraint is generated. Subsequently, this closed-loop constraint is added as a constraint edge to the pose graph and triggers the process to step S5.

[0136] When the pose graph introduces new closed-loop constraints or meets the update increment conditions, the system proceeds to step S5 to perform robust graph optimization (graph optimization) processing. This involves using a nonlinear least squares optimization method to perform overall correction and adjustment of all multi-source topological constraints in the entire pose graph, thereby gradually eliminating the cumulative drift caused by long-term operation and ultimately obtaining a globally consistent global pose. Finally, the precise global pose calculated in step S5 is directly provided to step S6 for map update output (occupancy map) processing. The system uniformly projects and fuses historical point cloud data into the global grid map based on the global pose, and dynamically updates the occupancy probability of each grid cell, thus ultimately constructing and outputting a stable and consistent grid map.

[0137] To achieve efficient collaboration among the running nodes, the module scheduling mechanism corresponding to each of the aforementioned steps is as follows: the front-end matching module for step S2 is triggered frame-by-frame; the loop closure detection module for step S4 is triggered based on time / displacement thresholds; and the back-end optimization module for step S5 executes incremental updates based on constraints. Each module (including the data acquisition and preprocessing module for step S1, the pose graph construction module for step S3, and the map update output module for step S6) interacts on the computing platform via a data queue. Furthermore, each module can run in a single-machine multi-threaded or asynchronous task environment, and the data structure employs queue caching to ensure real-time performance and controllable latency.

[0138] Those skilled in the art should understand that the various inventive points disclosed in the embodiments of this application (including but not limited to front-end multi-resolution scanning matching, dynamic point culling, closed-loop on-demand triggering based on motion state thresholds, and back-end optimization by introducing robust kernel functions, etc.) can be implemented individually or in any combination according to actual business needs. Any combination of the above-mentioned technical features can improve the mapping stability and global consistency of the 2D SLAM system in complex or dynamic scenes from different dimensions, without departing from the spirit and scope of this application.

[0139] This application also provides a map construction method.

[0140] During the research and development process, the inventors also discovered that while 2D Simultaneous Localization and Mapping (2D SLAM) technology is widely used in navigation fields such as service robots and automated guided vehicles, in large-scale, long-distance mapping tasks, the front-end odometry is prone to cumulative drift. Related systems typically rely on loop closure detection mechanisms to eliminate errors and maintain global consistency. However, when dealing with highly repetitive structural scenes such as corridors and similar rooms, these technologies often rely solely on scene descriptors for historical frame retrieval, which can easily lead to spatial perception confusion. Furthermore, the lack of an effective multi-level verification mechanism can easily cause false loop closure detections. False loop closure detections may cause the system to introduce incorrect loop constraint edges into the back-end pose graph. In subsequent graph optimization, these incorrect constraints not only fail to eliminate the original drift but may even cause incorrect associations and offsets of trajectory nodes, leading to topological errors and structural distortions in the global grid map, thus affecting the reliability of the map in actual navigation and path planning tasks.

[0141] To address the issue of false loop closure detections in complex scenarios, which can lead to map distortion, the inventors, after in-depth research, designed a map construction method. This method includes: acquiring point cloud data to characterize the robot's environment; in response to a target triggering indicator reaching a trigger threshold, locally fusing the point cloud data to generate a corresponding sub-graph; extracting sub-graph descriptors and performing similarity searches in a historical sub-graph library based on these descriptors to determine candidate sub-graphs for loop closure; performing geometric consistency checks between the sub-graphs and the candidate sub-graphs to generate loop closure constraints and adding them to the pose graph; and performing graph optimization on the pose graph after adding loop closure constraints to correct the robot's pose and construct the map.

[0142] According to the map construction method provided in the embodiments of this application, point cloud data is locally fused by a target trigger index, and a corresponding sub-graph is generated when the trigger threshold is reached. On this basis, unreliable candidate constraints are effectively eliminated by using sub-graph descriptor retrieval and geometric consistency verification, while taking into account the accuracy and real-time performance of loop closure detection. Furthermore, the generated loop closure constraints are added to the pose graph for collaborative optimization, which reduces trajectory accumulation drift under long-term operation, thereby improving the global consistency and real-time performance of map construction.

[0143] like Figure 3 As shown, the map construction method includes steps 310, 320, 330, 340, and 350.

[0144] Step 310: Obtain point cloud data to characterize the robot's environment; In this step, point cloud data refers to a set of data obtained by environmental perception sensors that contains spatial location information of the surfaces of various obstacles in the robot's surrounding environment. It can intuitively represent the geometric topology and physical boundaries of the local environment in which the robot is currently located.

[0145] In some embodiments, point cloud data can be acquired based on environmental perception devices such as two-dimensional LiDAR, three-dimensional LiDAR, depth camera (RGB-D camera) or millimeter-wave radar.

[0146] In actual operation, environmental sensing devices are installed on or connected to the robot to collect raw scan data of the surrounding environment according to the sampling frequency.

[0147] refer to Figure 2 In some embodiments, the "S1: Data Acquisition and Preprocessing" step can perform preprocessing operations such as point cloud filtering on the raw scanning data acquired by environmental perception devices such as LiDAR to obtain point cloud data. In actual execution, since the acquired raw scanning data may contain environmental noise, abnormal error points, and scanning distortion caused by the robot's own movement, preprocessing operations such as point cloud filtering, time synchronization, and motion distortion correction can be performed on the raw scanning data acquired by the environmental perception devices to obtain point cloud data. The point cloud filtering operation can include point cloud downsampling and outlier removal.

[0148] By downsampling point clouds, the number of point clouds can be reduced while preserving the geometric features of the environment, thereby reducing the computational resource consumption in the subsequent front-end matching process; performing outlier removal helps to reduce the interference of environmental noise or obvious erroneous points on spatial registration accuracy; performing time synchronization enables the collected data to be consistent with the robot's motion state in time; performing motion distortion correction can be used to correct scanning distortion errors caused by the robot's own movement.

[0149] The synergistic effect of the above preprocessing operations can effectively improve the quality and consistency of the input data, laying the foundation for subsequent high-precision mapping.

[0150] In some embodiments, after step 310, the map construction method further includes: Perform scanning and matching processing on the point cloud data, and combine it with motion prior information to obtain the robot's first pose information; The front-end constraints are generated based on the first pose information and added to the pose graph so that the pose graph contains both front-end constraints and closed-loop constraints.

[0151] In this embodiment, the front-end constraint is a constraint edge that characterizes the relative pose transformation relationship between the robot's first pose information at adjacent time points.

[0152] Scan matching processing refers to the process of spatially aligning the point cloud data of the current frame with historical maps or historical frame data using a multi-resolution matching algorithm to determine the robot's relative displacement.

[0153] Motion prior information refers to data collected by sensors such as the robot's wheel odometry that characterizes the relative motion trend between adjacent moments of the robot. It is used to provide an initial reference pose for the aforementioned scan matching process.

[0154] The first pose information refers to the spatial pose state of the robot in the local coordinate system, which is calculated by fusing the scan matching results and motion prior information. It is used as the basic node data for subsequent construction of the pose graph.

[0155] Closed-loop constraints are global observation constraints established by identifying whether the robot returns to its historical position. The specific acquisition method and verification mechanism will be explained in detail later.

[0156] In actual execution, the logical unit that performs the above-mentioned scanning matching process and front-end constraint generation process can be configured as a front-end matching module.

[0157] In some embodiments, the front-end matching module is triggered based on data output events from the environmental sensing device. For example, the front-end matching module may employ a frame-by-frame triggering mechanism.

[0158] After acquiring the current frame point cloud data output by the environmental sensing device, this module responds to the data frame by performing the corresponding front-end registration and generating the front-end constraints for that frame. When the environmental sensing device outputs a new point cloud data frame, this module can respond to the data frame by performing the corresponding front-end registration and constraint generation operations.

[0159] In actual implementation, refer to Figure 2 In the “S2: Multi-resolution scan matching + motion prior”, when performing scan matching, the motion prior information output by sensors such as odometers is first obtained and used as the initial value for spatial registration iteration. Then, the point cloud data of the current frame is iteratively adjusted using registration algorithms such as Iterative Closest Point (ICP) or Normal Distribution Transform (NDT) to make the current point cloud spatially aligned with historical frame data or local map, thereby solving the first pose information.

[0160] Meanwhile, the scanning matching process can be performed based on a multi-resolution strategy, that is, coarse matching is first performed at a lower resolution level, and then fine optimization is performed at a higher resolution level. The combination of the above multi-resolution mechanism and motion prior information helps to reduce the probability of the registration algorithm getting stuck in local extrema, thereby improving the convergence stability of the front-end localization results in complex scenarios.

[0161] In some embodiments, a scan matching process is performed on the point cloud data, and the robot's first pose information is obtained by combining prior motion information, including: Perform multi-resolution scanning matching on point cloud data and obtain prior motion information inferred from the robot's motion model; The registration residuals from multi-resolution scanning matching are jointly optimized and solved with motion prior information to obtain the first pose information.

[0162] In the above processing, the spatial alignment accuracy is refined layer by layer by multi-resolution scanning matching, and the joint calculation is performed by combining the prior state that characterizes the robot's motion trend. This helps to balance the weight of sensor observation and the underlying odometry, thereby improving the system's localization robustness in complex or feature-sparse environments.

[0163] In actual implementation, refer to Figure 2 In step S2, the current motion prior information is first calculated based on the pose state of the previous moment, and then used as the initial value to guide multi-resolution scanning matching.

[0164] In some embodiments, during joint solution, the joint registration objective function including prior constraints can be expressed by the following formula:

[0165] in, This represents the initial reference pose (or prior pose state) as prior information. Represents the spatial transformation matrix to be optimized; Represents the prior information matrix; This represents the i-th data point in the point cloud data; This indicates a reference point in the local map that matches the data points.

[0166] In some embodiments, the above-mentioned scanning matching process can be implemented based on point-to-line matching targets, point-to-point matching targets, or normal distribution transformation (NDT) matching targets, or a feature point-based scanning matching algorithm can be used.

[0167] In this embodiment, the point-to-line matching or NDT matching algorithm can use the local line feature normal vector or grid probability distribution to measure the registration error, which helps to improve the system's ability to resist environmental noise and sparse features.

[0168] If point-to-point matching is used, the objective function for solving the pose can be expressed as:

[0169] in, This represents the high-precision transformation matrix obtained after solving in the fine optimization stage...; Represents the spatial transformation matrix to be optimized; This indicates the total number of corresponding point pairs participating in the registration; This represents the i-th data point in the point cloud data; This indicates that a local map has been constructed with data points. The reference point for matching.

[0170] If point-to-line matching is used, its objective function can be expressed as:

[0171] in, This represents the high-precision transformation matrix obtained after solving in the fine optimization stage; Represents the spatial transformation matrix to be optimized; This indicates the total number of corresponding point pairs participating in the registration; This represents the i-th data point in the point cloud data; This indicates that a local map has been constructed with data points. The reference point for matching; The normal vector representing the local line characteristics.

[0172] If the NDT matching algorithm is used, its objective function can be expressed as:

[0173] in, This represents the transformation matrix obtained after optimization, used to characterize the transformation relationship corresponding to the first matched pose; Represents the spatial transformation matrix to be optimized; This represents the i-th data point in the point cloud data; and These represent the data points in the constructed local map. The probability distribution mean and covariance matrix of the matched target raster.

[0174] In some alternative embodiments, the scan matching process may also employ a feature-point-based scan matching algorithm.

[0175] In this embodiment, the computing unit extracts edge feature points and planar feature points from the point cloud data in advance and performs spatial alignment calculation based on the feature points. This can effectively reduce the amount of data involved in the registration calculation while ensuring the accuracy of pose estimation, which helps to further improve the real-time performance and computing efficiency of the front-end positioning.

[0176] In addition, in some embodiments, to further enhance the observation capabilities of the front-end matching module, a dynamic target elimination strategy based on semantic segmentation can be adopted, or visual odometry can be fused to construct a multimodal front-end.

[0177] In this embodiment, the introduction of a semantic segmentation model can identify and remove potential moving obstacles (such as pedestrians and vehicles) in point cloud data, thereby reducing the interference of highly dynamic environments on spatial registration; while the fusion of visual odometry can use the image texture information of the camera to make up for the lack of data in a single environment perception device in a scene with sparse structural features, thereby improving the robustness of pose estimation.

[0178] It should be noted that while the above enhancement scheme improves the robustness of the system, it will also increase the consumption of hardware computing resources and the joint calibration cost between multiple sensors. In practical applications, it can be adaptively configured according to the robot's computing platform conditions and hardware architecture.

[0179] refer to Figure 2 In the section "S3: Constructing a pose graph + front-end constraints," in some embodiments, front-end constraints are generated based on the first pose information and added to the pose graph, including: Calculate the relative pose transformation matrix between the first pose information of adjacent frames; The inverse position error covariance matrix generated by the scan matching process is extracted as the information matrix; The relative pose transformation matrix and the associated information matrix are added to the pose graph as local constraint edges.

[0180] In this embodiment, the relative pose transformation matrix corresponds to the high-precision spatial transformation relationship obtained after optimizing and solving the matching objective functions mentioned above (e.g., corresponding to the point-to-point, point-to-line, and NDT calculation formulas mentioned earlier). ); The information matrix is ​​the confidence matrix extracted from the registration residual evaluation in the optimization process described above. For example, it corresponds to the inverse position error covariance matrix obtained when solving the aforementioned objective function.

[0181] By constructing the above two together as front-end constraints, a reliable weighting basis can be provided for subsequent graph optimization, thereby giving higher optimization weights to high-quality matching results and effectively reducing the risk of topological distortion caused by feature sparsity or dynamic interference.

[0182] Step 320: In response to the target trigger indicator reaching the trigger threshold, perform local fusion of point cloud data to generate the corresponding sub-map; In this step, the target trigger index refers to the cumulative state of the robot during the mapping process, which is used as the basis for determining whether the system triggers the generation of a local subgraph.

[0183] In practical applications, different types of target trigger indicators correspond to their own trigger thresholds; the specific values ​​of each trigger threshold can be customized by technicians based on actual needs, or obtained through calibration of a large amount of experimental data. This application embodiment does not impose specific limitations on this.

[0184] In some embodiments, the target triggering metric includes at least one of time information, mileage information, and the number of submaps generated.

[0185] In some embodiments, step 320 includes: Acquire time information, mileage information, and the number of sub-maps generated during the robot's operation; If at least one of the time information, mileage information, and the number of generated sub-maps reaches the corresponding trigger threshold, the point cloud data and its corresponding pose information are locally fused to generate a sub-map.

[0186] In actual execution, based on the pose information corresponding to each frame of point cloud data, the multi-frame point cloud data acquired in the current triggering cycle is uniformly transformed into a local spatial coordinate system; then, the multi-frame point cloud data is spatially aligned and overlaid and updated in the local spatial coordinate system to construct a subgraph containing the local environment structure.

[0187] According to the map construction method provided in the embodiments of this application, by setting target trigger indicators such as cumulative mileage, time information or the number of sub-maps generated, point cloud data and their pose information are locally fused when the corresponding trigger threshold is met. This reduces the additional computational overhead caused by frequent sub-map generation, balances computational efficiency while taking into account the environmental coverage, and helps to realize the dynamic updating and optimization of map information, thereby improving the global consistency and real-time performance of map construction.

[0188] Step 330: Extract the subgraph descriptors of the subgraphs and perform similarity retrieval in the historical subgraph library based on the subgraph descriptors to determine the candidate subgraphs for loop closure; In this step, the historical subgraph library refers to the collection of subgraphs that the robot has built and stored in its historical running trajectory.

[0189] A closed-loop candidate subgraph refers to a historical subgraph in the historical subgraph library that has the highest similarity (or meets the preset matching criteria) to the currently generated subgraph at the feature level. Since strict spatial geometric consistency verification has not yet been performed, it is used as a candidate reference object for the occurrence of loop closure.

[0190] In some embodiments, the system may perform feature-level similarity retrieval calculations based on algorithms such as cosine similarity, Euclidean distance, or Manhattan distance. This application does not limit the specific similarity evaluation method.

[0191] In actual execution, the computing unit first extracts and encodes features of the currently generated subgraph to generate feature vectors (or feature matrices) that can characterize the global geometric structure or spatial topological distribution of the subgraph, which serve as subgraph descriptors. Then, the subgraph descriptors of the current subgraph are compared with each historical subgraph descriptor stored in the historical subgraph library, and the similarity scores between them are calculated. Finally, historical subgraphs whose similarity scores meet the preset retrieval conditions (e.g., the highest score and greater than the set similarity threshold) are selected and identified as closed-loop candidate subgraphs.

[0192] In some embodiments, step 330 includes: Feature extraction is performed on the point cloud data contained in the subgraph to generate feature vectors that represent the global attributes of the subgraph, which serve as subgraph descriptors; The cosine similarity between the subgraph descriptor and each historical subgraph descriptor in the historical subgraph library is calculated to obtain the similarity. Historical subgraphs with similarity greater than the similarity threshold are selected as candidate subgraphs for loop closure.

[0193] Feature extraction from point cloud data refers to extracting features from the original point cloud that can reflect the geometric or statistical laws of the environment and encoding them into a fixed-dimensional numerical matrix or array, i.e., feature vector (i.e., subgraph descriptor).

[0194] Global attributes refer to the feature vector representing the macroscopic topological structure and contour of the entire subgraph, rather than local features (such as individual corner points or line segments) that are easily affected by occlusion or repeated textures, thereby effectively reducing mismatches caused by local structural similarities.

[0195] In some embodiments, the cosine similarity calculation process can be expressed by the formula:

[0196] in, The cosine similarity between a subgraph and a given historical subgraph is expressed. and These are the subgraph and the subgraph descriptor corresponding to the historical subgraph, respectively. and These represent the norms of the corresponding subgraph descriptors. In the above calculation, the smaller the angle between the eigenvectors, the closer the cosine value is to 1, indicating that the corresponding geometric structures are more similar.

[0197] In subsequent execution, threshold filtering based on similarity thresholds can quickly eliminate obviously irrelevant historical subgraphs with extremely low computational cost, thereby narrowing the solution range for subsequent time-consuming spatial alignment operations (such as the ICP algorithm) and significantly improving the overall efficiency of loop closure detection.

[0198] According to the map construction method provided in this application, feature extraction is performed on subgraphs to construct subgraph descriptors. The current subgraph descriptor is then compared with historical subgraph descriptors in the historical subgraph library based on similarity, thereby improving the recall capability and retrieval efficiency of loop closure candidate subgraphs. Simultaneously, threshold filtering using a similarity threshold can narrow the solution range for subsequent spatial alignment operations, reducing the computational overhead in the loop closure detection process. This helps improve the accuracy of loop closure matching and the efficiency of map information updates, thereby enhancing the global consistency and real-time performance of map construction.

[0199] Step 340: Perform geometric consistency verification between the subgraph and the candidate subgraph for closed loops, generate closed loop constraints, and add them to the pose graph; In this step, refer to Figure 2 The "S4: Loop Closure Detection and Verification" section performs low-level spatial point cloud alignment verification on the subgraph and the candidate loop closure subgraph. If the geometric consistency check passes, the effective relative pose relationship between the two is established to generate closed-loop constraints, and these constraints are added to the pose graph as global constraint edges.

[0200] In some embodiments, step 340 includes: Spatial alignment is performed on the subgraph and the candidate subgraph for closed loops to obtain the relative pose transformation relationship between them, and the matching residual after alignment is calculated. The matching residuals are weighted and evaluated, and the effective relative pose transformation relationship is obtained based on the weighted evaluation results. Closed-loop constraints are generated based on effective relative pose transformation relationships, and these closed-loop constraints are added to the pose graph as constraint edges.

[0201] In this embodiment, spatial alignment refers to the process of using an iterative registration algorithm to make the subgraph and the candidate closed-loop subgraph fit together to the maximum extent in three-dimensional space, that is, to perform geometric consistency verification.

[0202] Matching residuals represent the distance error between corresponding point cloud data after spatial alignment.

[0203] Weighted evaluation refers to the introduction of an information matrix representing the confidence of the matching to penalize and test the matching residuals, in order to identify and eliminate false matches (i.e. false loops) caused by repetitive textures or similar structures, so that the final generated loop constraints have true geometric validity.

[0204] In some embodiments, registration algorithms such as the Iterative Closest Point (ICP) algorithm and the Normal Distribution Transform (NDT) algorithm can be used to perform spatial alignment processing in order to achieve geometric consistency verification.

[0205] In actual execution, the computing unit constructs a registration objective function for the subgraph and the candidate subgraph for loop closure, and solves it using an iterative optimization algorithm. By continuously solving the rotation and translation matrices, the point cloud of the current subgraph is made to fit the point cloud of the candidate subgraph for loop closure as closely as possible, thereby minimizing the spatial distance error between corresponding point pairs. The relative transformation matrix calculated in this way represents the relative pose transformation relationship when loop closure occurs.

[0206] In some embodiments, the Iterative Closest Point (ICP) algorithm is used to perform the above spatial alignment process to obtain the relative pose transformation relationship. Its objective function can be expressed by the formula:

[0207] in, and Let represent the rotation matrix and translation vector to be solved, respectively; This represents the spatial coordinates of the k-th point in the point cloud data; Let R and t represent the point cloud spatial coordinates corresponding to the candidate subgraphs in the closed-loop subgraphs. By minimizing the R and t calculated from the above objective function, the relative pose transformation relationship between the two subgraphs is constituted.

[0208] In some embodiments, the matching residuals are weighted and evaluated, and based on the weighted evaluation results, an effective relative pose transformation relationship is obtained, including: By combining the information matrix with matrix operations on the matching residuals, the weighted error is calculated. If the weighted error is less than the consistency determination threshold, the corresponding relative pose transformation relationship is determined to be valid.

[0209] In this embodiment, the matching residuals are weighted by introducing an information matrix that characterizes the matching confidence level in order to calculate the weighted error.

[0210] The consistency determination threshold is used to verify the reliability of the weighted error.

[0211] In some embodiments, a consistency determination threshold can be set based on the chi-square test, Mahalanobis distance criterion, or 3-sigma criterion.

[0212] If the weighted error is less than the consistency judgment threshold, the spatial alignment result is deemed valid. If the weighted error is greater than or equal to the consistency judgment threshold, it is considered a mismatch and is eliminated, thereby effectively reducing the risk of map distortion caused by false loop closure.

[0213] In some embodiments, the calculation of the weighted error and the determination logic of the consistency threshold can be expressed by the following formula:

[0214]

[0215] in, Represents the error-weighted norm; This represents the error direction vector (i.e., the matching residual) extracted after registration and alignment. This represents the information matrix extracted from the scanning and matching process (used to assign different weights to errors in different dimensions). This represents the consistency threshold (e.g., the chi-square test threshold). When the error-weighted norm is less than... When the relative pose transformation is determined, the corresponding relative pose transformation is identified as an effective closed loop.

[0216] In some embodiments, the closed-loop constraints generated based on effective relative pose transformation relationships can be expressed by the following formula:

[0217] in, The observations that characterize the relative pose transformation relationship between node i and node j are added to the pose graph as closed-loop constraint edges (also known as loop edges) during the map construction process. and These represent the historical pose node and the current pose node in the pose graph, respectively. This indicates the observation noise present during the observation process.

[0218] In some alternative embodiments, the above-described loop closure detection mechanism can also be replaced by a loop closure detection method based on semantic information or pure visual features to provide loop closure constraints in specific scenarios rich in visual features. This disclosure does not specifically limit this.

[0219] According to the map construction method provided in this application, by performing geometric consistency verification on the subgraph and the candidate subgraph for closed loops, and combining the matching residuals and information matrices for weighted evaluation, the candidate results for closed loops can be further verified, which helps to reduce mismatches caused by repetitive environmental structures or similar local features. Simultaneously, by generating closed loop constraints based on the relative pose transformation relationships verified through consistency verification and adding them to the pose graph, more reliable global constraint information can be provided for subsequent pose optimization, which is beneficial to improving the stability and accuracy of the map optimization process, thereby enhancing the global consistency and real-time performance of map construction.

[0220] Step 350: Perform graph optimization on the pose graph after adding closed-loop constraints to correct the robot pose and build a map.

[0221] In this step, refer to Figure 2 S5 in the context refers to graph optimization (robust graph optimization), which is the process of jointly adjusting all historical pose nodes in the pose graph by constructing a global error optimization model (or a global optimization cost function).

[0222] In some embodiments, a global error optimization model can be constructed based on inference frameworks such as nonlinear least squares, factor graph optimization, or maximum a posteriori estimation (MAP).

[0223] It should be noted that the pose graph also includes front-end constraints, which represent the local relative motion transformation relationship between adjacent pose nodes during continuous robot operation; the front-end constraints and the closed-loop constraints established across a long spatial trajectory together constitute the complete topological network structure of the pose graph.

[0224] Since both front-end constraints and closed-loop constraints contain observation noise, the cumulative mileage error generated by the robot during long-term operation is reduced by performing overall collaborative optimization on the errors corresponding to all constraint edges, thereby promoting the robot's global pose to converge to a consistent state under multiple constraints.

[0225] In some embodiments, step 350 includes: Construct a global optimization model based on the nodes, front-end constraints, and closed-loop constraints in the pose graph; Error evaluation based on robust kernel function is used to dynamically weight the constraint terms in the global optimization model and solve the global optimization model to correct the node pose in the pose graph. Based on the corrected node poses, the point cloud data is converted to the global coordinate system and fused into the raster map to complete map construction.

[0226] In actual implementation, the overall objective function of the global optimization model can be expressed by the formula:

[0227] in, This represents the set of robot poses to be optimized. This represents the set of all front-end and closed-loop constraint edges in the pose graph; This represents the observation constraint relationship between node i and node j; This represents a robust kernel function used to suppress outlier constraints.

[0228] In some embodiments, the constraint terms can be dynamically weighted based on the error evaluation of robust kernel functions such as the Huber kernel function, Cauchy kernel function, or Tukey kernel function to achieve adaptive suppression of observation noise of different degrees.

[0229] In some embodiments, the Huber kernel function is defined as follows:

[0230] in, This represents the standardized weighted squared error (corresponding to the norm term in the aforementioned objective function). Indicates the transition threshold of the robust kernel; This represents a robust kernel function used to suppress outlier constraints.

[0231] For outlier observation edges with larger residuals (i.e.) In cases where outliers cause interference with the global pose topology, this processing mechanism automatically transforms them from quadratic penalties to linear penalties, assigning them extremely low optimization weights, thereby reducing the risk of outliers interfering with the global pose topology. Through iterative solving of this model, the corrected pose of each node after reducing accumulated errors can be calculated.

[0232] In some alternative embodiments, a combination of switchable constraints and graph optimization can be used in the back-end optimization stage to replace the robust kernel function structure described above, in order to identify and remove abnormal constraints. However, compared to these alternatives, the robust kernel function used in the embodiments of this disclosure offers a more balanced performance in terms of computational real-time performance and convergence stability when dealing with high-frequency dynamic noise.

[0233] In actual implementation, refer to, for example Figure 2 The S6 module, as shown, performs occupancy map processing. Based on the corrected node poses, it dynamically projects the point cloud data from each frame onto a unified global coordinate system to achieve the fusion of all point cloud data into the raster map. In the raster map, each grid cell represents the probability that the corresponding spatial area is occupied by an obstacle. The system calculates and updates the occupancy state of each grid cell by combining the global pose with the current laser observations, thereby generating a structurally stable and globally consistent raster map for output.

[0234] The map building method provided in this application can be executed by a map building device. This application uses the example of a map building device executing the map building method to illustrate the map building device provided in this application.

[0235] This application also provides a map building apparatus.

[0236] like Figure 4 As shown, the map building device includes: a first processing module 410, a second processing module 420, a third processing module 430, a fourth processing module 440, and a fifth processing module 450.

[0237] The first processing module 410 is used to acquire point cloud data representing the environment in which the robot is located; The second processing module 420 is used to perform multi-resolution scanning matching on point cloud data to obtain local poses and generate front-end constraints that characterize the relative transformation relationship between adjacent local poses in order to construct a pose graph. The third processing module 430 is used to perform similarity retrieval and geometric consistency verification on point cloud data, and generate closed-loop constraints based on the verification results and add them to the pose graph; wherein, the closed-loop constraints represent the relative transformation relationship between local pose and historical local pose. The fourth processing module 440 is used to perform global consistency correction on the pose graph using front-end constraints and closed-loop constraints to obtain the global pose. The fifth processing module 450 is used to project point cloud data onto the global coordinate system based on the global pose in order to construct and output a map.

[0238] According to the map building apparatus provided in this application embodiment, multi-resolution scanning matching is performed on point cloud data to generate front-end constraints, which improves the robustness of local registration in complex scenes, effectively reduces the cumulative error of pose estimation, and alleviates map drift. Furthermore, by combining similarity retrieval and geometric consistency verification to generate closed-loop constraints, reliable verification of candidate loop closures is achieved, reducing the risk of erroneous constraints introduced by feature confusion. Then, global consistency correction is performed on the pose graph, realizing the integrated collaboration of front-end matching, closed-loop detection, and back-end optimization, and constructing a data exchange and constraint management mechanism between the front-end, back-end, and closed loop. Through the collaborative strategy of multi-resolution scanning matching, closed-loop geometric consistency verification, and global consistency correction, registration errors are effectively controlled in the early stages, achieving timely error constraint and gradual convergence, thereby improving the global consistency and real-time performance of map building.

[0239] In some embodiments, the second processing module 420 may also be used for: The first estimated pose of point cloud data is obtained based on the motion model; Based on the first estimated pose, multi-resolution scanning matching is performed on the point cloud data to obtain the local pose. Local poses are added to the pose graph as pose nodes, and front-end constraints are generated based on the relative transformation relationships between pose nodes.

[0240] In some embodiments, the second processing module 420 may also be used for: Based on the first estimated pose, perform the first registration of point cloud data and the constructed local map at the first resolution to obtain the first matching pose; Dynamic point culling is performed on the point cloud data based on the first matched pose to obtain the culled point cloud data. Using the first matched pose as a reference, a second registration is performed on the removed point cloud data and the constructed local map at a second resolution to obtain the local pose; wherein, the first resolution is lower than the second resolution.

[0241] In some embodiments, the third processing module 430 can also be used for: When the robot's motion state reaches the motion state threshold, the subgraph is filtered by subgraph descriptor retrieval to obtain closed-loop candidate positions; the subgraph is obtained by aggregating pose nodes and their corresponding point cloud data. Perform geometric consistency checks on the candidate locations for closed loops, and generate closed loop constraints based on the check results, adding them as constraint edges to the pose graph.

[0242] In some embodiments, the fourth processing module 440 can also be used for: Obtain the relative pose transformation relationship and matching residual between point cloud data and historical point cloud data corresponding to closed-loop candidate positions; Geometric consistency verification of relative pose transformation relationship is performed based on matching residuals; If the geometric consistency check passes, closed-loop constraints will be generated based on the relative pose transformation relationship and added to the pose graph as constraint edges.

[0243] In some embodiments, the fourth processing module 440 can also be used for: A global objective function is constructed based on the residual terms corresponding to the front-end constraints and the closed-loop constraints, respectively. The residuals in the global objective function are weighted to obtain the weighted global objective function. The weighted global objective function is solved iteratively to obtain the pose increment of the pose nodes; The pose of the pose nodes is updated by overlaying based on the pose increment, and the updated pose nodes are combined into a global pose.

[0244] In some embodiments, the fifth processing module 450 can also be used for: Extract the local coordinates of each data point in the point cloud data; Based on the global pose, determine the corresponding coordinate transformation matrix; The local coordinates of each data point are transformed and calculated using a coordinate transformation matrix to obtain the global coordinates of each data point in the global coordinate system. A map is constructed and output by stitching together data points containing global coordinates.

[0245] The map building device in this application embodiment can be an electronic device or a component within an electronic device, such as an integrated circuit or a chip. The electronic device can be a terminal or other devices besides a terminal. For example, the electronic device can be a mobile phone, tablet computer, laptop computer, PDA, in-vehicle electronic device, mobile internet device (MID), augmented reality (AR) / virtual reality (VR) device, robot, wearable device, ultra-mobile personal computer (UMPC), netbook, or personal digital assistant (PDA), etc. It can also be a server, network attached storage (NAS), personal computer (PC), television set (TV), ATM, or self-service machine, etc. This application embodiment does not specifically limit the device.

[0246] The map building device in this application embodiment can be a device with an operating system. This operating system can be Android, iOS, or other possible operating systems; this application embodiment does not specifically limit the specific operating system used.

[0247] The map building apparatus provided in this application embodiment can achieve... Figures 1 to 3 The various processes implemented in the method implementation examples will not be described again here to avoid repetition.

[0248] This application also provides a robot, including: an environmental ranging sensor and a map building device based on any of the above embodiments.

[0249] The environmental ranging sensor is electrically connected to the map building device.

[0250] The environmental ranging sensor is used to acquire point cloud data characterizing the environment in which the robot is located, i.e., to execute step 110.

[0251] In some embodiments, the environmental ranging sensor is preferably a laser ranging sensor (such as a single-line lidar, multi-line lidar, or solid-state lidar).

[0252] In some alternative embodiments, a depth camera or a time-of-flight (ToF) sensor may also be used.

[0253] In some embodiments, such as Figure 5As shown, this application embodiment also provides an electronic device 500, including a processor 501, a memory 502, and a computer program stored in the memory 502 and executable on the processor 501. When the program is executed by the processor 501, it implements the various processes of the above-described map construction method embodiment and achieves the same technical effect. To avoid repetition, it will not be described again here.

[0254] It should be noted that the electronic devices in the embodiments of this application include the mobile electronic devices and non-mobile electronic devices described above.

[0255] This application also provides a non-transitory computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the various processes of the above-described map construction method embodiments and achieves the same technical effect. To avoid repetition, it will not be described again here.

[0256] The processor is the processor in the electronic device described in the above embodiments. The readable storage medium includes computer-readable storage media, such as computer read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk.

[0257] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the above-described map construction method.

[0258] The processor is the processor in the electronic device described in the above embodiments. The readable storage medium includes computer-readable storage media, such as computer read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk.

[0259] This application embodiment also provides a chip, which includes a processor and a communication interface. The communication interface is coupled to the processor. The processor is used to run programs or instructions to implement the various processes of the above map construction method embodiments and can achieve the same technical effect. To avoid repetition, it will not be described again here.

[0260] It should be understood that the chip mentioned in the embodiments of this application may also be referred to as a system-on-a-chip, system chip, chip system, or system-on-a-chip, etc.

[0261] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element. Furthermore, it should be noted that the scope of the methods and apparatuses in the embodiments of this application is not limited to performing functions in the order shown or discussed, but may also include performing functions substantially simultaneously or in the reverse order, depending on the functions involved. For example, the described methods may be performed in a different order than described, and various steps may be added, omitted, or combined. Additionally, features described with reference to certain examples may be combined in other examples.

[0262] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a computer software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) and includes several instructions to cause a terminal (which may be a mobile phone, computer, server, or network device, etc.) to execute the methods described in the various embodiments of this application.

[0263] The embodiments of this application have been described above with reference to the accompanying drawings. However, this application is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of this application without departing from the spirit and scope of the claims, and all of these forms are within the protection scope of this application.

[0264] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0265] Although embodiments of this application have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of this application, the scope of which is defined by the claims and their equivalents.

Claims

1. A map construction method, characterized in that, include: Acquire point cloud data representing the robot's environment; Multi-resolution scanning matching is performed on the point cloud data to obtain local poses, and front-end constraints representing the relative transformation relationship between adjacent local poses are generated to construct a pose graph. The point cloud data is subjected to similarity retrieval and geometric consistency verification, and closed-loop constraints are generated and added to the pose graph based on the verification results; wherein, the closed-loop constraints represent the relative transformation relationship between the local pose and the historical local pose. The pose graph is subjected to global consistency correction using the front-end constraints and the closed-loop constraints to obtain the global pose. The point cloud data is projected onto the global coordinate system based on the global pose to construct and output a map.

2. The map construction method according to claim 1, characterized in that, The step of performing multi-resolution scan matching on the point cloud data to obtain local poses and generating front-end constraints representing the relative transformation relationship between adjacent local poses to construct a pose graph includes: The first estimated pose of the point cloud data is obtained based on the motion model; Based on the first estimated pose, multi-resolution scanning matching is performed on the point cloud data to obtain the local pose; The local pose is added to the pose graph as a pose node, and front-end constraints are generated based on the relative transformation relationship between the pose nodes.

3. The map construction method according to claim 2, characterized in that, The step of performing multi-resolution scan matching on the point cloud data based on the first estimated pose to obtain the local pose includes: Based on the first estimated pose, the point cloud data and the constructed local map are registered at the first resolution to obtain the first matching pose; Based on the first matched pose, dynamic point culling processing is performed on the point cloud data to obtain the culled point cloud data. Using the first matched pose as a reference, a second registration is performed on the removed point cloud data and the constructed local map at a second resolution to obtain the local pose; wherein, the first resolution is lower than the second resolution.

4. The map construction method according to any one of claims 1-3, characterized in that, The step of performing similarity retrieval and geometric consistency verification on the point cloud data, and generating closed-loop constraints based on the verification results and adding them to the pose graph, includes: When the robot's motion state reaches a motion state threshold, a subgraph descriptor retrieval and filtering process is performed on the subgraph to obtain closed-loop candidate positions; wherein, the subgraph is obtained by aggregating the pose nodes and their corresponding point cloud data; Perform the geometric consistency check on the candidate closed-loop positions, and generate the closed-loop constraints based on the check results, adding them as constraint edges to the pose graph.

5. The map construction method according to claim 4, characterized in that, The step of performing the geometric consistency check on the candidate closed-loop positions and generating the closed-loop constraints based on the check results, adding them as constraint edges to the pose graph, includes: Obtain the relative pose transformation relationship and matching residual between the point cloud data and the historical point cloud data corresponding to the closed-loop candidate position; The geometric consistency verification is performed on the relative pose transformation relationship based on the matching residual. If the geometric consistency check passes, the closed-loop constraint generated based on the relative pose transformation relationship will be added as a constraint edge to the pose graph.

6. The map construction method according to any one of claims 1-3, characterized in that, The step of performing global consistency correction on the pose graph using the front-end constraints and the closed-loop constraints to obtain the global pose includes: Based on the residual terms corresponding to the front-end constraints and the closed-loop constraints, a global objective function is constructed. The residual terms in the global objective function are weighted to obtain the weighted global objective function; The weighted global objective function is iteratively solved to obtain the pose increment of the pose node; The pose of the pose nodes is updated by superimposing the pose increment, and the updated pose nodes are combined into the global pose.

7. The map construction method according to any one of claims 1-3, characterized in that, The step of projecting the point cloud data onto a global coordinate system based on the global pose to construct and output a map includes: Extract the local coordinates of each data point in the point cloud data; Based on the global pose, determine the corresponding coordinate transformation matrix; The local coordinates of each data point are transformed and calculated using the coordinate transformation matrix to obtain the global coordinates of each data point in the global coordinate system. The map is constructed and output by stitching together data points containing the global coordinates of each data point.

8. A map building device, characterized in that, include: The first processing module is used to acquire point cloud data that characterizes the environment in which the robot is located; The second processing module is used to perform multi-resolution scanning matching on the point cloud data to obtain local poses and generate front-end constraints that characterize the relative transformation relationship between adjacent local poses in order to construct a pose graph. The third processing module is used to perform similarity retrieval and geometric consistency verification on the point cloud data, and generate closed-loop constraints based on the verification results and add them to the pose graph; wherein, the closed-loop constraints represent the relative transformation relationship between the local pose and the historical local pose. The fourth processing module is used to perform global consistency correction on the pose graph using the front-end constraints and the closed-loop constraints to obtain the global pose. The fifth processing module is used to project the point cloud data onto the global coordinate system based on the global pose in order to construct and output a map.

9. A robot, characterized in that, include: Environmental ranging sensors are used to acquire point cloud data characterizing the robot's environment; The map building apparatus as described in claim 8; The environmental ranging sensor is electrically connected to the map building device.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the map construction method as described in any one of claims 1-7.

11. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the map construction method as described in any one of claims 1-7.

Images