Graph expansion method and device based on grid map, robot and computer readable storage medium

Abstract

The invention relates to the technical field of robots, and discloses a raster map-based map expansion method and device, a robot and a computer readable storage medium, the raster map-based map expansion method comprises the following steps: according to data of an old raster map constructed last time, and current laser point cloud data and current pose data of the robot, determining the current laser point cloud data and the current pose data of the robot; obtaining an accurate pose of the robot in the old grid map; real-time odometer data and real-time laser point cloud data collected in the moving process of the robot in the target area are obtained in real time by taking the accurate pose as an initial image expansion position, so that multiple pieces of key frame data corresponding to the target area are continuously generated; performing optimization matching on the plurality of pieces of key frame data according to a pre-constructed boundary constraint to obtain a plurality of pieces of optimized key frame data; and according to the old raster map data and the plurality of optimized key frame data, generating new raster map data including the target area.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a grid map-based map expansion method and device, a robot and a computer readable storage medium. BACKGROUND

[0002] In the field of robot autonomous navigation, simultaneous localization and mapping (SLAM) of robots is a key technology, which enables the robot to match laser and map on a known map to achieve real-time positioning and build a map of the surrounding environment. At present, although the SLAM technology based on 2D laser radar is mature, it still faces severe challenges in actual application scenarios.

[0003] In the process of implementing the embodiments of the present disclosure, it is found that at least the following problems exist in the related art: On the one hand, in large-scale warehouse logistics centers, underground comprehensive pipe galleries or cross-floor industrial plants and other large-scale complex scenes, due to the huge physical space scale or the existence of regional access permission restrictions, the robot often cannot complete the global environment mapping in a single task, resulting in a large number of unexplored blank areas in the initially obtained map. Such an incomplete map seriously restricts the working range and navigation efficiency of the robot.

[0004] On the other hand, the actual industrial environment has significant dynamic evolution characteristics, such as the addition of isolated areas in the plant, the adjustment of warehouse rack layout or the phased expansion of construction sites, and other continuous changes. These dynamic characteristics require the robot system to have the ability to dynamically expand and real-time correct the existing map, rather than simply repeat the global mapping to cope with the continuous evolution of the environment.

[0005] In addition, the traditional SLAM technology will amplify the cumulative error problem in the map expansion process. When the robot explores unknown areas based on the old map, as the moving distance continues to increase, the errors of the odometry and scan matching will continue to accumulate. Due to the lack of effective global constraint mechanism, significant pose drift often occurs between the newly built map area and the original map. This drift directly leads to serious misalignment, overlap and other data inconsistency phenomena at the junction boundary of the map, which not only affects the accuracy of the map, but also causes path planning errors and even navigation failures and other serious consequences.

[0006] It should be noted that the information disclosed in the above background section is only used to strengthen the understanding of the background of the present application, and therefore can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0007] The following presents a simplified summary of some aspects of the disclosed embodiments in order to provide a basic understanding of such embodiments. This summary is not an extensive overview of the embodiments and is not intended to identify key / critical elements or to delineate the scope of the embodiments. Its sole purpose is to present some embodiments in a simplified form as a prelude to the more detailed description that is presented later.

[0008] The embodiment of the present disclosure provides a grid map-based expansion method and device, a robot and a computer readable storage medium, which realizes effective suppression of accumulated error and smooth and seamless expansion of incremental map construction based on an original grid map, thereby promoting a key technology for realizing real practicalization of a robot in a large-scale, long-period and dynamic evolution scene.

[0009] In some embodiments, the grid map-based expansion method comprises: acquiring an accurate pose of the robot in the old grid map according to old grid map data of the last time construction and current laser point cloud data and current pose data of the robot; continuously generating a plurality of key frame data corresponding to the target area by taking the accurate pose as a starting expansion position and acquiring real-time odometry data and real-time laser point cloud data collected in the moving process of the robot in the target area; optimizing and matching the plurality of key frame data according to the pre-constructed boundary constraint to obtain a plurality of optimized key frame data; generating new grid map data containing the target area according to the old grid map data and the plurality of optimized key frame data.

[0010] acquiring an accurate pose of the robot in the old grid map according to old grid map data of the last time construction and current laser point cloud data and current pose data of the robot, comprising: loading the old grid map data of the last time construction, wherein the old grid map data comprises old laser point cloud data, old pose data and old map optimization data saved after the last time map construction is completed; preliminarily estimating an initial pose of the robot in the old grid map according to the old laser point cloud data, the old pose data and the old map optimization data and the current laser point cloud data and the current pose data of the robot; optimizing the initial pose by an iterative closest point algorithm to obtain a converged accurate pose after a plurality of iterations.

[0011] Optionally, the optimization of the initial pose by the iterative closest point algorithm to obtain a converged accurate pose after a plurality of iterations comprises: acquiring a plurality of candidate poses within a preset range; For each candidate pose, the iterative nearest point algorithm based on SVD singular values ​​is used to iteratively match the optimal translation and rotation between two laser point clouds by minimizing the objective function, and finally obtain the converged accurate pose. Among them, p i and q i This represents two laser point clouds that need to be registered. The goal is to minimize the following objective function:

[0012] Then calculate the centroid: ,

[0013] Decentralized point set: ,

[0014] Expand and differentiate the objective function:

[0015] The optimal translation is obtained:

[0016] Substituting t* back into the objective function, we simplify and expand the objective function:

[0017] The objective function is equivalent to:

[0018] Define the covariance matrix:

[0019] but:

[0020] Perform singular value decomposition on H:

[0021]

[0022] Where U is an orthogonal matrix, E is a diagonal matrix containing singular values, and V T It is the transpose of another orthogonal matrix V; We need to find the optimal pose:

[0023] The optimal rotation matrix is:

[0024] Optimal translation:

[0025] Final transformation:

[0026] Where, p i and q i Given two laser point cloud datasets that need to be matched, R is the rotation matrix and t is the translation matrix.

[0027] Optionally, the step of using the precise pose as the starting mapping position and acquiring real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area to continuously generate multiple keyframe data corresponding to the target area includes: A local map is constructed at the initial map expansion location, and the real-time laser point cloud data is fused into the old raster map data according to the precise pose. When the robot moves a target distance within the target area, multiple keyframe data are continuously generated based on real-time odometer data and real-time laser point cloud data. The target area is either a known area inside the old grid map or an unknown area at the boundary of the old grid map. The keyframe data is stored in a preset dynamic update layer.

[0028] Optionally, the step of optimizing and matching the multiple keyframe data according to pre-constructed boundary constraints to obtain optimized multiple keyframe data includes: When the robot is located in an unknown region of the boundary of the old grid map, the boundary constraint optimizer is activated; Unary edge constraints and binary edge constraints are constructed through a boundary constraint optimizer. The unary edge constraints include point-to-point and point-to-edge constraints, and the binary edge constraints include relative pose relationship constraints established between two consecutive keyframes and relative pose relationship constraints established between two non-consecutive keyframes generated by loop closure detection. Local graph optimization is performed on multiple keyframe data in the dynamically updated layer using unary edge constraints; and... Global graph optimization is performed on multiple keyframe data in the dynamically updated layer using binary edge constraints.

[0029] Optionally, generating new raster map data containing the target area based on the old raster map data and the optimized multiple keyframe data includes: The old laser point cloud data, old pose data, and old image optimization data are updated based on the optimized keyframe data to obtain new laser point cloud data, new pose data, and new image optimization data; By using a multi-resolution raster matching algorithm, new laser point cloud data is mapped to different locations on the old raster map according to probability, thereby generating new raster map data containing the target area.

[0030] Optionally, the map expansion method further includes: A new raster map is generated in real time based on the new raster map data; The new grid map is displayed via a terminal device.

[0031] In some embodiments, the raster map-based map expansion device includes: The initial pose estimation module is configured to obtain the robot's precise pose in the old grid map based on the previously constructed old grid map data, the robot's current laser point cloud data, and the current pose data. The keyframe acquisition module is configured to use the precise pose as the starting position for mapping expansion, and to acquire real-time odometry data and real-time laser point cloud data collected by the robot during its movement within the target area, so as to continuously generate multiple keyframe data corresponding to the target area. A boundary constraint module is configured to perform optimized matching on multiple keyframe data according to pre-built boundary constraints to obtain optimized multiple keyframe data. The map update fusion module is configured to generate new raster map data containing the target area based on the old raster map data and the optimized keyframe data.

[0032] In some embodiments, the robot includes a processor and a memory storing program instructions, the processor being configured to execute, when running the program instructions, a raster map-based map expansion method as described in this application.

[0033] In some embodiments, the computer-readable storage medium stores program instructions that, when executed, cause a computer to perform the raster map-based map expansion method as described in this application.

[0034] The raster map-based map expansion method, apparatus, robot, and computer-readable storage medium provided in this disclosure can achieve the following technical effects: Based on the previously constructed old grid map data, and the robot's current laser point cloud data and current pose data, the robot's precise pose in the old grid map is obtained. This initial pose estimation based on multi-source information fusion ensures positioning accuracy and lays a reliable foundation for subsequent expansion. Simultaneously, using the precise pose as the starting position for map expansion, real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area are acquired in real time to continuously generate multiple keyframe data corresponding to the target area. Then, the multiple keyframe data are optimized and matched according to pre-constructed boundary constraints to obtain optimized keyframe data. Finally, based on the old grid map data and the optimized keyframe data, a new grid map data containing the target area is generated. Therefore, this application provides a dynamic multi-layer map update mechanism that achieves real-time response to environmental changes and continuous optimization of map content. Furthermore, through seamless global map fusion based on boundary constraints, accumulated errors are effectively eliminated, ensuring a natural transition and geometric consistency between the old and new maps at the boundary, significantly improving practicality and reliability. Meanwhile, this application achieves dynamic map expansion and updating while maintaining positioning accuracy, effectively solving the problems of cumulative error and boundary fusion faced in the map expansion process in related technologies.

[0035] The above general description and the description below are exemplary and illustrative only and are not intended to limit this application. Attached Figure Description

[0036] One or more embodiments are illustrated by way of example with reference to the accompanying drawings. These illustrations and drawings do not constitute a limitation on the embodiments. Elements having the same reference numerals in the drawings are shown as similar elements. The drawings are not to be scaled. And wherein: Figure 1 This is a schematic diagram of a raster map-based map expansion method provided in an embodiment of this disclosure; Figure 2 This is a schematic diagram of another raster map-based map expansion method provided in this embodiment of the disclosure; Figure 3 This is a schematic diagram of another raster map-based map expansion method provided in this embodiment of the disclosure; Figure 4 This is a schematic diagram of another raster map-based map expansion method provided in this embodiment of the disclosure; Figure 5 This is a schematic diagram of another raster map-based map expansion method provided in this embodiment of the disclosure; Figure 6 This is a schematic diagram of another raster map-based map expansion method provided in this embodiment of the disclosure; Figure 7This is a schematic diagram of another raster map-based map expansion method provided in this embodiment of the disclosure; Figure 8 This is a schematic diagram of an actual mapping process provided in an embodiment of this disclosure; Figure 9 This is another schematic diagram of an actual mapping process provided in this embodiment of the disclosure; Figure 10 This is another schematic diagram of an actual mapping process provided in this embodiment of the disclosure; Figure 11 This is a schematic diagram of a raster map-based map expansion device provided in an embodiment of this disclosure; Figure 12 This is a structural block diagram of a robot provided in an embodiment of this disclosure. Detailed Implementation

