A mapping and positioning method and system suitable for a fire-fighting robot

Abstract

The application discloses a kind of suitable for fire-fighting robot mapping positioning method and system, comprising: the laser radar data, IMU data and time stamp obtained are preprocessed, and IMU pre-integral result is generated;Positioning mapping is carried out based on LIO-SAM, laser radar data is motion compensated, feature is extracted and local map matching is generated to generate pose initial estimation, and basic factor graph is constructed;When positioning error is over threshold or lost, start ScanContext repositioning to generate repositioning result;It is converted into repositioning factor and added to basic factor graph, and optimization pose is solved and positioning state is judged;According to optimization pose, update local map, construct indoor and outdoor layered global map, and guarantee splicing accuracy through repositioning factor;Output pose, state, map, and alarm when positioning is lost or fails;The application realizes real-time positioning and autonomous repositioning closed loop, improves accuracy and robustness.

Description

Technical Field

[0001] This invention relates to a mapping and positioning method and system suitable for firefighting robots, belonging to the field of robot positioning and mapping technology. Background Technology

[0002] In complex environments such as fire rescue and disaster response, firefighting robots need to acquire high-precision maps of their own pose and the surrounding environment in real time to perform tasks such as autonomous navigation, target search, and path planning. However, firefighting environments are typically characterized by high temperatures, dense smoke, dust, water mist, and insufficient lighting, posing a severe challenge to the robot's localization and mapping capabilities.

[0003] Existing firefighting robots rely on lidar point cloud data for feature matching and pose estimation. In smoke and dust environments, the lidar point cloud generates numerous noisy and isolated points, leading to feature extraction failures. Furthermore, pure lidar methods suffer from severe motion distortion in single-frame point clouds during rapid robot rotation or translation, affecting matching accuracy. In addition, they lack global relocalization capabilities, meaning they cannot automatically recover from lost localization (e.g., due to obstruction or entry into long, straight corridors). Current technologies still exhibit significant shortcomings in robustness, continuity, and automatic recovery capabilities after localization loss in complex fire environments. Summary of the Invention

[0004] The technical problem to be solved by the present invention is that the existing technology suffers from problems such as easy loss of positioning in complex environments, low accuracy of multi-sensor fusion, and lack of automatic repositioning.

[0005] To solve the above-mentioned technical problems, the present invention is implemented using the following technical solution.

[0006] On one hand, the present invention provides a mapping and positioning method suitable for firefighting robots, comprising:

[0007] Acquire data from each sensor of the firefighting robot and the timestamp of each sensor. The sensor data includes lidar data and IMU data.

[0008] The sensor data and timestamps of each sensor are preprocessed to generate IMU pre-integration results;

[0009] Based on LIO-SAM, localization and mapping are performed to obtain the real-time pose and local map of the fire-fighting robot. The motion compensation of the LiDAR data is performed using the IMU pre-integration results to obtain a distortion-free point cloud. The features of the LiDAR data of the current frame are extracted from the distortion-free point cloud and matched with the local map to generate an initial pose estimate of the current frame. A basic factor map is constructed based on the IMU pre-integration results and the initial pose estimate.

[0010] Calculate the localization error between the pose of the current frame and the distortion-free point cloud. When the localization error exceeds a preset threshold or localization is lost, initiate ScanContext relocalization to generate a relocalization result.

[0011] The relocation result is converted into a relocation factor and added to the basic factor graph to form a complete factor graph model. The optimized pose is obtained by solving the complete factor graph model, and the positioning status is determined based on the positioning error.

[0012] The local map is updated based on the optimized pose and historical LiDAR frame point clouds that exceed the preset range are removed. A layered global map containing indoor and outdoor layers is constructed based on the optimized pose, and the stitching accuracy of the indoor and outdoor maps is ensured through the relocation factor.

[0013] It outputs the real-time pose, positioning status, local map, and global map of the fire-fighting robot, and outputs an alarm signal when positioning is lost or repositioning fails.

