Stereo structure multi-layer mapping method and system based on ground plane correlation constraint

Abstract

The application provides a three-dimensional structure multi-layer mapping method and system based on ground plane correlation constraints, comprising: a real-time maintenance height state machine, which is used for controlling the robot to switch between a cruising state representing cruising on the same floor and a crossing state representing height change; in the cruising state, the robot acquires environment point cloud data in real time and is correlated to a floor sub-map corresponding to the current height; in the crossing state, the robot suspends updating the floor sub-map and maintains a channel map containing a topological connection relationship; layered map data of each floor is generated and output, and the layered map data comprises a floor index file, a layered point cloud file and a topological connection file. Through the introduction of the ground plane geometric prior constraint and the height probability distribution statistical mechanism, accurate pose determination in the vertical direction and structured management of the multi-floor map are realized.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and in particular to a method and system for multi-layer mapping of three-dimensional structures based on ground plane association constraints. Background Technology

[0002] Intelligent robots with autonomous mobility are increasingly used in indoor scenarios, such as building delivery, automated cleaning, and security patrols. Simultaneous localization and mapping (SLAM) technology is the core foundation for enabling these robots to navigate autonomously. It requires robots to estimate their own trajectory and build an environmental map using their onboard sensors (such as LiDAR, cameras, and IMUs) in completely unknown environments.

[0003] However, when faced with complex building environments including staircases, elevators, long corridors, and multi-story structures, existing SLAM technologies, especially the mainstream Laser Inertial Odometry (LIO) algorithm, still face severe technical challenges, mainly manifested in the following technical problems: severe cumulative drift in the vertical direction (Z-axis) and failure of multi-floor loop detection due to structural similarity. Therefore, cross-floor inspection and mapping in complex building environments remains an urgent technical problem to be solved. Summary of the Invention

[0004] This invention provides a method and system for multi-layer mapping of three-dimensional structures based on ground plane association constraints. It addresses the shortcomings of existing technologies, such as map layering disorder caused by cumulative drift of Z-axis height, false loop detection due to similar floor structures, and low efficiency in large-scene map management when mobile robots perform cross-floor inspection tasks. The invention achieves a high-precision method and system for multi-layer mapping of three-dimensional structures in the vertical direction.

[0005] This invention provides a method for multi-layer mapping of three-dimensional structures based on ground plane correlation constraints, comprising:

[0006] A real-time maintenance height state machine is used to control the robot to switch between a cruise state representing cruising on the same floor and a traversal state representing a change in height.

[0007] During the cruise phase, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height.

[0008] During the traversal state, the robot pauses updating the floor sub-map and maintains a channel map containing topological connections.

[0009] In the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that integrates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection.

[0010] Generate and output layered map data for each floor, including floor index files, layered point cloud files, and topology connection files.

[0011] According to the multi-layer mapping method for three-dimensional structures based on ground plane association constraints provided by the present invention, the step of real-time maintenance of the height state machine specifically includes:

[0012] A sliding window is run in real time to record the robot's vertical axis height data for the most recent preset number of frames, and the distribution histogram of the height data is calculated and the distribution peak value is extracted.

[0013] When the variance of the height data of the sliding window is less than a preset stability threshold, the robot is controlled to switch to cruise mode, and when it first enters a floor, the distribution peak of the sliding window is recorded as the nominal height of that floor.

[0014] When the height data of the sliding window changes monotonically and the difference between the height data and the nominal floor height of the current floor is greater than a preset height threshold, the robot is controlled to switch to the crossing state.

[0015] According to the present invention, a multi-layer mapping method for three-dimensional structures based on ground plane association constraints includes the step of using the nominal floor height of each floor determined by the height state machine as a priori constraint closed-loop detection, specifically comprising:

[0016] Filter multiple candidate frames that are similar in appearance to the current frame;

[0017] Before performing fine registration of loop closure in the same layer, the absolute height difference between the current frame and the candidate frame is calculated. If the absolute height difference is greater than the preset same layer determination threshold, the candidate frame is rejected for loop closure.

[0018] Otherwise, the incremental optimization algorithm is triggered to distribute the closed-loop error to the complete trajectory.

[0019] According to the present invention, a multi-layer mapping method for three-dimensional structures based on ground plane correlation constraints is provided. The step of suppressing robot attitude error on the vertical axis by constructing a laser inertial odometry system that incorporates ground plane constraint factors specifically includes:

[0020] When the robot first enters a floor, a local ground plane extraction process is triggered to obtain and record the effective plane of that floor;

[0021] After the effective plane is output, a ground plane geometric constraint factor is added to the pose node of the current frame. The residual of the ground plane geometric constraint factor is defined as the vertical axis angle error after the local ground plane normal vector of the current frame is transformed to the global reference ground plane. The global reference ground plane is the effective plane of the corresponding floor of the current frame.

[0022] According to the multi-level mapping method for three-dimensional structures based on ground plane association constraints provided by the present invention, the step of triggering a local ground plane extraction process to obtain and record the effective plane of the floor specifically includes:

[0023] Acquire the trigger frame point cloud data when the local ground plane extraction process is triggered, and extract the point cloud data within a preset vertical axis range as a candidate ground point set;

[0024] The random sampling consensus algorithm is executed on the candidate ground point set. Three points are selected iteratively to construct a plane model. The distances from the remaining points to the plane are calculated, and the number of interior points is counted.

[0025] After multiple iterations, the model with the most interior points is selected as the optimal fitting plane;