[0037] To provide a more detailed understanding of the features and technical content of the embodiments of this disclosure, the implementation of the embodiments of this disclosure will be described in detail below with reference to the accompanying drawings. The accompanying drawings are for illustrative purposes only and are not intended to limit the embodiments of this disclosure. In the following technical description, for ease of explanation, several details are used to provide a full understanding of the disclosed embodiments. However, one or more embodiments may still be implemented without these details. In other cases, well-known structures and devices may be simplified in their depiction to simplify the drawings.

[0038] The terms "first," "second," etc., used in the specification, claims, and accompanying drawings of this disclosure are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this disclosure described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion.

[0039] Unless otherwise stated, the term "multiple" means two or more.

[0040] In this embodiment of the disclosure, the character " / " indicates that the objects before and after it are in an "or" relationship. For example, A / B means: A or B.

[0041] The term "and / or" describes an association between objects, indicating that three relationships can exist. For example, A and / or B means: A or B, or A and B.

[0042] The term "correspondence" can refer to an association or binding relationship. The correspondence between A and B means that there is an association or binding relationship between A and B.

[0043] First, the technical terms used in this application will be explained: Laser point cloud data: 3D laser point cloud data consists of a large number of discrete points with spatial coordinates (x, y, z), and usually also includes additional information such as color (RGB) and intensity. These points are acquired by laser scanning equipment, which can accurately reconstruct the spatial structure of the real world.

[0044] Mapping: Mapping refers to creating environmental maps based on sensor data. Maps can be two-dimensional, such as raster maps, or three-dimensional point cloud maps.

[0045] Raster map: A pixel map composed of black, white and gray pixels. Black pixels represent obstacles, white pixels represent free space, and gray pixels represent unmapped areas.

[0046] Laser point cloud data refers to the location of the center point in space acquired by laser, i.e., a large number of discrete points with spatial coordinates (x, y, z), which can be obtained through 2D LiDAR.

[0047] Pose: A combined concept describing the position and orientation of an object in three-dimensional space, consisting of two parts: position and orientation. Specifically: Position: refers to the object's coordinates in space, usually represented by (x, y, z), describing the object's three-dimensional position relative to a reference point. Orientation: refers to the object's direction or orientation in space, describing the object's rotational state, commonly represented by Euler angles (yaw, pitch, roll) or a rotation matrix. In practical applications, pose is usually uniformly described using a homogeneous transformation matrix (4×4 matrix).

[0048] Keyframes: Keyframes are representative frames selected from a laser point cloud sequence, used to optimize map building and pose estimation. During SLAM mapping, the system does not process every frame of the laser point cloud; instead, it selects a subset of frames as keyframes based on a specific strategy, discarding the rest or using them for matching assistance. A keyframe contains two core elements: pose data – recording the position and orientation of the point cloud in the global coordinate system (six degrees of freedom parameters); and laser point cloud data – the laser scan results at the corresponding moment, used to construct local or global maps. Optimizing and selecting a specific frame of laser data, along with the robot's pose, forms a keyframe, which in turn generates a map.

[0049] Constraints: These refer to the relative pose relationship between two keyframes, i.e., the pose difference between them. Constraints can include adjacency constraints and loop closure constraints. Specifically, the pose difference between adjacent keyframe data is the adjacency constraint, and the loop closure constraint is the pose difference between adjacent keyframe data and the keyframes related to the robot's lap closure. Figure 1When the robot returns to its original position, there will be a pose difference between the two times it passes through this position. This pose difference is the closure constraint term. This pose difference is generated by the cumulative error. That is, the robot does not know that it has returned to the original position. It needs to identify the environmental features before the robot knows that it has returned to the same place.

[0050] Loop: Looping refers to identifying whether the robot returns to a previously visited location.

[0051] SLAM (Simultaneous Localization and Mapping) technology refers to a method where a subject equipped with specific sensors simultaneously creates an environmental map and determines its own position while moving in an unknown environment. The core principle is to perceive the environment through sensors (such as LiDAR, cameras, and IMUs), estimate the device's pose (position and orientation) in real time, and construct an environmental map. Localization and mapping are interdependent: accurate maps require precise poses, while localization requires maps to avoid drift. The workflow includes: sensor data acquisition, feature extraction, pose estimation, map optimization, and loop closure detection (correcting accumulated errors).

[0052] It is evident that the technical solutions in related technologies generally face a common challenge during map expansion: how to ensure high-precision geometric alignment between the newly added map area and the original map, avoiding problems such as map misalignment, overlap, or breakage. Especially in large-scale expansion scenarios, controlling cumulative errors and smoothly connecting boundary areas become key technical challenges.

[0053] Regarding incremental map expansion based on existing maps, the relevant technical solutions mainly include the following categories: Multi-session SLAM: This technique requires the robot to create multiple independent sub-maps at different times, and then stitch them together into a global map by finding overlapping areas between the sub-maps (using a matching method similar to loop closure detection). The core challenge of this method is ensuring sufficient overlap between the sub-maps for accurate matching, and it is also sensitive to long-term changes in the scene.

[0054] Local map update technology: This approach primarily addresses the dynamic nature of the map's internal environment. When the robot detects new static obstacles (such as newly added tables, chairs, or equipment) in an existing map, it updates the map in real time at a local grid level. However, this method is mainly suitable for modifications to the map's internal environment and struggles to handle large-scale boundary expansion requirements.

[0055] Incremental mapping based on relocalization: This approach first requires the robot to determine its initial pose in the existing map through precise relocalization, then initiates a new SLAM process for incremental mapping, and periodically aligns the newly created map with the original map. While this method can achieve map expansion, the process is complex and carries the risk of error accumulation.