[0014] By integrating LIO-SAM and ScanContext, a closed loop of real-time positioning and autonomous relocation is achieved. Through tight coupling optimization of relocation factors and basic factor maps, the overall positioning accuracy and robustness are improved. A hierarchical global map of indoor and outdoor areas is constructed and the stitching accuracy is ensured by using relocation factors. At the same time, alarm output is provided, enhancing the reliability of the system.

[0015] The preprocessing includes:

[0016] The timestamps of each sensor are synchronized using linear interpolation.

[0017] The lidar data is downsampled to reduce the amount of data, and isolated points are removed by radius filtering.

[0018] The IMU data is subjected to zero bias calibration and noise filtering. Then, the relative pose, velocity and deviation between adjacent lidar frames are calculated based on the pre-integration algorithm to generate the IMU pre-integration result.

[0019] Time synchronization eliminates data association errors caused by differences in sampling frequencies among sensors.

[0020] The steps of extracting features from the current frame's LiDAR data from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate for the current frame, and constructing a base factor map based on the IMU pre-integration results and the initial pose estimate, include:

[0021] Features are extracted from the distortion-free point cloud. Corner points and face points are extracted separately during feature extraction, and the number of features extracted in each frame is controlled.

[0022] A local map is constructed centered on the current pose of the fire-fighting robot. The corner points and face points are matched with the corner points and face points in the local map, respectively. The pose increment is calculated by matching similar features. The pose increment is fused with the IMU pre-integration result to generate the initial pose estimate of the current frame.

[0023] A basic factor graph containing IMU factors and lidar factors is constructed. The IMU factors are generated from the IMU pre-integration results, and the lidar factors are generated from the feature matching results.

[0024] The differentiation and extraction of corner points and surface points, as well as the control of their number, ensure matching accuracy while meeting real-time requirements. The local map is composed of point clouds from the most recent frames, and combined with IMU pre-integration prediction, the degradation resistance of the initial pose estimation is improved.

[0025] The steps for generating relocation results using ScanContext relocation include:

[0026] When the robot's localization error exceeds a preset threshold or localization is lost, a ScanContext descriptor for the current frame point cloud is generated, and a descriptor similar to the ScanContext descriptor is retrieved from a pre-built global descriptor database to obtain candidate poses.

[0027] The candidate poses are verified by ICP matching, and the successfully verified poses are used as the relocalization results.

[0028] The ScanContext global descriptor retrieval mechanism is introduced, enabling the system to quickly find historically similar scenes when localization is lost or the accumulated error is large. Combined with ICP verification to filter candidate poses, the reliability of the relocalization results is ensured.

[0029] When generating the ScanContext descriptor, the point cloud is divided into grids according to polar coordinates, and the height information of each grid is statistically analyzed and normalized.

[0030] The global descriptor sub-database adopts a KD-tree structure;

[0031] Descriptor matching uses cosine similarity to filter several candidates with similarity greater than a threshold;

[0032] Pose verification uses ICP matching, and candidates with errors less than a threshold are selected as relocalization results.

[0033] When generating the ScanContext descriptor, the distortion-free LiDAR point cloud is transformed into the robot coordinate system, and the grid is divided according to polar coordinates. The radial grid is divided into 20 grids, and the angular grid is divided into 60 grids. The maximum height and average height of the point cloud in each grid are counted to form a 20×60 feature matrix. The feature matrix is ​​then subjected to circular shift normalization to generate the ScanContext descriptor.

[0034] The steps of converting the relocalization result into a relocalization factor and adding it to the basic factor graph to form a complete factor graph model, solving the complete factor graph model to obtain the optimized pose, and determining the localization state based on the localization error include:

[0035] The relocation results are converted into relocation factors, and the relocation factors are added to the basic factor graph to form a complete factor graph model.

[0036] The complete factor graph model is solved using the Levenberg-Marquardt algorithm, and the calculation is accelerated by sparse matrix decomposition to obtain the optimized robot pose. The positioning status is then determined based on the positioning error between the optimized pose and the point cloud.

[0037] The relocalization results are added to the original base factor map in the form of factors, so that the relocalization constraints, IMU factors, and LiDAR factors are jointly optimized in the same framework, avoiding the situation where relocalization only resets the pose and discards historical information.