[0026] Perform a normal vector gravity consistency check on the optimal fitting plane, and determine the optimal fitting plane that passes the check as the valid plane of the current floor, and mark the optimal fitting plane that fails the check as an invalid frame.

[0027] The method for multi-layer mapping of three-dimensional structures based on ground plane correlation constraints provided by the present invention further includes:

[0028] Point cloud data is preprocessed using monitoring data from inertial sensors mounted on the robot to eliminate motion distortion.

[0029] This invention also provides a multi-layer mapping system for three-dimensional structures based on ground plane correlation constraints, comprising:

[0030] The state switching module is used to maintain the height state machine in real time. The height state machine is used to control the robot to switch between the cruise state, which represents cruising on the same floor, and the traversal state, which represents a change in height.

[0031] During the cruise phase, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height.

[0032] During the traversal state, the robot pauses updating the floor sub-map and maintains the channel map that protects the topological connections.

[0033] The mapping module is used to generate and output layered map data for each floor. The layered map data includes floor index files, layered point cloud files, and topology connection files.

[0034] In the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that integrates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection.

[0035] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the three-dimensional structure multi-layer mapping method based on ground plane association constraints as described above.

[0036] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the three-dimensional structure multi-layer mapping method based on ground plane association constraints as described above.

[0037] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the three-dimensional structure multi-layer mapping method based on ground plane association constraints as described above.

[0038] The invention provides a method and system for multi-layer mapping of three-dimensional structures based on ground plane association constraints. By introducing a height state machine and a layered map construction mechanism, and combining a laser inertial odometry that integrates ground plane constraint factors, it achieves vertical drift suppression and map structure optimization for robot mapping in multi-story environments, ensuring the planar independence and clarity of each floor's sub-map, and avoiding interference from dynamic structures such as stairs. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0040] Figure 1 This is a flowchart illustrating the multi-layer mapping method for three-dimensional structures based on ground plane correlation constraints provided by the present invention.

[0041] Figure 2 This is a schematic diagram of the three-dimensional multi-layer mapping system based on ground plane correlation constraints provided by the present invention;

[0042] Figure 3 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0043] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0044] The following is combined with Figure 1 This invention introduces a multi-layer mapping method for three-dimensional structures based on ground plane correlation constraints, such as... Figure 1 As shown, it includes:

[0045] Step 101: Maintain the height state machine in real time. The height state machine is used to control the robot to switch between a cruise state representing cruising on the same floor and a traversal state representing a change in height.

[0046] During the cruise phase, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height.

[0047] During the traversal state, the robot pauses updating the floor sub-map and maintains a channel map containing topological connections.

[0048] In the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that integrates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection.

[0049] In this embodiment, the method is applied to a quadrupedal wheeled robot equipped with sensors including a Velodyne VLP-16 LiDAR (mounted at a height of 0.5 meters) for acquiring environmental point cloud data, a RealSense depth camera, and an Xsens MTi-630 IMU. The system is developed based on the ROS2 architecture, with the core algorithm module written in C++. The PCL library is used for point cloud processing, and the GTSAM library is used for factor graph optimization.

[0050] The LiDAR mounted on the robot collects point cloud data in real time to build a map of the three-dimensional structure, while the IMU inertial sensor is used to correct the robot's walking posture and the acquired point cloud data.

[0051] To avoid map layering errors caused by the cumulative drift of vertical axis (Z-axis) height during the robot's mapping of 3D structures, the system maintains a height state machine in the background in real time to control the robot's cruise / traverse state.

[0052] The cruise state indicates that the robot moves within the same floor at a relatively fixed height. In the cruise state, the robot associates the acquired environmental point cloud data with the floor sub-map corresponding to the current height, thereby constructing and / or updating the floor sub-map.

[0053] The "crossing state" indicates that the robot is undergoing a height change that affects floor determination, that is, the robot is in the process of going up or down the stairs. When in the crossing state, the system stops updating the current floor sub-map and instead generates a channel map that mainly contains topological connections, thereby ensuring that the planar map of each floor sub-map is flat and clear and will not be destroyed by the slope point cloud of the stairs.

[0054] Understandably, in this embodiment, the maps of each floor of the three-dimensional structure are constructed, stored, and optimized in layers. The robot's initial floor and initial height are known. After the robot switches from the traversal state to the cruise state, it indicates that the robot has entered a new floor. At this time, the floor matching process is triggered, the relative difference between the current height and the initial height is calculated, and a match is made. If no match is found for the corresponding relative difference, it means that the robot is currently on a completely new floor that it has not been on before. An index is created for this floor and the relative difference is recorded for subsequent matching. If the calculated relative difference can match the corresponding floor, the floor sub-map file of this floor is determined through the floor index so that the point cloud data obtained during the cruise on this floor can be used for loop closure optimization.

[0055] Optionally, the height state machine is implemented by the semantic state machine module, which is at least responsible for map switching of floor sub-maps.

[0056] Optionally, the robot's height data for a recent period of time can be acquired in real time, and the robot can be switched between cruising and traversing states based on changes in its height.

[0057] Furthermore, in order to suppress Z-axis offset during the mapping process, the system constructs a nonlinear optimization model based on factor graphs. The factor graph, which integrates ground plane constraint factors, is used as the optimization engine for Lidar-Inertial Odometry (LIO) to force the robot to adjust its pitch angle, roll angle, and Z-axis height. This results in a higher-precision, lower-drift LIO system, and finally, the model is used in cruise mode to obtain a model that always fits the real physical floor structure.