[0056] However, the above-mentioned technical solutions generally face a common challenge in the process of map expansion: how to ensure that the newly added map area achieves (1) high-precision geometric alignment with the original map; (2) avoid problems such as map misalignment, overlap or breakage, especially in the case of large-scale expansion; (3) control of cumulative error and smooth connection of boundary areas have become the difficulties in technical implementation.

[0057] Specifically, full-map SLAM schemes in related technologies are unsuitable for large-scale or phased exploration scenarios. They require repeated processing of perception data from already mapped areas, resulting in low efficiency and ineffective utilization of historical map data, leading to a severe waste of computing resources. Secondly, while existing mapping technologies such as multi-session SLAM theoretically support map stitching, they have extremely high requirements for the accuracy of scene lighting changes, dynamic object interference, and initial pose matching between two mapping phases. In practical applications, their robustness is clearly insufficient, and they are prone to map overlap or misalignment errors due to feature matching failures. Map relocalization technology is too rigid, and its localization accuracy heavily depends on the stability of the environment. Once the robot moves to a completely unknown area outside the boundaries of the old map, the uncertainty of pose estimation increases exponentially due to the lack of effective prior information for motion constraints. This can easily lead to severe localization drift or even complete loss, ultimately resulting in irreparable breaks, overlaps, or geometric misalignments between the new and old maps. Existing local incremental update methods mainly target the modification of static obstacles within the map. When faced with continuous and large-scale boundary expansion, they lack an effective global constraint mechanism to suppress the cumulative errors caused by odometry and sensor noise. This results in the expansion area and the old map not being able to achieve accurate geometric alignment at the boundary, which seriously affects the accuracy of subsequent navigation.

[0058] Furthermore, the map expansion methods in related technologies generally lack systematic boundary error control strategies, making it difficult to guarantee the overall consistency quality of the map during the expansion process.

[0059] To address this, this application aims to overcome the inherent shortcomings of existing SLAM technologies in map expansion, focusing on solving the following three key technical problems: First, this application aims to achieve continuous and seamless map expansion based on existing maps, while effectively suppressing accumulated errors and adapting to dynamically changing environments. Traditional SLAM schemes, when expanding from known to unknown areas, suffer from a lack of global constraints, causing pose estimation errors to accumulate with increasing exploration distance, resulting in severe map distortion. Therefore, this application seeks to establish a mechanism that fully utilizes existing map information as a stable reference frame, ensuring map expansion continuity while significantly reducing the impact of accumulated errors on map quality, and adapting to dynamic changes in environmental layout.

[0060] Second, this invention overcomes the limitations of modification techniques at map boundaries, achieving a smooth transition and reliable mapping from known to completely unknown areas. When a robot crosses existing map boundaries into unexplored areas, existing methods often result in positioning drift due to a lack of prior information. This invention addresses how to provide sufficient pose estimation constraints for the robot's mapping in unknown areas while maintaining positioning accuracy in known map regions, ensuring consistency between the newly created map area and the original map in the global coordinate system.

[0061] Third, address the issue of high-precision fusion of old and new maps in boundary areas. When a map expands to a boundary area, how can we utilize the stable features of the known map as strong constraints to optimize the pose estimation of the current frame, achieving geometric alignment of the old and new map data and avoiding inconsistencies such as misalignment and overlap? This requires designing specialized boundary processing algorithms to ensure that the expanded map achieves sub-pixel-level accurate stitching at the boundary, forming a complete and unified representation of the navigation environment.

[0062] Based on this, combined Figure 1 As shown, this application provides a raster map-based map expansion method, including: Step 101: Based on the previously constructed old grid map data, as well as the robot's current laser point cloud data and current pose data, obtain the robot's precise pose in the old grid map.

[0063] Step 102: Using the precise pose as the starting position for map expansion, acquire real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area, so as to continuously generate multiple keyframe data corresponding to the target area.

[0064] Step 103: Optimize and match multiple keyframe data according to pre-constructed boundary constraints to obtain optimized multiple keyframe data.

[0065] Step 104: Generate new raster map data containing the target area based on the old raster map data and the optimized keyframe data.

[0066] The raster map expansion method provided in this disclosure obtains the robot's precise pose in the old raster map by using the previously constructed old raster map data, the robot's current laser point cloud data, and the current pose data. This initial pose estimation based on multi-source information fusion ensures positioning accuracy and lays a reliable foundation for subsequent expansion. Simultaneously, using the precise pose as the starting expansion position, real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area are acquired in real time to continuously generate multiple keyframe data corresponding to the target area. Then, the multiple keyframe data are optimized and matched according to pre-constructed boundary constraints to obtain optimized keyframe data. Finally, a new raster map containing the target area is generated based on the old raster map data and the optimized keyframe data. Therefore, this application provides a dynamic multi-layer map update mechanism that achieves real-time response to environmental changes and continuous optimization of map content. Furthermore, through seamless global map fusion based on boundary constraints, it effectively eliminates accumulated errors, ensures a natural transition and geometric consistency between the old and new maps at the boundary, and significantly improves the system's practicality and reliability. It enables dynamic map expansion and updating while maintaining positioning accuracy, effectively solving the problems of cumulative error and boundary fusion faced in the map expansion process in related technologies.

[0067] Optionally, combined Figure 2 As shown, the step of obtaining the robot's precise pose in the old grid map based on the previously constructed old grid map data, the robot's current laser point cloud data, and the current pose data includes: Step 201: Load the old raster map data built previously, wherein the old raster map data includes the old laser point cloud data, old pose data and old map optimization data saved after the previous map was built.

[0068] Step 202: Based on the old laser point cloud data, old pose data, and old map optimization data, as well as the robot's current laser point cloud data and current pose data, make a preliminary estimate of the robot's initial pose in the old grid map.