[0038] Also includes:

[0039] The global map is periodically optimized, and historical pose associations are discovered through loop closure detection. Loop closure factors are then constructed and added to the global factor map.

[0040] The global map is stored in an octree structure and supports version rollback.

[0041] By incorporating periodic loop closure detection and a loop closure factor, accumulated errors that may occur after long-term operation or multiple relocations can be eliminated, ensuring the consistency of the global map. The octree structure compresses idle areas, significantly reducing map storage overhead, making it suitable for firefighting robots to operate for extended periods.

[0042] Secondly, the present invention provides a system for the mapping and positioning method applicable to firefighting robots, comprising:

[0043] Data acquisition module: used to acquire data from various sensors of the fire-fighting robot and the timestamps of each sensor. The sensor data includes lidar data and IMU data.

[0044] Data preprocessing module: used to preprocess the sensor data and the timestamps of each sensor to generate IMU pre-integration results;

[0045] The LIO-SAM and ScanContext fusion module is used for: obtaining real-time fire robot pose and local map based on LIO-SAM localization and mapping; using the IMU pre-integration results to perform motion compensation on the LiDAR data to obtain a distortion-free point cloud; extracting features of the LiDAR data of the current frame from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate for the current frame; constructing a basic factor map based on the IMU pre-integration results and the initial pose estimate; calculating the localization error between the pose of the current frame and the distortion-free point cloud; when the localization error exceeds a preset threshold or localization is lost, initiating ScanContext relocalization to generate a relocalization result; converting the relocalization result into a relocalization factor and adding it to the basic factor map to form a complete factor map model; solving the complete factor map model to obtain the optimized pose; and determining the localization status based on the localization error.

[0046] Map management module: used to: update the local map according to the optimized pose and remove historical LiDAR frame point clouds that exceed the preset range, construct a layered global map containing indoor and outdoor layers based on the optimized pose, and ensure the stitching accuracy of indoor and outdoor maps through the repositioning factor;

[0047] Application output module: Used to output the real-time pose, positioning status, local map and global map of the fire-fighting robot, and to output an alarm signal when positioning is lost or repositioning fails.

[0048] The LIO-SAM and ScanContext fusion module includes:

[0049] LIO-SAM localization and mapping unit: used for: performing localization and mapping based on LIO-SAM to obtain the real-time pose and local map of the fire-fighting robot; using the IMU pre-integration result to perform motion compensation on the lidar data to obtain a distortion-free point cloud; extracting features of the lidar data of the current frame from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate of the current frame; and constructing a basic factor map based on the IMU pre-integration result and the initial pose estimate.

[0050] ScanContext relocalization unit: used to: calculate the localization error between the pose of the current frame and the distortion-free point cloud; when the localization error exceeds a preset threshold or localization is lost, ScanContext relocalization is initiated to generate a relocalization result.

[0051] Fusion optimization unit: used to: convert the relocation result into a relocation factor and add it to the basic factor graph to form a complete factor graph model, solve the complete factor graph model to obtain the optimized pose, and determine the positioning state based on the positioning error.

[0052] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0053] This invention integrates LIO-SAM and ScanContext to achieve a closed loop of real-time positioning and autonomous relocation, effectively solving the problem of fire-fighting robots easily losing positioning and being unable to automatically recover in complex environments. Through tight coupling optimization of relocation factors and basic factor maps, the overall positioning accuracy and robustness are improved. By constructing a hierarchical global map of indoor and outdoor environments and using relocation factors to ensure stitching accuracy, continuous and seamless positioning indoor and outdoor environments are achieved. At the same time, alarm output is provided to enhance the reliability of the system. Attached Figure Description

[0054] Figure 1 This is a schematic diagram of the mapping and positioning method for fire-fighting robots, as shown in Embodiment 1 of the present invention. Detailed Implementation

[0055] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations thereof. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0056] The term "and / or" simply describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. Additionally, the character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0057] Example 1

[0058] like Figure 1 As shown in the figure, this embodiment introduces a mapping and positioning method suitable for firefighting robots, including:

[0059] Acquire data from each sensor of the firefighting robot, including timestamps for each sensor. Sensor data includes LiDAR data and IMU data.

[0060] Preprocess the sensor data and the timestamps of each sensor to generate IMU pre-integration results;

[0061] Based on LIO-SAM, localization and mapping are performed to obtain the real-time pose and local map of the fire-fighting robot. The motion compensation of the LiDAR data is performed using the IMU pre-integration results to obtain the distortion-free point cloud. The features of the LiDAR data of the current frame are extracted from the distortion-free point cloud and matched with the local map to generate the initial pose estimate of the current frame. The basic factor map is constructed based on the IMU pre-integration results and the initial pose estimate.

[0062] Calculate the localization error between the pose of the current frame and the distortion-free point cloud. When the localization error exceeds a preset threshold or localization is lost, initiate ScanContext relocalization to generate a relocalization result.

[0063] The relocalization results are converted into relocalization factors and added to the base factor graph to form a complete factor graph model. The optimized pose is obtained by solving the complete factor graph model, and the localization status is determined based on the localization error.

[0064] The local map is updated based on the optimized pose and historical LiDAR frame point clouds that exceed the preset range are removed. A layered global map containing indoor and outdoor layers is constructed based on the optimized pose, and the stitching accuracy of indoor and outdoor maps is ensured by the relocation factor.

[0065] It outputs the real-time pose, positioning status, local map, and global map of the fire-fighting robot, and outputs an alarm signal when positioning is lost or repositioning fails.

[0066] Specifically, LIO-SAM stands for LiDAR-IMU Odometry and Simultaneous And Mapping; IMU stands for Inertial Measurement Unit; and ScanContext stands for Scene Recognition and Relocalization.

[0067] Specifically, the system outputs the real-time pose, positioning accuracy, and positioning status of the firefighting robot at a frequency of 10Hz, using the ROS PoseStamped message type as the data format; it outputs local and global maps in real time, with the local map output frequency at 10Hz and the global map output frequency at 1Hz, supporting PCD and occupancy grid map formats; when positioning is lost or repositioning fails, it outputs audible and visual alarm signals and simultaneously sends alarm information to the rescue command center to prompt manual intervention.

[0068] Preprocessing, including:

[0069] Linear interpolation is used to synchronize the timestamps of each sensor.

[0070] The lidar data is downsampled to reduce the amount of data, and isolated points are removed by radius filtering.

[0071] The IMU data is subjected to zero bias calibration and noise filtering. Then, the relative pose, velocity and deviation between adjacent lidar frames are calculated based on the pre-integration algorithm to generate the IMU pre-integration results.

[0072] Specifically, the raw IMU data is zero-biased by calculating the zero bias value through static initialization, and the raw IMU data is noise filtered by removing high-frequency noise using Kalman filtering. Then, the relative pose, velocity and deviation between adjacent lidar frames are calculated based on the integration of the IMU's angular velocity and acceleration to generate the IMU pre-integration result. The pre-integration window length is consistent with the lidar frame interval of 100ms.

[0073] Specifically, the zero-bias calibration time for the raw IMU data is 3 seconds. The IMU zero bias is calculated during static initialization to ensure pre-integration accuracy. The noise filtering threshold for the raw IMU data is [value missing]. .

[0074] Specifically, voxel filtering is used to downsample the original point cloud of the lidar to reduce the amount of data. The voxel size is 0.1m×0.1m×0.1m. At the same time, radius filtering is used to remove isolated points and eliminate invalid points caused by dust and noise.

[0075] Specifically, sensor data includes GPS data. The GPS data undergoes pseudorange error correction and multipath effect suppression. The positioning results and confidence scores are extracted. When the confidence score is ≥0.8, it is in RTK positioning mode, and the positioning data is retained for subsequent fusion. When the confidence score is <0.8, it is in single-point positioning mode, which is temporarily abandoned to avoid introducing errors.

[0076] The steps of extracting features from the current frame's LiDAR data from the distorted point cloud and matching them with the local map to generate an initial pose estimate for the current frame, and constructing a base factor map based on the IMU pre-integration results and the initial pose estimate, include:

[0077] Features are extracted from the distortion-free point cloud. Corner points and face points are extracted separately during feature extraction, and the number of features extracted in each frame is controlled.

[0078] A local map is constructed centered on the current pose of the fire-fighting robot. Corner points and face points are matched with corner points and face points in the local map, respectively. The pose increment is calculated by matching similar features. The pose increment is fused with the IMU pre-integration result to generate the initial pose estimate of the current frame.

[0079] A basic factor graph containing IMU factors and lidar factors is constructed. The IMU factors are generated from the IMU pre-integration results, and the lidar factors are generated from the feature matching results.

[0080] Specifically, motion information obtained through IMU pre-integration is used to perform point-by-point motion compensation on the LiDAR data, eliminating point cloud distortion caused by the robot's translation and rotation during the LiDAR scanning cycle. Corner and face features are extracted from the distorted point cloud. Corner points are filtered by calculating the rate of change of the point cloud normal vector, and face points are filtered by the rate of change of the normal vector being less than a threshold. To ensure real-time performance, 200 corner points and 400 face points are extracted for each LiDAR frame. A local map with a radius of 5m is constructed with the current robot pose as the center. The local map consists of the distorted point clouds from the most recent 10 LiDAR frames and is updated using a sliding window mechanism to remove historical point clouds that exceed the window range, reducing storage and computational overhead. The corner and face points extracted in the current frame are matched with the corresponding features in the local map, and the improved ICP (Iterative Closest Point) algorithm is used to calculate the pose increment. Combined with the pose prediction results obtained through IMU pre-integration, an initial pose estimate for the current frame is generated. A factor graph model is constructed, where the robot's pose is the variable.

[0081] Specifically, the threshold for the rate of change of the corner normal vector is 0.8, which filters corner points with obvious geometric features; the threshold for the rate of change of the surface normal vector is 0.2, which filters surface points in planar regions.

[0082] Specifically, the basic factors include: IMU factors: generated from IMU pre-integration results, constraining changes in adjacent poses; and LiDAR factors: generated from point cloud matching results, constraining the consistency between the current pose and the local map.

[0083] The steps for generating relocation results using ScanContext relocation include:

[0084] When the robot's localization error exceeds a preset threshold or localization is lost, a ScanContext descriptor for the current frame point cloud is generated. A descriptor similar to the ScanContext descriptor is retrieved from a pre-built global descriptor database to obtain candidate poses.

[0085] Candidate poses are verified by ICP matching, and the successfully verified poses are used as the relocalization results.

[0086] When generating the ScanContext descriptor, the point cloud is divided into grids according to polar coordinates, and the height information of each grid is statistically analyzed and normalized.

[0087] The global descriptor database uses a KD-tree structure;

[0088] Descriptor matching uses cosine similarity to filter several candidates with similarity greater than a threshold;

[0089] Pose verification uses ICP matching, and candidates with errors less than a threshold are selected as relocalization results.

[0090] Specifically, the construction of the global descriptor database includes: as the robot moves, storing the ScanContext descriptor of each frame and the corresponding optimized pose into the global descriptor database. The database is organized using a KD tree structure to support fast retrieval.

[0091] Specifically, the cosine similarity between the current frame ScanContext descriptor and all descriptors in the global descriptor database is calculated, and the top 5 descriptors with a similarity greater than 0.8 are selected as candidate matches; the historical poses corresponding to the candidate descriptors are obtained as candidate poses.

[0092] Specifically, ICP matching is performed between the current frame point cloud and the historical point cloud corresponding to each candidate pose, the matching error is calculated, and the candidate pose with the smallest error is selected as the relocalization result. If the matching error of all candidate poses is greater than the threshold, it is determined that there is no valid match, the current localization mode is maintained and a relocalization failure is indicated, requiring manual assistance.

[0093] Specifically, the ICP matching error threshold is 0.1m to verify the effectiveness of the candidate pose.