[0058] Among them, the ground plane constraint factor is a univariate constraint factor based on the ground plane point cloud data of the current floor. The pre-determined ground plane point cloud data of the current floor is used as the global ground plane. It is compared with the local ground plane of the key frame point cloud data acquired by the robot in the cruise state. The Z-axis angle error between the two planes in the same coordinate system is calculated, which is the attitude residual. Optimization is achieved by minimizing the residual.

[0059] Furthermore, in order to suppress the cumulative error of the Z-axis, before performing point cloud registration and fusion during the loop closure process, the nominal floor height of each floor maintained by the height state machine during the initial mapping process is first used. Point cloud data registration and fusion is only performed when it is determined that the robot's current height corresponds to the same floor as the nominal floor height of the matched floor. This is to fundamentally eliminate the "vertical collapse" or "floor overlap" errors commonly found in multi-layer three-dimensional structures.

[0060] Step 102: Generate and output layered map data for each floor. The layered map data includes floor index files, layered point cloud files, and topology connection files.

[0061] The system controls the robot to move within a three-dimensional structure and constructs layered map data for each floor based on the environmental point cloud data acquired by the robot. It can be understood that, through the above method, the system ultimately generates and outputs not a single large file, but a structured dataset, including floor index files, layered point cloud files, and topology connection files.

[0062] The floor index file records the number of floors (represented as floor IDs), the nominal height of each floor, and the path to the point cloud file corresponding to each floor.

[0063] The layered point cloud file stores an independent point cloud map (i.e., a floor sub-map) for each floor, and each file contains only the static structure of the corresponding floor.

[0064] The topology connection file describes the entrance and exit coordinates of a staircase or elevator shaft, which is used to guide downstream navigation calculations.

[0065] Optionally, the layered map data is incrementally updated using the following dynamic update logic: The `UpdateGlobalMap` function is executed. Based on the current floor ID, the program locks the corresponding point cloud map object `Map_Layer_X`. Then, the `RayCastingUpdate` function is called to perform path planning between different floor maps. Finally, the voxel occupancy probability is updated using a ray casting algorithm. This logic increases the probability of hitting a voxel and reduces the probability of the light path passing through a voxel, thereby automatically removing dynamic obstacles such as pedestrians during long-term observation.

[0066] Optionally, after the mapping task is completed, the layered map data is output in the following way: After the task is completed, the program executes the SaveMapPackage function. This function iterates through all floor containers, serializes and saves each layer map as an independent .pcd file (such as Floor_1.pcd, which represents the floor sub-map of a floor), and generates a .yaml configuration file describing the topological connections between floors (such as the coordinates of stairwells) for use by downstream navigation algorithms.

[0067] This invention introduces a height state machine and a layered map construction mechanism, combined with a laser inertial odometry that incorporates ground plane constraint factors, to achieve vertical drift suppression and map structure optimization for robot mapping in multi-story environments. This ensures the planar independence and clarity of each floor's sub-map and avoids interference from dynamic structures such as stairs.

[0068] In the multi-layer mapping method for three-dimensional structures based on ground plane association constraints of the present invention, the step of real-time maintenance of the height state machine specifically includes:

[0069] A sliding window is run in real time to record the robot's vertical axis height data for the most recent preset number of frames, and the distribution histogram of the height data is calculated and the distribution peak value is extracted.

[0070] In this embodiment, a sliding window statistics thread runs in the background in real time to record the robot's Z-axis (vertical axis) height trajectory within the most recent preset number of frames.

[0071] Optionally, the preset frame number is set to 50, that is, the robot's Z-axis height data of the most recent 50 frames are used to form the Z-axis height trajectory.

[0072] Based on this, histogram statistics are used to analyze the distribution density and peak value of height data in the sliding window in real time.

[0073] In a specific instance, the program executes the `nalyzeHeightDistribution` function to maintain a sliding window `Pose_Window` with a length of 50 frames. The program calls the `ComputeHistogram` subroutine to calculate the histogram of the Z-axis height data distribution within the window and extracts the distribution peak `Z_peak`.

[0074] When the variance of the height data of the sliding window is less than a preset stability threshold, the robot is controlled to switch to cruise mode, and when it first enters a floor, the distribution peak of the sliding window is recorded as the nominal height of that floor.

[0075] When the variance of all height data in the sliding window is less than a preset stable threshold, it indicates that the robot's current height data distribution is concentrated around a specific value. Therefore, the robot is determined to be in a cruising state. In this embodiment, the distribution peak is recorded as the aforementioned specific value, which represents the robot's current height.

[0076] At the same time, the relative difference between the peak value of the sliding window distribution and the initial height is calculated and matched. If no corresponding relative difference data is matched, it indicates that the robot has entered the floor for the first time. At the same time, the peak value of the distribution is recorded as the nominal height of the floor, which represents the accurate height of the floor.

[0077] Optionally, the preset stability threshold is determined based on experience.

[0078] When the height data of the sliding window changes monotonically and the difference between the height data and the nominal floor height of the current floor is greater than a preset height threshold, the robot is controlled to switch to the crossing state.

[0079] When the height data of the sliding window changes monotonically, and the difference between the peak value of the sliding window distribution and the nominal height of the current floor recorded by the system is greater than the preset height threshold, it indicates that this difference must be generated by the robot traversing the floor, rather than within the same floor. Therefore, the robot is switched to traversing mode.