[0069] Step 203: Optimize the initial pose using the iterative nearest point algorithm, and obtain the converged accurate pose after several iterations.

[0070] Specifically, before the robot begins the mapping task, the system loads all old raster map data related to the previous map construction. This old raster map data includes old laser point cloud data, old pose data, and old map optimization data. Based on this data, high-precision map files can be generated at any time for subsequent localization, navigation, and other functions. After loading the old raster map data, the system relocalizes the robot. After pushing the robot to the starting mapping position, a local raster map is generated. Combining the robot's precise pose obtained from the relocalization, the current laser point cloud data can be fused into the old raster map. Subsequent generation of new raster maps for unknown areas is entirely supplementary to the old raster map.

[0071] Furthermore, by fusing the robot's old laser point cloud data, old pose data, and old map optimization data, along with the robot's current laser point cloud data and current pose data, the robot's initial pose in the old grid map is initially estimated. A multi-hypothesis matching strategy can be used to initially estimate the robot's initial pose in the old map coordinate system. Specifically, data acquired by the robot's odometry and 2D LiDAR is used for relocalization on the old map. Then, graph optimization is performed using laser data generated during map expansion, including point-to-point, point-to-edge, and adjacent-edge optimizations. Each optimization part has its own objective function to ensure map accuracy. The multi-hypothesis matching strategy is a set of underlying assumptions in the laser matching algorithm. Taking point-to-point optimization as an example, a key assumption is that the nearest point is used as the matching point. Many such assumptions exist, involving fundamental theories, and these assumptions directly determine the definition of the objective function. Based on this, this application further optimizes the pose estimation results through an iterative nearest-point algorithm, significantly improving the initial localization accuracy and establishing a reliable pose reference for subsequent map expansion.

[0072] Optionally, combined Figure 3 As shown, the step of optimizing the initial pose using the iterative nearest point algorithm to obtain the converged accurate pose after several iterations includes: Step 301: Obtain several candidate poses within a preset range.

[0073] Step 302: For each candidate pose, the iterative nearest point algorithm based on SVD singular values ​​is used to iteratively match the optimal translation and rotation between two laser point clouds by minimizing the objective function, and finally obtain the converged accurate pose.

[0074] Among them, p i and q i This represents two laser point clouds that need to be registered. The goal is to minimize the following objective function:

[0075] Then calculate the centroid: ,

[0076] Decentralized point set: ,

[0077] Expand and differentiate the objective function:

[0078] The optimal translation is obtained:

[0079] Substituting t* back into the objective function, we simplify and expand the objective function:

[0080] The objective function is equivalent to:

[0081] Define the covariance matrix:

[0082] but:

[0083] Perform singular value decomposition on H:

[0084] Where U is an orthogonal matrix, E is a diagonal matrix containing singular values, and V T It is the transpose of another orthogonal matrix V; We need to find the optimal pose:

[0085] The optimal rotation matrix is:

[0086] Optimal translation:

[0087] Final transformation:

[0088] Where, p i and q i Given two laser point cloud datasets that need to be matched, R is the rotation matrix and t is the translation matrix.

[0089] The formula above can be used to calculate the translation and rotation between the two laser point clouds to be matched. The pose is composed of translation and rotation; determining the robot's position and orientation in space is called translation and rotation. After several iterations, the pose can quickly converge to an optimized position.

[0090] Optionally, combined Figure 4 As shown, the process involves using the precise pose as the starting point for mapping expansion, and acquiring real-time odometry data and real-time laser point cloud data during the robot's movement within the target area. This data is used to continuously generate multiple keyframe data corresponding to the target area, including: Step 401: Construct a local map at the initial map expansion location, and fuse the real-time laser point cloud data into the old raster map data according to the precise pose.

[0091] Step 402: After the robot has moved the target distance within the target area, multiple keyframe data are continuously generated based on real-time odometry data and real-time laser point cloud data.

[0092] The target area is either a known area within the old raster map or an unknown area at the boundary of the old raster map.

[0093] Step 403: Store the multiple keyframe data in a preset dynamic update layer.

[0094] Specifically, this application adopts a dynamic layered map management strategy. While maintaining the integrity of the original old map, a dynamic update layer is established. This dynamic update layer can be understood as a newly expanded part of the map. That is, an old map can be expanded multiple times. It can be expanded to the outside (unknown area) of the old map or to the inside (known area) of the old map. Each expansion generates a dynamic update layer. Although the final result is a complete map, the map is the result of superimposing multi-region and multi-layer laser data.

[0095] Meanwhile, this dynamic update layer not only processes incremental scan data from unknown areas but also continuously optimizes the environmental representation within the old map. Unknown areas are determined by humans, not robots. For the robot, newly acquired real-time laser point cloud data continuously overwrites the existing laser point cloud data. Keyframe data is the primary data for map generation and is also incremental data. It is generated through multiple processing steps based on laser data and odometry, and its size continuously increases, thus expanding the map area. When a new obstacle is detected in an old map area or an existing obstacle disappears, the system locally updates the old map based on real-time laser point cloud data. This dynamic update mechanism is implemented through a probabilistic update model. The basic theory of probabilistic update models already exists, and various improvements are made in practical applications based on actual conditions. For example, in this invention, point-to-point and point-to-edge optimizations are concentrated in the local map, optimizing local poses. Optimization of adjacent edges is concentrated in loop closure optimization. The combination of multiple methods ensures map accuracy. This ensures both the stability of historical data and timely reflection of environmental changes, guaranteeing the timeliness and accuracy of the map.