[0094] When generating the ScanContext descriptor, the distortion-free LiDAR point cloud is transformed into the robot coordinate system, and the grid is divided according to polar coordinates. The radial grid is divided into 20 grids, and the angular grid is divided into 60 grids. The maximum height and average height of the point cloud in each grid are counted to form a 20×60 feature matrix. The feature matrix is ​​then subjected to circular shift normalization to generate the ScanContext descriptor.

[0095] The steps of converting the relocalization results into relocalization factors and adding them to the base factor graph to form a complete factor graph model, solving the complete factor graph model to obtain the optimized pose, and determining the localization state based on the localization error include:

[0096] The relocation results are converted into relocation factors, and the relocation factors are added to the basic factor graph to form a complete factor graph model.

[0097] The Levenberg-Marquardt algorithm is used to solve the complete factor graph model, and sparse matrix decomposition is used to accelerate the calculation to obtain the optimized robot pose. The positioning status is then determined based on the positioning error between the optimized pose and the point cloud.

[0098] Specifically, the ScanContext relocation results and GPS data are integrated into the basic factor graph, including: converting the ScanContext relocation results into relocation factors and setting factor weights based on the relocation matching error; if the GPS data is valid, converting the GPS positioning results into GPS factors and setting weights positively correlated with confidence; adding the relocation factors and GPS factors to the basic factor graph of LIO-SAM to form a complete factor graph model; using the Levenberg-Marquardt algorithm to solve the factor graph, minimizing the observation error of all factors, and obtaining the optimized robot pose; during the solution process, sparse matrix factorization is used to accelerate the calculation and ensure real-time performance; based on the matching error between the optimized pose and the LiDAR point cloud, the positioning status is determined: error <3cm indicates accurate positioning, 3-5cm indicates positioning to be calibrated and triggers the next relocation detection, and >5cm indicates positioning loss and immediately initiates the relocation process.

[0099] Also includes:

[0100] The global map is optimized periodically, and historical pose correlations are discovered through loop closure detection. Loop closure factors are then constructed and added to the global factor map.

[0101] The global map is stored using an octree structure and supports version rollback.

[0102] Example 2

[0103] Based on the same inventive concept as Embodiment 1, this embodiment introduces a system based on a mapping and positioning method suitable for firefighting robots, comprising:

[0104] Data acquisition module: used to acquire data from various sensors of the fire-fighting robot and the timestamps of each sensor. Sensor data includes lidar data and IMU data.

[0105] Data preprocessing module: used to preprocess sensor data and timestamps of each sensor to generate IMU pre-integration results;

[0106] The LIO-SAM and ScanContext fusion module is used for: obtaining real-time fire robot pose and local map based on LIO-SAM localization and mapping; using IMU pre-integration results to perform motion compensation on LiDAR data to obtain a distortion-free point cloud; extracting features of the current frame's LiDAR data from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate for the current frame; constructing a basic factor map based on the IMU pre-integration results and the initial pose estimate; calculating the localization error between the current frame's pose and the distortion-free point cloud; when the localization error exceeds a preset threshold or localization is lost, initiating ScanContext relocalization to generate a relocalization result; converting the relocalization result into a relocalization factor and adding it to the basic factor map to form a complete factor map model; solving the complete factor map model to obtain the optimized pose; and determining the localization status based on the localization error.

[0107] Map Management Module: Used to update local maps based on optimized poses and remove historical LiDAR frame point clouds that exceed preset ranges; construct a layered global map containing indoor and outdoor layers based on optimized poses; and ensure the stitching accuracy of indoor and outdoor maps through relocation factors.

[0108] Application output module: Used to output the real-time pose, positioning status, local map and global map of the fire-fighting robot, and to output an alarm signal when positioning is lost or repositioning fails.

[0109] The LIO-SAM and ScanContext fusion module includes:

[0110] LIO-SAM localization and mapping unit: used for: localization and mapping based on LIO-SAM to obtain real-time fire robot pose and local map, motion compensation of LiDAR data using IMU pre-integration results to obtain distortion-free point cloud, extracting features of the current frame LiDAR data from the distortion-free point cloud and matching them with the local map to generate initial pose estimation of the current frame, and constructing basic factor map based on IMU pre-integration results and initial pose estimation.