[0080] Understandably, when determining the state of the first crossing, the nominal floor height used is the known initial height, which is determined by the initial floor placed when the robot builds the map. The nominal floor height used thereafter is calculated and maintained by the height state machine.

[0081] Optionally, the preset height threshold is set to 1.5 meters.

[0082] In a specific instance, the program instantiates a FloorStateMachine object, maintains the current state CurrentState, and when in cruise state, calculates in real time the difference between the peak value of the sliding window distribution and the nominal floor height of the current floor. If the difference exceeds the preset height threshold, the program switches the state to TRANSIT and calls SuspendCurrentLayerMapping to pause the mapping of the current floor.

[0083] When in traversal mode, the program calls IsHeightStable to monitor altitude stability. If the altitude is stable and a new numerical range is entered, the program determines that a new floor has been entered, switches the state back to cruise mode, and identifies the floor ID based on the peak values ​​of the sliding window. At this point, the program calls the SwitchMapLayer function to create or load the map container corresponding to the ID.

[0084] This invention achieves automatic floor perception and map switching through a real-time running height state machine, thereby realizing the structured storage of sub-maps for each floor.

[0085] In the multi-layer mapping method for three-dimensional structures based on ground plane association constraints of the present invention, the step of using the nominal floor height of each floor determined by the height state machine as a priori constraint closed-loop detection specifically includes:

[0086] Filter multiple candidate frames that are similar in appearance to the current frame;

[0087] To eliminate accumulated errors after long-distance operation and prevent mismatches across floors, in the closed-loop detection process, optionally, the SearchLoopCandidates function is first called to use point cloud global operators (such as Scan Context) to search for frames with similar appearances in the historical keyframe database, and obtain multiple candidate frames with similar appearances to the current frame.

[0088] Before performing fine registration of loop closure in the same layer, the absolute height difference between the current frame and the candidate frame is calculated. If the absolute height difference is greater than the preset same layer determination threshold, the candidate frame is rejected for loop closure.

[0089] Before performing fine registration using ICP (Iterative Closest Point, 3D point cloud registration), a geometric verification logic is added to perform a geometric verification of floor consistency based on the pre-determined nominal floor height of each floor.

[0090] Specifically, the absolute value of the Z-axis height difference between the nominal floor height of the current frame and the nominal floor height of the candidate frame is calculated. If the difference is greater than the preset same-floor judgment threshold, it means that although the current frame and the candidate frame look similar, they are located on different floors (e.g., the corridor on the third floor and the corridor on the second floor). The closed-loop candidate is directly rejected to fundamentally eliminate the common "vertical collapse" or "floor overlap" errors in multi-story buildings.

[0091] Optionally, the preset threshold for determining the same floor is set to 0.8 meters.

[0092] Otherwise, the incremental optimization algorithm is triggered to distribute the closed-loop error to the complete trajectory.

[0093] Otherwise, if the candidate frame passes the floor consistency geometric check, then a valid loop closure is determined to have occurred.

[0094] Incremental global optimization: When a valid closed loop is detected and verified, the system triggers the iSAM2 incremental optimization algorithm to reasonably distribute the closed loop error across the entire trajectory and correct the drift of the historical trajectory.

[0095] In a specific instance, the program executes the core CheckHeightConsistency check function for each candidate frame. This function calculates the absolute value of the Z-axis height difference, delta_z, between the current frame and the candidate frame.

[0096] The program executes a conditional check: if delta_z > 0.8m, it is determined that although the candidate frames look similar, they are located on different floors, and RejectCandidate is executed directly to reject the loop closure;

[0097] Otherwise, execute AcceptCandidate and proceed with the ICP fine registration process.

[0098] Global optimization logic: When a valid closed loop is detected, the program calls the PerformGlobalOptimization function, which triggers the iSAM2 (Incremental Smoothing and Mapping 2) incremental optimization algorithm to allocate the closed loop error to the historical trajectory and correct the Z-axis drift.

[0099] In the multi-layer mapping method for three-dimensional structures based on ground plane correlation constraints of this invention, the step of suppressing the robot's attitude error on the vertical axis by constructing a laser inertial odometry that incorporates ground plane constraint factors specifically includes:

[0100] When the robot first enters a floor, a local ground plane extraction process is triggered to obtain and record the effective plane of that floor;

[0101] When it is determined that the robot has entered a new floor that has not been recorded, the local ground plane extraction process is triggered. Based on the point cloud data of the trigger frame, the ground point cloud data is extracted and recorded as the effective plane of this floor.

[0102] After the effective plane is output, a ground plane geometric constraint factor is added to the pose node of the current frame. The residual of the ground plane geometric constraint factor is defined as the vertical axis angle error after the local ground plane normal vector of the current frame is transformed to the global reference ground plane. The global reference ground plane is the effective plane of the corresponding floor of the current frame.

[0103] The system maintains the robot's pose state variables, velocity state variables, and IMU bias state variables in the factor graph, including three types of factors:

[0104] IMU pre-integration factor: connects state variables at adjacent time points, constraining the robot's dynamic motion.

[0105] Laser odometry factor: constructed based on point cloud feature matching (edge ​​points and planar points), constraining the relative pose between frames.

[0106] Ground plane geometric constraint factor (core of this invention): After outputting the effective plane of the corresponding floor, the system calls this effective plane data to add a univariate constraint factor to the pose node of the current frame optimized by LIO. The residual function of the ground plane geometric constraint factor is defined as the difference between the "local ground observed in the current frame" and the "global reference ground plane".