[0096] Furthermore, after initial pose optimization, the current pose is determined to be the pose of the first frame of the expanded map. The old grid map data is loaded synchronously. Every time the robot moves a certain distance, new keyframe data is generated based on odometry and laser data. When the robot has traversed the entire target area (which can be an area never previously visited or an area already mapped), when saving the new grid map data, the old keyframe data is removed based on the update time and pose, achieving multi-layer map updates. Each time map expansion is needed, it is overlaid on the old map. The keyframe data includes pose data. When merging the new and old maps, keyframe data with close proximity in the old map is removed based on the pose data of the new map, ensuring the map is up-to-date. The overlay of multi-layer updates is essentially the overlay of keyframes from multiple map expansions, similar to the dynamic update layer. Each expansion generates new keyframes, which always overlap with the keyframes in the old map. Each overlay is on top of the old map, thus achieving dynamic map updates.

[0097] Optionally, combined Figure 5 As shown, the step of optimizing and matching multiple keyframe data according to pre-constructed boundary constraints to obtain optimized keyframe data includes: Step 501: When the robot is located in an unknown area of ​​the boundary of the old grid map, start the boundary constraint optimizer.

[0098] Step 502: Construct unary edge constraints and binary edge constraints through the boundary constraint optimizer. The unary edge constraints include point-to-point and point-to-edge constraints, and the binary edge constraints include relative pose relationship constraints established between two consecutive keyframes and relative pose relationship constraints established between two non-consecutive keyframes generated by loop closure detection.

[0099] Step 503: Perform local graph optimization on multiple keyframe data in the dynamic update layer using unary edge constraints; and perform global graph optimization on multiple keyframe data in the dynamic update layer using binary edge constraints.

[0100] Specifically, when the map expands to the boundary region, the system activates a dedicated boundary constraint optimizer. For example, after pushing the robot to the edge of the old map and completing the initial relocalization, the boundary constraint optimizer is started, with a dedicated module performing optimization in an independent thread. This boundary constraint optimizer can use stable environmental features at the boundary of the old map (such as walls, pillars, etc.) as strong constraints, accurately matching them with real-time laser data, effectively suppressing the cumulative error of pose estimation.

[0101] First, a large number of unary edges are generated by feature matching between the current keyframe and the local map. Unary edges are defined in graph optimization. Taking this application as an example, after pose transformation, the distance between the nearest points in the point cloud data of adjacent keyframes is made as close as possible, or the distance from a point to a line is made as close as possible. These edges include "point-to-point" and "point-to-line" distance constraints, which directly associate the observation data of a single keyframe with the geometric elements in the map, providing basic observation constraints for optimization.

[0102] Simultaneously, the system establishes binary edges, which are graph optimization processing data composed of adjacent keyframes. Two adjacent keyframes form two vertices based on their poses, and their relative poses form an edge. These edges are added to the boundary constraint optimizer to complete the global optimization of the map. The binary edges used in this application mainly include two types: one is a binary edge connecting two consecutive keyframes, which uses odometry or sensor data to provide relative pose estimation; the other, more important type, is a binary edge established between two non-consecutive keyframes when a loop closure is detected, providing a strong pose transformation constraint for global optimization. The entire optimization process integrates accurate geometric observations from unary edges and pose relationship constraints from binary edges. Local optimization mainly utilizes unary edges and binary edges from adjacent keyframes to ensure accuracy in local areas; while global optimization, driven by loop closure binary edges, effectively corrects cumulative drift, ultimately achieving accurate and seamless fusion of the new extended map and the old map.

[0103] It should be noted that each keyframe contains pose errors, which are negligible in the short term. However, as the number of keyframes increases, the accumulated error grows. Returning to the initial position reveals a significant positional shift, which is the cumulative error. Global optimization primarily relies on loop closure detection. The robot generates keyframes after traversing a certain distance. The pose of each keyframe is generated by fusing odometry and LiDAR data. When the robot returns to previously visited locations, the relative poses between keyframes are calculated using loop closure detection. All keyframes form a loop, and the relative poses form multiple binary edges. Each vertex represents the pose of a keyframe, which is the term to be optimized. The objective function is defined as the sum of squared errors of all constraints. Various optimization methods, such as the Gauss-Newton method and the LM method, are used to adjust the poses and reduce errors, ultimately generating a map with satisfactory accuracy.

[0104] Optionally, combined Figure 6 As shown, generating new raster map data containing the target area based on the old raster map data and the optimized keyframe data includes: Step 601: Update the old laser point cloud data, old pose data, and old image optimization data according to the optimized keyframe data to obtain new laser point cloud data, new pose data, and new image optimization data.

[0105] Step 602: Using a multi-resolution raster matching algorithm, the new laser point cloud data is mapped to different locations on the old raster map according to probability, generating new raster map data containing the target area.

[0106] Specifically, after exploring the current target area, the system merges the dynamically updated layer with the original old map through coordinate transformation. The fusion process employs a multi-resolution raster registration algorithm. Multi-resolution raster maps are a map storage format generated based on keyframes. These keyframes map all laser data to different locations on the raster map according to probability, generating a map that is visually appealing to humans. First, rapid alignment is performed at a coarse resolution level, followed by fine-tuning at a fine resolution level to ensure seamless stitching of the old and new maps at the boundaries, ultimately generating a globally consistent high-precision raster map.

[0107] In another embodiment, combined with Figure 7 As shown, this application provides a raster map-based map expansion method, including: Step 701: Based on the previously constructed old grid map data, as well as the robot's current laser point cloud data and current pose data, obtain the robot's precise pose in the old grid map.

[0108] Step 702: Using the precise pose as the starting position for map expansion, acquire real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area, so as to continuously generate multiple keyframe data corresponding to the target area.

[0109] Step 703: Optimize and match multiple keyframe data according to pre-constructed boundary constraints to obtain optimized multiple keyframe data.

[0110] Step 704: Generate new raster map data containing the target area based on the old raster map data and the optimized keyframe data.