[0111] ScanContext relocalization unit: used to calculate the localization error between the pose of the current frame and the distortion-free point cloud. When the localization error exceeds a preset threshold or localization is lost, ScanContext relocalization is initiated to generate a relocalization result.

[0112] Fusion optimization unit: used to: convert the relocalization result into relocalization factors and add them to the base factor graph to form a complete factor graph model, solve the complete factor graph model to obtain the optimized pose, and determine the localization status based on the localization error.

[0113] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0114] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0115] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0116] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0117] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention 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 the present invention without departing from the spirit and scope of the claims. All of these forms are within the protection scope of the present invention.

Claims

1. A mapping and positioning method suitable for firefighting robots, characterized in that, include: Acquire data from each sensor of the firefighting robot and the timestamp of each sensor. The sensor data includes lidar data and IMU data. The sensor data and timestamps of each sensor are preprocessed to generate IMU pre-integration results; Based on LIO-SAM, localization and mapping are performed to obtain the real-time pose and local map of the fire-fighting robot. The motion compensation of the LiDAR data is performed using the IMU pre-integration results to obtain a distortion-free point cloud. The features of the LiDAR data of the current frame are extracted from the distortion-free point cloud and matched with the local map to generate an initial pose estimate of the current frame. A basic factor map is constructed based on the IMU pre-integration results and the initial pose estimate. Calculate the localization error between the pose of the current frame and the distortion-free point cloud. When the localization error exceeds a preset threshold or localization is lost, initiate ScanContext relocalization to generate a relocalization result. The relocation result is converted into a relocation factor and added to the basic factor graph to form a complete factor graph model. The optimized pose is obtained by solving the complete factor graph model, and the positioning status is determined based on the positioning error. The local map is updated based on the optimized pose and historical LiDAR frame point clouds that exceed the preset range are removed. A layered global map containing indoor and outdoor layers is constructed based on the optimized pose, and the stitching accuracy of the indoor and outdoor maps is ensured through the relocation factor. It outputs the real-time pose, positioning status, local map, and global map of the fire-fighting robot, and outputs an alarm signal when positioning is lost or repositioning fails.

2. The mapping and positioning method for firefighting robots according to claim 1, characterized in that, The preprocessing includes: The timestamps of each sensor are synchronized using linear interpolation. The lidar data is downsampled to reduce the amount of data, and isolated points are removed by radius filtering. The IMU data is subjected to zero bias calibration and noise filtering. Then, the relative pose, velocity and deviation between adjacent lidar frames are calculated based on the pre-integration algorithm to generate the IMU pre-integration result.

3. The mapping and positioning method for firefighting robots according to claim 1, characterized in that, The steps of extracting features from the current frame's LiDAR data from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate for the current frame, and constructing a base factor map based on the IMU pre-integration results and the initial pose estimate, include: Features are extracted from the distortion-free point cloud. Corner points and face points are extracted separately during feature extraction, and the number of features extracted in each frame is controlled. A local map is constructed centered on the current pose of the fire-fighting robot. The corner points and face points are matched with the corner points and face points in the local map, respectively. The pose increment is calculated by matching similar features. The pose increment is fused with the IMU pre-integration result to generate the initial pose estimate of the current frame. A basic factor graph containing IMU factors and lidar factors is constructed. The IMU factors are generated from the IMU pre-integration results, and the lidar factors are generated from the feature matching results.

4. The mapping and positioning method for firefighting robots according to claim 1, characterized in that, The steps for generating relocation results using ScanContext relocation include: When the robot's localization error exceeds a preset threshold or localization is lost, a ScanContext descriptor for the current frame point cloud is generated, and a descriptor similar to the ScanContext descriptor is retrieved from a pre-built global descriptor database to obtain candidate poses. The candidate poses are verified by ICP matching, and the successfully verified poses are used as the relocalization results.

5. The mapping and positioning method for firefighting robots according to claim 4, characterized in that, When generating the ScanContext descriptor, the point cloud is divided into grids according to polar coordinates, and the height information of each grid is statistically analyzed and normalized. The global descriptor sub-database adopts a KD-tree structure; Descriptor matching uses cosine similarity to filter several candidates with similarity greater than a threshold; Pose verification uses ICP matching, and candidates with errors less than a threshold are selected as relocalization results.