[0107] The global reference ground plane is the effective plane of the corresponding floor output.

[0108] Specifically, the system calculates the angle error (attitude residual) between the local ground plane normal vector and the vertical axis (Z-axis) after transformation to the global coordinate system, and the height difference (position residual) between the local ground plane center and the nominal floor height after transformation to the global coordinate system. By minimizing these residuals, the optimizer is forced to adjust the robot's pitch angle, roll angle, and Z-axis height to ensure that it always conforms to the actual physical floor structure.

[0109] In a specific instance, the backend optimization module, based on the GTSAM library, executes the process, initializing a nonlinear factor graph object. For each keyframe, the program sequentially calls AddImuFactor to add pre-integration factors to constrain the dynamic state, and calls AddLidarOdometryFactor to add point-line / point-surface matching factors to constrain the relative pose, thus constructing the factor graph.

[0110] The program executes the AddGroundConstraint function, which, when a valid plane is detected (e.g., the recorded ground plane flag is true), constructs a custom "univariate plane factor" and adds a ground plane geometric constraint factor.

[0111] Calculate the local ground plane normal vector observed in the current frame after pose transformation and its perpendicular vector to the global vector. The deviation (attitude residual) and the deviation between the local ground plane height and the nominal height of the current floor (height residual).

[0112] The program dynamically calculates the noise covariance matrix of the factor based on the root mean square error (RMSE) of the residuals of the plane fitting. The better the fitting quality, the smaller the covariance, and thus it occupies a larger weight in the optimization.

[0113] In the multi-layer mapping method for three-dimensional structures based on ground plane association constraints of the present invention, the step of triggering the local ground plane extraction process to obtain and record the effective plane of the floor specifically includes:

[0114] Acquire the trigger frame point cloud data when the local ground plane extraction process is triggered, and extract the point cloud data within a preset vertical axis range as a candidate ground point set;

[0115] After triggering the local ground plane extraction process, the point cloud data of the trigger frame is extracted. Considering the installation height of the quadruped robot and the physical characteristics of the ground, the system first sets a height filtering interval as the region of interest for dynamic filtering.

[0116] Optionally, using the origin of the lidar coordinate system as a reference, the point cloud with the Z-axis coordinate within a preset vertical axis range is selected as the "candidate ground point set" to eliminate interference from ceilings, walls and low obstacles.

[0117] Optionally, the preset vertical axis range is set to [-0.8m, -0.2m].

[0118] The random sampling consensus algorithm is executed on the candidate ground point set. Three points are selected iteratively to construct a plane model. The distances from the remaining points to the plane are calculated, and the number of interior points is counted.

[0119] After multiple iterations, the model with the most interior points is selected as the optimal fitting plane;

[0120] Furthermore, the Random Sample Consensus (RANSAC) algorithm is applied to the candidate ground point set. The system iteratively selects three points to construct a planar model, calculates the distances from the remaining points to this plane, and counts the number of interior points that meet a distance threshold (e.g., 5cm). After multiple iterations, the model with the most interior points is selected as the optimal fitting plane.

[0121] Perform a normal vector gravity consistency check on the optimal fitting plane, and determine the optimal fitting plane that passes the check as the valid plane of the current floor, and mark the optimal fitting plane that fails the check as an invalid frame.

[0122] To prevent the algorithm from mistakenly identifying slopes, staircases, or collapsed walls as "ground planes," the system executes gravity verification logic: calculating the angle between the normal vector of the fitted plane and the gravity direction measured by the IMU. If this angle is less than a preset threshold (15 degrees in this embodiment), the plane is determined to be a valid horizontal ground, and its plane parameters (normal vector and distance) are output; otherwise, it is determined that no reliable ground has been detected in the current frame, and it is marked as an invalid frame, not participating in subsequent ground plane constraints.

[0123] In a specific example, the ExtractROI function is first called. Considering the physical mounting height of the quadruped robot, this function is configured with a pass-through filter, retaining only point cloud data with Z-axis coordinates in the range of [-0.7m, -0.3m]. The filtered point cloud is labeled as Candidate_Ground_Points, thus eliminating interference from ceilings, walls, and low obstacles.

[0124] The program then calls the RANSAC_PlaneFitting function. This function uses a random sample consensus algorithm, setting the maximum number of iterations to 100 and the distance threshold to 0.05 meters. In each iteration, three points are randomly selected to construct the plane equation Ax + By + Cz + D = 0, and the number of interior points is counted. After the iteration is complete, the program outputs the optimal plane model parameters with the highest number of interior points.

[0125] To prevent erroneous extraction, the program executes the ValidatePlane function. This function calculates the normal vector of the fitted plane. n relative to IMU gravity direction g The included angle θ .

[0126] like If the condition is met, the plane is determined to be a valid plane, and the flag s_ground_valid=true is set; otherwise, it is determined to be a slope or an invalid area, the flag is set to false, it does not participate in subsequent optimization, and the local ground plane extraction process is restarted until the valid plane of the current floor is extracted.

[0127] The multi-layer mapping method for three-dimensional structures based on ground plane correlation constraints of the present invention also includes:

[0128] Point cloud data is preprocessed using monitoring data from inertial sensors mounted on the robot to eliminate motion distortion.