[0111] Step 705: Generate a new raster map in real time based on the new raster map data.

[0112] Step 706: The new grid map is displayed through a terminal device.

[0113] Specifically, combined Figures 8 to 10 The results of multiple map expansions are shown, gradually expanding to include unknown areas. Each expansion is accurately merged, with no obvious layering or mismatch at the edges. The overall map quality meets the requirements for positioning and navigation. Furthermore, this application allows real-time display of the expanded map area on the terminal device's interface, enabling users to decide whether to traverse the entire area. For details regarding steps 701 to 704, please refer to the preceding descriptions of steps 101 to 104; these details will not be repeated here.

[0114] Combination Figure 11 As shown, this disclosure provides a raster map-based map expansion device, including: The initial pose estimation module 1101 is configured to obtain the robot's precise pose in the old grid map based on the previously constructed old grid map data, the robot's current laser point cloud data, and the current pose data. The keyframe acquisition module 1102 is configured to acquire real-time odometry data and real-time laser point cloud data collected by the robot during its movement within the target area, starting from the precise pose as the initial mapping position, so as to continuously generate multiple keyframe data corresponding to the target area. Boundary constraint module 1103 is configured to perform optimized matching on multiple keyframe data according to pre-constructed boundary constraints to obtain optimized multiple keyframe data; The map update fusion module 1104 is configured to generate new raster map data containing the target area based on the old raster map data and the optimized keyframe data.

[0115] This disclosure employs a grid map-based map expansion device. Based on previously constructed old grid map data, and the robot's current laser point cloud data and current pose data, the precise pose of the robot in the old grid map is obtained. Initial pose estimation based on multi-source information fusion ensures positioning accuracy and lays a reliable foundation for subsequent expansion. Simultaneously, using the precise pose as the starting expansion position, real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area are acquired in real time to continuously generate multiple keyframe data corresponding to the target area. Then, the multiple keyframe data are optimized and matched according to pre-constructed boundary constraints to obtain optimized keyframe data. Finally, based on the old grid map data and the optimized keyframe data, new grid map data containing the target area is generated. This provides a dynamic multi-layer map update mechanism, enabling real-time response to environmental changes and continuous optimization of map content. Furthermore, through seamless global map fusion based on boundary constraints, accumulated errors are effectively eliminated, ensuring a natural transition and geometric consistency between the old and new maps at the boundary, significantly improving the system's practicality and reliability. It enables dynamic map expansion and updating while maintaining positioning accuracy, effectively solving the problems of cumulative error and boundary fusion faced in the map expansion process in related technologies.

[0116] Optionally, the raster map-based map expansion device further includes a map display module. The map display module is configured to generate a new raster map in real time based on the new raster map data; the new raster map is displayed through a terminal device.

[0117] Combination Figure 12 As shown, this disclosure provides a robot including a processor 120 and a memory 121. Optionally, the device 12 may further include a communication interface 122 and a bus 123. The processor 120, communication interface 122, and memory 121 can communicate with each other via the bus 123. The communication interface 122 can be used for information transmission. The processor 120 can call logical instructions in the memory 121 to execute the grid map-based map expansion method of the above embodiments.

[0118] Furthermore, the logic instructions in the aforementioned memory 121 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium.

[0119] The memory 121, as a computer-readable storage medium, can be used to store software programs and computer-executable programs, such as program instructions / modules corresponding to the methods in the embodiments of this disclosure. The processor 120 executes functional applications and data processing by running the program instructions / modules stored in the memory 121, that is, it implements the raster map-based map expansion method in the above embodiments.

[0120] The memory 121 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the terminal device. Furthermore, the memory 121 may include high-speed random access memory and may also include non-volatile memory.

[0121] This disclosure provides a computer-readable storage medium storing computer-executable instructions configured to perform the above-described raster map-based map expansion method.

[0122] The technical solutions of this disclosure can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes one or more instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in this disclosure. The aforementioned storage medium can be a non-transitory storage medium, such as a USB flash drive, external hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk, etc., and other media capable of storing program code.

[0123] The foregoing description and accompanying drawings fully illustrate embodiments of this disclosure to enable those skilled in the art to practice them. Other embodiments may include structural, logical, electrical, procedural, and other changes. The embodiments represent only possible variations. Individual components and functions are optional unless explicitly required, and the order of operation may vary. Parts and features of some embodiments may be included in or replace parts and features of other embodiments. Moreover, the terminology used in this application is for describing embodiments only and is not intended to limit the claims. As used in the description of embodiments and claims, the singular forms “a,” “an,” and “the” are intended to equally include the plural forms unless the context clearly indicates otherwise. Similarly, the term “and / or” as used in this application means including one or more of the associated listed items and all possible combinations thereof. Additionally, when used in this application, the term "comprise" and its variations "comprises" and / or "comprising" refer to the presence of stated features, integrals, steps, operations, elements, and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components, and / or groups thereof. Without further limitations, an element defined by the phrase "comprises a..." does not exclude the presence of other identical elements in the process, method, or apparatus that includes said element. In this document, each embodiment focuses on the differences that may exist from other embodiments, and similar or identical parts between embodiments can be referred to mutually. For methods, products, etc., disclosed in the embodiments, if they correspond to the method section disclosed in the embodiments, the relevant parts can be referred to the description of the method section.

[0124] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the embodiments of this disclosure. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0125] The methods and products disclosed in the embodiments herein (including but not limited to devices and equipment) can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For instance, the division of units may be merely a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed may be through some interfaces, and the indirect coupling or communication connection of devices or units may be electrical, mechanical, or other forms. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to implement this embodiment according to actual needs. In addition, the functional units in the embodiments of this disclosure may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0126] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to embodiments of this disclosure. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the blocks may occur in a different order than that shown in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. In the descriptions corresponding to the flowcharts and block diagrams in the accompanying drawings, the operations or steps corresponding to different blocks may also occur in a different order than disclosed in the description, and sometimes there is no specific order between different operations or steps. For example, two consecutive operations or steps may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. Each block in a block diagram and / or flowchart, and combinations of blocks in a block diagram and / or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