6. The mapping and positioning method for firefighting robots according to claim 4, characterized in that, When generating the ScanContext descriptor, the distortion-free LiDAR point cloud is transformed into the robot coordinate system, and the grid is divided according to polar coordinates. The radial grid is divided into 20 grids, and the angular grid is divided into 60 grids. The maximum height and average height of the point cloud in each grid are counted to form a 20×60 feature matrix. The feature matrix is ​​then subjected to circular shift normalization to generate the ScanContext descriptor.

7. The mapping and positioning method for firefighting robots according to claim 1, characterized in that, The steps of converting the relocalization result into a relocalization factor and adding it to the basic factor graph to form a complete factor graph model, solving the complete factor graph model to obtain the optimized pose, and determining the localization state based on the localization error include: The relocation results are converted into relocation factors, and the relocation factors are added to the basic factor graph to form a complete factor graph model. The complete factor graph model is solved using the Levenberg-Marquardt algorithm, and the calculation is accelerated by sparse matrix decomposition to obtain the optimized robot pose. The positioning status is then determined based on the positioning error between the optimized pose and the point cloud.

8. The mapping and positioning method for firefighting robots according to claim 1, characterized in that, Also includes: The global map is periodically optimized, and historical pose associations are discovered through loop closure detection. Loop closure factors are then constructed and added to the global factor map. The global map is stored in an octree structure and supports version rollback.

9. A system based on the mapping and positioning method for firefighting robots according to any one of claims 1-8, characterized in that, include: Data acquisition module: used to acquire data from various sensors of the fire-fighting robot and the timestamps of each sensor. The sensor data includes lidar data and IMU data. Data preprocessing module: used to preprocess the sensor data and the timestamps of each sensor to generate IMU pre-integration results; The LIO-SAM and ScanContext fusion module is used for: obtaining real-time fire robot pose and local map based on LIO-SAM localization and mapping; using the IMU pre-integration results to perform motion compensation on the LiDAR data to obtain a distortion-free point cloud; extracting features of the LiDAR data of the current frame from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate for the current frame; constructing a basic factor map based on the IMU pre-integration results and the initial pose estimate; calculating the localization error between the pose of the current frame and the distortion-free point cloud; when the localization error exceeds a preset threshold or localization is lost, initiating ScanContext relocalization to generate a relocalization result; converting the relocalization result into a relocalization factor and adding it to the basic factor map to form a complete factor map model; solving the complete factor map model to obtain the optimized pose; and determining the localization status based on the localization error. Map management module: used to: update the local map according to the optimized pose and remove historical LiDAR frame point clouds that exceed the preset range, construct a layered global map containing indoor and outdoor layers based on the optimized pose, and ensure the stitching accuracy of indoor and outdoor maps through the repositioning factor; Application output module: Used to output the real-time pose, positioning status, local map and global map of the fire-fighting robot, and to output an alarm signal when positioning is lost or repositioning fails.

10. The system according to claim 9, characterized in that, The LIO-SAM and ScanContext fusion module includes: LIO-SAM localization and mapping unit: used for: performing localization and mapping based on LIO-SAM to obtain the real-time pose and local map of the fire-fighting robot; using the IMU pre-integration result to perform motion compensation on the lidar data to obtain a distortion-free point cloud; extracting features of the lidar data of the current frame from the distortion-free point cloud and matching them with the local map to generate an initial pose estimate of the current frame; and constructing a basic factor map based on the IMU pre-integration result and the initial pose estimate. ScanContext relocalization unit: used to: calculate the localization error between the pose of the current frame and the distortion-free point cloud; when the localization error exceeds a preset threshold or localization is lost, ScanContext relocalization is initiated to generate a relocalization result. Fusion optimization unit: used to: convert the relocation result into a relocation factor and add it to the basic factor graph to form a complete factor graph model, solve the complete factor graph model to obtain the optimized pose, and determine the positioning state based on the positioning error.

Images