[0129] In this embodiment, the system is configured with an FPGA as the main control clock source, sending a trigger signal to the LiDAR at a frequency of 10Hz, while continuously acquiring IMU data at a frequency of 200Hz. For non-trigger IMU data, the system maintains a timestamp index queue and uses a linear interpolation algorithm to align the timeline of the IMU data to the acquisition time of each frame of point cloud from the LiDAR (i.e., the frame header time).

[0130] To address the severe shaking caused by the quadruped robot when climbing stairs or running, the system executes the following distortion correction logic: First, the IMU pre-integration algorithm is used to calculate the relative pose change trajectory from the start time t_start to the end time t_end within one frame scan cycle (usually 100ms) of the lidar.

[0131] Then, for each laser point in the original point cloud, its time offset relative to the frame header is calculated based on its horizontal rotation angle.

[0132] Finally, the time offset is used to interpolate on the relative pose trajectory to query the instantaneous pose of the point at the moment of acquisition, and the coordinate transformation formula is used to project the point onto the coordinate system at the frame header time, thereby eliminating the point cloud "tailing" and "layering" phenomena caused by the robot's own motion.

[0133] The above method enables real-time preprocessing of point cloud data and eliminates motion distortion.

[0134] In a specific example, this is performed through a preprocessing module, which mainly includes two core processing logics: time soft synchronization and motion distortion removal.

[0135] Time synchronization logic: The system defines and executes a processing function named SyncSensorData. This function maintains two circular buffers: LidarBuffer and ImuBuffer. When a new frame of data arrives from the LiDAR, the program first reads its frame header timestamp t_head and frame tail timestamp t_tail.

[0136] Next, the program calls the FindImuRange subroutine to search the ImuBuffer for all IMU data packets covering the time range [t_head-ε, t_tail+ε]. If a time gap is detected in the IMU data, the program executes the LinearInterpolateIMU logic to use linear interpolation to fill in the missing time points, ensuring that each laser point can find an inertial reference for the corresponding time.

[0137] Distortion correction logic: The system executes a distortion correction function named UndistortPointCloud. This function first calls the IMU_Preintegration module to calculate the robot's relative pose change trajectory from t_head to t_tail. Then, the program iterates through each point in the original point cloud. The program calculates the time ratio 's' of that point relative to the frame header. The program then calls the GetInterpolatedPose function to obtain the instantaneous pose of that point at the moment of acquisition. and perform inverse coordinate transformation The point is then uniformly transformed to the frame header coordinate system, thereby outputting a clean point cloud after removing motion distortion.

[0138] This invention introduces ground plane geometric prior constraints and height probability distribution statistical mechanisms to achieve precise vertical orientation determination and structured management of multi-level maps, providing a high-precision and robust environmental model for autonomous navigation in complex three-dimensional environments.

[0139] The following describes the multi-layer mapping system for three-dimensional structures based on ground plane association constraints provided by the present invention. The multi-layer mapping system for three-dimensional structures based on ground plane association constraints described below can be referred to in correspondence with the multi-layer mapping method for three-dimensional structures based on ground plane association constraints described above.

[0140] like Figure 2As shown, the three-dimensional multi-layer mapping system based on ground plane association constraints provided by the present invention includes a state switching module 201 and a mapping module 202.

[0141] The state switching module 201 is used to maintain the height state machine in real time. The height state machine is used to control the robot to switch between a cruise state representing cruising on the same floor and a crossing state representing a change in height.

[0142] During the cruise phase, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height.

[0143] During the traversal state, the robot pauses updating the floor sub-map and maintains the channel map that protects the topological connections.

[0144] In this embodiment, the method is applied to a quadrupedal wheeled robot equipped with sensors including a Velodyne VLP-16 LiDAR (mounted at a height of 0.5 meters) for acquiring environmental point cloud data, a RealSense depth camera, and an Xsens MTi-630 IMU. The system is developed based on the ROS2 architecture, with the core algorithm module written in C++. The PCL library is used for point cloud processing, and the GTSAM library is used for factor graph optimization.

[0145] The LiDAR mounted on the robot collects point cloud data in real time to build a map of the three-dimensional structure, while the IMU inertial sensor is used to correct the robot's walking posture and the acquired point cloud data.

[0146] To avoid map layering errors caused by the cumulative drift of vertical axis (Z-axis) height during the robot's mapping of 3D structures, the system maintains a height state machine in the background in real time to control the robot's cruise / traverse state.

[0147] The cruise state indicates that the robot moves within the same floor at a relatively fixed height. In the cruise state, the robot associates the acquired environmental point cloud data with the floor sub-map corresponding to the current height, thereby constructing and / or updating the floor sub-map.

[0148] The "crossing state" indicates that the robot is undergoing a height change that affects floor determination, that is, the robot is in the process of going up or down the stairs. When in the crossing state, the system stops updating the current floor sub-map and instead generates a channel map that mainly contains topological connections, thereby ensuring that the planar map of each floor sub-map is flat and clear and will not be destroyed by the slope point cloud of the stairs.

[0149] Understandably, in this embodiment, the maps of each floor of the three-dimensional structure are constructed, stored, and optimized in layers. The robot's initial floor and initial height are known. After the robot switches from the traversal state to the cruise state, it indicates that the robot has entered a new floor. At this time, the floor matching process is triggered, the relative difference between the current height and the initial height is calculated, and a match is made. If no match is found for the corresponding relative difference, it means that the robot is currently on a completely new floor that it has not been on before. An index is created for this floor and the relative difference is recorded for subsequent matching. If the calculated relative difference can match the corresponding floor, the floor sub-map file of this floor is determined through the floor index so that the point cloud data obtained during the cruise on this floor can be used for loop closure optimization.