Claims

1. A method for expanding a raster map, characterized in that, include: Based on the previously constructed old grid map data, as well as the robot's current laser point cloud data and current pose data, obtain the robot's precise pose in the old grid map; Using the precise pose as the starting position for map expansion, real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area are acquired in real time to continuously generate multiple keyframe data corresponding to the target area. The multiple keyframe data are optimized and matched according to the pre-constructed boundary constraints to obtain the optimized multiple keyframe data. Based on the old raster map data and the optimized keyframe data, a new raster map data containing the target area is generated.

2. The mapping expansion method according to claim 1, characterized in that, The step of obtaining the robot's precise pose in the old grid map based on the previously constructed old grid map data, the robot's current laser point cloud data, and the current pose data includes: Load the old raster map data that was previously constructed, wherein the old raster map data includes the old laser point cloud data, old pose data and old map optimization data saved after the previous map was constructed; Based on the old laser point cloud data, old pose data, and old map optimization data, as well as the robot's current laser point cloud data and current pose data, the robot's initial pose in the old grid map is initially estimated. The initial pose is optimized by the iterative nearest point algorithm, and the converged accurate pose is obtained after several iterations.

3. The mapping expansion method according to claim 2, characterized in that, The step of optimizing the initial pose using an iterative nearest-point algorithm to obtain a converged accurate pose after several iterations includes: Obtain several candidate poses within a preset range; For each candidate pose, the iterative nearest point algorithm based on SVD singular values ​​is used to iteratively match the optimal translation and rotation between two laser point clouds by minimizing the objective function, and finally obtain the converged accurate pose. Among them, p i and q i This represents two laser point clouds that need to be registered. The goal is to minimize the following objective function: Then calculate the centroid: ， Decentralized point set: , Expand and differentiate the objective function: The optimal translation is obtained: Substituting t* back into the objective function, we simplify and expand the objective function: The objective function is equivalent to: Define the covariance matrix: but: Perform singular value decomposition on H: Where U is an orthogonal matrix, E is a diagonal matrix containing singular values, and V T It is the transpose of another orthogonal matrix V; We need to find the optimal pose: The optimal rotation matrix is: Optimal translation: Final transformation: Where, p i and q i Given two laser point cloud datasets that need to be matched, R is the rotation matrix and t is the translation matrix.

4. The mapping expansion method according to claim 1, characterized in that, The process involves using the precise pose as the starting point for mapping expansion, and continuously acquiring real-time odometry data and real-time laser point cloud data collected during the robot's movement within the target area. This data is used to continuously generate multiple keyframe data corresponding to the target area, including: A local map is constructed at the initial map expansion location, and the real-time laser point cloud data is fused into the old raster map data according to the precise pose. When the robot moves a target distance within the target area, multiple keyframe data are continuously generated based on real-time odometer data and real-time laser point cloud data. The target area is either a known area inside the old grid map or an unknown area at the boundary of the old grid map. The keyframe data is stored in a preset dynamic update layer.

5. The mapping expansion method according to claim 4, characterized in that, The step of optimizing and matching multiple keyframe data according to pre-constructed boundary constraints to obtain optimized multiple keyframe data includes: When the robot is located in an unknown region of the boundary of the old grid map, the boundary constraint optimizer is activated; Unary edge constraints and binary edge constraints are constructed through a boundary constraint optimizer. The unary edge constraints include point-to-point and point-to-edge constraints, and the binary edge constraints include relative pose relationship constraints established between two consecutive keyframes and relative pose relationship constraints established between two non-consecutive keyframes generated by loop closure detection. Local graph optimization is performed on multiple keyframe data in the dynamically updated layer using unary edge constraints; and... Global graph optimization is performed on multiple keyframe data in the dynamically updated layer using binary edge constraints.

6. The mapping expansion method according to claim 1, characterized in that, The step of generating new raster map data containing the target area based on the old raster map data and the optimized keyframe data includes: The old laser point cloud data, old pose data, and old image optimization data are updated based on the optimized keyframe data to obtain new laser point cloud data, new pose data, and new image optimization data; By using a multi-resolution raster matching algorithm, new laser point cloud data is mapped to different locations on the old raster map according to probability, thereby generating new raster map data containing the target area.

7. The mapping expansion method according to any one of claims 1 to 6, characterized in that, Also includes: A new raster map is generated in real time based on the new raster map data; The new grid map is displayed via a terminal device.

8. A raster map-based map expansion device, characterized in that, include: The initial pose estimation module is configured to obtain the robot's precise pose in the old grid map based on the previously constructed old grid map data, the robot's current laser point cloud data, and the current pose data. The keyframe acquisition module is configured to use the precise pose as the starting position for mapping expansion, and to acquire real-time odometry data and real-time laser point cloud data collected by the robot during its movement within the target area, so as to continuously generate multiple keyframe data corresponding to the target area. A boundary constraint module is configured to perform optimized matching on multiple keyframe data according to pre-built boundary constraints to obtain optimized multiple keyframe data. The map update fusion module is configured to generate new raster map data containing the target area based on the old raster map data and the optimized keyframe data.

9. A robot comprising a processor and a memory storing program instructions, characterized in that, The processor is configured to execute the raster map-based map expansion method as described in any one of claims 1 to 7 when running the program instructions.

10. A computer-readable storage medium storing program instructions, characterized in that, When the program instructions are executed, they cause the computer to perform the raster map-based map expansion method as described in any one of claims 1 to 7.

Images