[0150] Optionally, the height state machine is implemented by the semantic state machine module, which is at least responsible for map switching of floor sub-maps.

[0151] Optionally, the robot's height data for a recent period of time can be acquired in real time, and the robot can be switched between cruising and traversing states based on changes in its height.

[0152] Furthermore, in order to suppress Z-axis offset during the mapping process, the system constructs a nonlinear optimization model based on factor graphs. The factor graph, which integrates ground plane constraint factors, is used as the optimization engine for Lidar-Inertial Odometry (LIO) to force the robot to adjust its pitch angle, roll angle, and Z-axis height. This results in a higher-precision, lower-drift LIO system, and finally, the model is used in cruise mode to obtain a model that always fits the real physical floor structure.

[0153] Among them, the ground plane constraint factor is a univariate constraint factor based on the ground plane point cloud data of the current floor. The pre-determined ground plane point cloud data of the current floor is used as the global ground plane. It is compared with the local ground plane of the key frame point cloud data acquired by the robot in the cruise state. The Z-axis angle error between the two planes in the same coordinate system is calculated, which is the attitude residual. Optimization is achieved by minimizing the residual.

[0154] Furthermore, in order to suppress the cumulative error of the Z-axis, before performing point cloud registration and fusion during the loop closure process, the nominal floor height of each floor maintained by the height state machine during the initial mapping process is first used. Point cloud data registration and fusion is only performed when it is determined that the robot's current height corresponds to the same floor as the nominal floor height of the matched floor. This is to fundamentally eliminate the "vertical collapse" or "floor overlap" errors commonly found in multi-layer three-dimensional structures.

[0155] The mapping module 202 is used to generate and output layered map data for each floor. The layered map data includes floor index files, layered point cloud files, and topology connection files.

[0156] In the cruise state, a laser inertial odometry system incorporating ground plane constraint factors is used to suppress the robot's attitude error on the vertical axis, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint for closed-loop detection.

[0157] The system controls the robot to move within a three-dimensional structure and constructs layered map data for each floor based on the environmental point cloud data acquired by the robot. It can be understood that, through the above method, the system ultimately generates and outputs not a single large file, but a structured dataset, including floor index files, layered point cloud files, and topology connection files.

[0158] The floor index file records the number of floors (represented as floor IDs), the nominal height of each floor, and the path to the point cloud file corresponding to each floor.

[0159] The layered point cloud file stores an independent point cloud map (i.e., a floor sub-map) for each floor, and each file contains only the static structure of the corresponding floor.

[0160] The topology connection file describes the entrance and exit coordinates of a staircase or elevator shaft, which is used to guide downstream navigation calculations.

[0161] This invention introduces a height state machine and a layered map construction mechanism, combined with a laser inertial odometry that incorporates ground plane constraint factors, to achieve vertical drift suppression and map structure optimization for robot mapping in multi-story environments. This ensures the planar independence and clarity of each floor's sub-map and avoids interference from dynamic structures such as stairs.

[0162] Figure 3 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 3As shown, the electronic device may include: a processor 310, a communication interface 320, a memory 330, and a communication bus 340, wherein the processor 310, the communication interface 320, and the memory 330 communicate with each other through the communication bus 340. The processor 310 can call logic instructions in the memory 330 to execute a multi-layer mapping method for three-dimensional structures based on ground plane association constraints. This method includes: real-time maintenance of a height state machine, which controls the robot to switch between a cruise state representing cruising on the same floor and a traversal state representing a change in height; in the cruise state, the robot acquires environmental point cloud data in real time and associates it with a floor sub-map corresponding to the current height; in the traversal state, the robot pauses updating the floor sub-map and maintains a channel map containing topological connections; wherein, in the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that incorporates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint for loop closure detection; and the layered map data for each floor is generated and output, the layered map data including a floor index file, a layered point cloud file, and a topological connection file.

[0163] Furthermore, the logical instructions in the aforementioned memory 330 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several 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 methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0164] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the multi-layer mapping method for three-dimensional structures based on ground plane association constraints provided by the above methods. The method includes: maintaining a height state machine in real time, which is used to control the robot to switch between a cruise state representing cruising on the same floor and a traversal state representing a change in height; in the cruise state, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height; in the traversal state, the robot pauses updating the floor sub-map and maintains a channel map containing topological connections; wherein, in the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that integrates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection; generating and outputting layered map data for each floor, which includes a floor index file, a layered point cloud file, and a topological connection file.

[0165] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements the multi-layer mapping method for three-dimensional structures based on ground plane association constraints provided by the above methods. The method includes: maintaining a height state machine in real time, the height state machine being used to control the robot to switch between a cruise state representing cruising on the same floor and a traversal state representing a change in height; in the cruise state, the robot acquires environmental point cloud data in real time and associates it with a floor sub-map corresponding to the current height; in the traversal state, the robot pauses updating the floor sub-map and maintains a channel map containing topological connections; wherein, in the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that incorporates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection; generating and outputting layered map data for each floor, the layered map data including a floor index file, a layered point cloud file, and a topological connection file.

[0166] The device embodiments described above are merely illustrative. 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 modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0167] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0168] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for stereo structure multi-layer mapping based on ground plane association constraints, characterized in that, include: A real-time maintenance height state machine is used to control the robot to switch between a cruise state representing cruising on the same floor and a traversal state representing a change in height. During the cruise phase, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height. During the traversal state, the robot pauses updating the floor sub-map and maintains a channel map containing topological connections. In the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that integrates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection. Generate and output layered map data for each floor, the layered map data including floor index file, layered point cloud file and topology connection file; The steps for maintaining the altitude state machine in real time specifically include: A sliding window is run in real time to record the robot's vertical axis height data for the most recent preset number of frames, and the distribution histogram of the height data is calculated and the distribution peak value is extracted. The robot's vertical axis height data is obtained through a laser inertial odometry. When the variance of the height data of the sliding window is less than a preset stability threshold, the robot is controlled to switch to cruise mode, and when it enters a floor for the first time, the distribution peak of the sliding window is recorded as the nominal floor height of that floor. When the height data of the sliding window changes monotonically and the difference between the height data and the nominal floor height of the current floor is greater than a preset height threshold, the robot is controlled to switch to the crossing state. The step of using the nominal floor height of each floor determined by the height state machine as a priori constraint for closed-loop detection specifically includes: Filter multiple candidate frames that are similar in appearance to the current frame; Before performing fine registration of same-floor loop closure, calculate the absolute height difference between the nominal floor height corresponding to the current frame and the nominal floor height corresponding to the candidate frame. If the absolute height difference is greater than the preset same-floor judgment threshold, reject the candidate frame for loop closure. Otherwise, the incremental optimization algorithm is triggered to distribute the closed-loop error to the complete trajectory.

2. The method of claim 1, wherein, The step of suppressing the robot's attitude error on the vertical axis by constructing a laser inertial odometry that incorporates ground plane constraint factors specifically includes: When the robot first enters a floor, a local ground plane extraction process is triggered to obtain and record the effective plane of that floor; After the effective plane is output, a ground plane geometric constraint factor is added to the pose node of the current frame. The residual of the ground plane geometric constraint factor is defined as the vertical axis angle error after the local ground plane normal vector of the current frame is transformed to the global reference ground plane. The global reference ground plane is the effective plane of the corresponding floor of the current frame.

3. The method for multi-layer mapping of three-dimensional structures based on ground plane correlation constraints according to claim 2, characterized in that, The step of triggering the local ground plane extraction process to obtain and record the effective plane of the floor specifically includes: Acquire the trigger frame point cloud data when the local ground plane extraction process is triggered, and extract the point cloud data within a preset vertical axis range as a candidate ground point set; The random sampling consensus algorithm is executed on the candidate ground point set. Three points are selected iteratively to construct a plane model. The distances from the remaining points to the plane are calculated, and the number of interior points is counted. After multiple iterations, the model with the most interior points is selected as the optimal fitting plane; Perform a normal vector gravity consistency check on the optimal fitting plane, and determine the optimal fitting plane that passes the check as the valid plane of the current floor, and mark the optimal fitting plane that fails the check as an invalid frame.

4. The method for multi-layer mapping of three-dimensional structures based on ground plane association constraints according to claim 1, characterized in that, Also includes: Point cloud data is preprocessed using monitoring data from inertial sensors mounted on the robot to eliminate motion distortion.

5. A multi-layer mapping system for three-dimensional structures based on ground plane correlation constraints, characterized in that, include: The state switching module is used to maintain the height state machine in real time. The height state machine is used to control the robot to switch between the cruise state, which represents cruising on the same floor, and the traversal state, which represents a change in height. During the cruise phase, the robot acquires environmental point cloud data in real time and associates it with the floor sub-map corresponding to the current height. During the traversal state, the robot pauses updating the floor sub-map and maintains the channel map that protects the topological connections. The mapping module is used to generate and output layered map data for each floor. The layered map data includes floor index files, layered point cloud files, and topology connection files. In the cruise state, the robot's attitude error on the vertical axis is suppressed by constructing a laser inertial odometry that integrates ground plane constraint factors, and the nominal floor height of each floor determined by the height state machine is used as a priori constraint closed-loop detection. Specifically, the state switching module is used for: A sliding window is run in real time to record the robot's vertical axis height data for the most recent preset number of frames, and the distribution histogram of the height data is calculated and the distribution peak value is extracted. The robot's vertical axis height data is obtained through a laser inertial odometry. When the variance of the height data of the sliding window is less than a preset stability threshold, the robot is controlled to switch to cruise mode, and when it enters a floor for the first time, the distribution peak of the sliding window is recorded as the nominal floor height of that floor. When the height data of the sliding window changes monotonically and the difference between the height data and the nominal floor height of the current floor is greater than a preset height threshold, the robot is controlled to switch to the crossing state. The step of using the nominal floor height of each floor determined by the height state machine as a priori constraint for closed-loop detection specifically includes: Filter multiple candidate frames that are similar in appearance to the current frame; Before performing fine registration of same-floor loop closure, calculate the absolute height difference between the nominal floor height corresponding to the current frame and the nominal floor height corresponding to the candidate frame. If the absolute height difference is greater than the preset same-floor judgment threshold, reject the candidate frame for loop closure. Otherwise, the incremental optimization algorithm is triggered to distribute the closed-loop error to the complete trajectory.

6. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the multi-layer mapping method for three-dimensional structures based on ground plane association constraints as described in any one of claims 1 to 4.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the multi-layer mapping method for three-dimensional structures based on ground plane association constraints as described in any one of claims 1 to 4.

8. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the multi-layer mapping method for three-dimensional structures based on ground plane association constraints as described in any one of claims 1 to 4.

Images