A robot environment perception method based on multi-sensor fusion

Abstract

The application provides a robot environment perception method based on multi-sensor fusion, comprising the following steps: performing space-time synchronization processing on original multi-sensor data to generate synchronized perception data; judging whether there is a semantic landmark matching the original or synchronized data in a topological map, wherein the matching condition is that the geometric primitive of the semantic landmark in the topological map and the same geometric primitive existing in the data are matched; when it is confirmed that there is a matching landmark, selecting the matching landmarks to form a set; based on the set, performing pose correction on the original multi-sensor data, fusing different pose constraints in the matching landmarks into the original data, and obtaining corrected data for robot path planning. The application can improve the consistency of multi-sensor data and the accuracy of robot environment perception, and provides reliable correction data for path planning.

Description

Technical Field

[0001] This invention relates to the field of robot environmental perception and autonomous navigation technology, and more specifically, to a robot environmental perception method based on multi-sensor fusion. Background Technology

[0002] With the rapid development of robotics technology, robots are increasingly being used in industrial manufacturing, logistics and warehousing, home services, and autonomous driving. For robots to achieve autonomous navigation and path planning in complex environments, high-precision perception and localization of their surroundings are essential. Currently, robot environmental perception primarily relies on multi-sensor fusion technology, which integrates data from various sensors such as LiDAR, vision cameras, and inertial measurement units (IMUs) to obtain more comprehensive and robust environmental information. LiDAR provides high-precision three-dimensional spatial structure information, vision cameras capture rich texture and semantic features, and IMUs provide high-frequency motion state estimation. To effectively fuse this heterogeneous sensor data, researchers typically employ spatiotemporal synchronization methods to align the data from different modalities to a unified time reference and construct environmental maps based on feature extraction and matching techniques.

[0003] The existing technology has at least the following problems: it lacks a semantic landmark matching mechanism based on the layer-by-layer comparison of geometric primitives, resulting in insufficient matching accuracy between the original multi-sensor data and the topology map; the pose correction process does not comprehensively consider semantic category, number of observations and key frame conditions, making it difficult to adapt to the multi-source data fusion requirements in complex dynamic environments; and it lacks effective means of processing dynamic obstacle areas and updating the map when there are no matching semantic landmarks in the map. Summary of the Invention

[0004] This invention provides a robot environment perception method based on multi-sensor fusion.

[0005] include:

[0006] Spatiotemporal synchronization processing is performed on raw multi-sensor data to generate synchronized sensing data;

[0007] Determine whether there are semantic landmarks in the topology map stored in the topology map server that match the original multi-sensor data or the synchronous sensing data. The matching semantic landmark is a semantic landmark in the semantic landmark stored in the topology map that corresponds to the same geometric primitive in the original multi-sensor data or the synchronous sensing data.

[0008] When there are semantic landmarks in the topology map that match the original multi-sensor data or the synchronous sensing data, select the semantic landmarks that match the original multi-sensor data or the synchronous sensing data from the topology map as matching landmarks to obtain a set of matching landmarks;

[0009] Based on the set of matching landmarks, pose correction processing is performed on the original multi-sensor data to obtain multiple corrected original multi-sensor data sets. Further, the pose correction processing of the original multi-sensor data based on the set of matching landmarks includes: for each preset condition in a preset condition group, in response to determining that the original multi-sensor data or the synchronous sensing data satisfies the preset condition, pose correction processing is performed on the original multi-sensor data according to the semantic category and observation frequency of each matching landmark included in the topology map.

[0010] Further, the pose correction processing of the original multi-sensor data based on the matching landmark set includes: in response to the original multi-sensor data or the synchronous sensing data satisfying the keyframe condition, determining the joint covariance matrix of each matching landmark in the matching landmark set based on the original multi-sensor data to obtain a covariance matrix set; and performing pose correction processing on the original multi-sensor data according to the covariance matrix set and the matching landmark set.

[0011] Furthermore, the method also includes: when there are no semantic landmarks in the topology map that match the original multi-sensor data or the synchronous sensing data, displaying a grid map interface for the user to set a dynamic obstacle area; and binarizing the dynamic obstacle area set by the user and inputting it into the topology map.

[0012] Furthermore, the method further includes: inputting the corrected original multi-sensor data into a local factor graph optimizer, and performing the following steps: extracting edge features and planar features from the corrected lidar point cloud frame; extracting ORB feature points from the corrected image frame; jointly initializing the inertial measurement unit pre-integration result with the lidar edge features, planar features, and image ORB feature points to construct a local sliding window; performing local bundle adjustment based on the co-view relationship of multiple keyframes in the local sliding window to update the robot pose, and outputting the optimized pose sequence for selection by the robot path planning module.

[0013] Further, the topology map is obtained through the following steps: collecting multiple segments of continuous sensing data from multiple robot runs, each segment including LiDAR scan sequences, image sequences, and inertial measurement unit data, to obtain an original sensing dataset; performing offline joint calibration on each segment of continuous sensing data in the original sensing dataset to generate a calibration parameter set; projecting each modal data in each segment of continuous sensing data onto a unified timestamp according to the calibration parameter set to obtain a synchronous sensing dataset; extracting three-dimensional geometric primitives and semantic labels from each frame of data in the synchronous sensing dataset to obtain a semantic feature set; and constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map.

[0014] Furthermore, the step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map includes: initializing an empty pose graph; adding each semantic feature in the semantic feature set and its corresponding pose node to the empty pose graph to form a node set; and adding edge constraints to the relative pose measurement between every two pose nodes in the node set to generate an initial pose graph.

[0015] Furthermore, the step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map further includes: for each semantic feature in the semantic feature set, clustering semantic labels belonging to the same semantic category to generate landmark clusters, obtaining a landmark cluster set; calculating the centroid position and orientation of each landmark cluster in the landmark cluster set to obtain a centroid-orientation pair set; and inputting the constraints between each centroid-orientation pair in the centroid-orientation pair set and the corresponding pose node into the initial pose graph to update the initial pose graph.

[0016] Furthermore, the step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map further includes: performing graph optimization iterative solution on each frame of data in the synchronous perception dataset to generate an optimized pose sequence; performing principal component analysis on each landmark cluster in the landmark cluster set to generate a principal direction vector, obtaining a principal direction vector set; determining the cumulative error gradient of each pose node in the node set to obtain a gradient set; and for every two principal direction vectors in the principal direction vector set, in response to determining that the dot product of the two principal direction vectors is greater than or equal to a similarity threshold, merging the two landmark clusters corresponding to the two principal direction vectors to update the semantic feature set.

[0017] Furthermore, the step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map further includes: rigidly registering each pose node in the optimized pose sequence with each semantic feature in the merged semantic feature set and inputting it into the updated pose graph; and labeling each pose node and semantic feature included in the updated pose graph based on a preset semantic category group to generate a topology map.

[0018] The embodiments of the present invention have at least the following beneficial effects:

[0019] 1. This invention uses a geometric primitive layer-by-layer comparison mechanism to determine whether there are semantic landmarks in the topology map that match the original multi-sensor data or synchronous perception data, and selects matching landmarks to form a set, enabling the robot to accurately identify known semantic structures in complex environments, thus solving the problem of insufficient matching accuracy mentioned above.

[0020] 2. This invention performs pose correction processing on the original multi-sensor data based on a set of matched landmarks. By fusing pose constraints in the matched landmarks that differ from those in the original data into the original data, and combining semantic categories, observation times, and joint covariance matrices under keyframe conditions in a preset set of conditions for multi-dimensional correction, this invention overcomes the shortcomings of existing technologies that do not fully consider multi-source condition constraints in pose correction. It achieves accurate compensation for drift errors in the original multi-sensor data and improves the accuracy and robustness of robot pose estimation in dynamic and complex environments.

[0021] 3. When no matching semantic landmarks exist in the topology map, this invention displays a raster map interface for users to set dynamic obstacle areas, which are then binarized and input into the topology map. Simultaneously, the corrected data is input into a local factor graph optimizer, which integrates lidar edge features, planar features, image ORB feature points, and inertial measurement unit pre-integration results for joint initialization and local bundle adjustment optimization. This solves the problems of map update lag and insufficient joint optimization of multimodal features in the prior art under dynamic environments, and enhances the system's adaptability to unknown dynamic environments. Attached Figure Description

[0022] The above and other objects, features, and advantages of exemplary embodiments of the present invention will become readily apparent from the following detailed description taken in conjunction with the accompanying drawings. Several embodiments of the invention are illustrated in the drawings by way of example and not limitation, wherein: Figure 1 This is a flowchart illustrating a robot environment perception method based on multi-sensor fusion, as provided in an embodiment of the present invention. Detailed Implementation

[0023] The principles and spirit of the invention will now be described with reference to several exemplary embodiments. It should be understood that these embodiments are provided merely to enable those skilled in the art to better understand and implement the invention, and are not intended to limit the scope of the invention in any way. Rather, these embodiments are provided to make the invention more thorough and complete, and to fully convey the scope of the invention to those skilled in the art.

[0024] Those skilled in the art will recognize that embodiments of the present invention can be implemented as a system, apparatus, device, method, or computer program product. Therefore, the present invention can be specifically implemented in the following forms: entirely hardware, entirely software (including firmware, resident software, microcode, etc.), or a combination of hardware and software.

[0025] It should be noted that the number of any elements in the accompanying drawings is for illustrative purposes only and not as a limitation, and any naming is for distinction only and has no limiting meaning.

[0026] The following is for reference. Figure 1 , Figure 1 This is a flowchart illustrating a robot environment perception method based on multi-sensor fusion according to an embodiment of the present invention. Figure 1 As shown, a robot environment perception method based on multi-sensor fusion includes:

[0027] S1: Perform spatiotemporal synchronization processing on the raw multi-sensor data to generate synchronized sensing data.

[0028] In the scenario of environmental perception for warehouse mobile robots, the original multi-sensor data includes 3D point cloud data collected by LiDAR, image data collected by monocular or binocular cameras, and acceleration and angular velocity data collected by inertial measurement units.

[0029] Spatiotemporal synchronization processing comprises two parts: time synchronization and spatial synchronization. Time synchronization employs the message_filter time aligner based on the ROS robot operating system to unify the timestamps of different sensors to the clock source of the inertial measurement unit, with a threshold set to 10 milliseconds.

[0030] Spatial synchronization employs a known sensor extrinsic parameter calibration matrix to project the LiDAR point cloud onto the camera coordinate system, and then transforms the data from the camera coordinate system to the robot's body coordinate system. In practice, for each point of the LiDAR... Through the extrinsic matrix Projecting it onto the camera coordinate system yields Then through Projected onto the robot's body coordinate system. After time synchronization is complete, at each time t, a set of synchronized sensing data is obtained, containing aligned point cloud frames, image frames, and inertial measurement unit data.

[0031] S2: Determine whether there are semantic landmarks in the topology map stored in the topology map server that match the original multi-sensor data or synchronous sensing data. The topology map stores multiple semantic landmarks. The matching semantic landmark is the semantic landmark in the topology map whose geometric primitives are the same as those in the original multi-sensor data or synchronous sensing data.

[0032] The topology map server is deployed on the robot's local industrial control computer or in the cloud. The topology map stored therein consists of a series of semantic landmarks. Each semantic landmark contains multiple geometric primitives, which can be line segments, arcs, planar patches, or cube corners, such as a planar primitive for a wall or a cylindrical primitive for a column.

[0033] When determining whether a matching semantic landmark exists, geometric primitives are first extracted from the synchronous sensing data: point cloud data is fitted to planes and lines using a random sampling consensus algorithm, such as the RANSAC algorithm, implemented using the pcl::SACSegmentation class in the PCL library. Image data is processed through a deep learning semantic segmentation network, such as Mask R-CNN implemented based on TensorFlow or PyTorch, to obtain pixel-level semantic labels, which are then back-projected into three-dimensional space to form geometric primitives.

[0034] The extracted geometric primitives are compared element-by-element with the geometric primitives of each semantic landmark in the topology map. The comparison uses a hash indexing method, pre-calculating a shape signature for each geometric primitive, such as the hash value of the plane normal vector and distance. If the shape signature of a geometric primitive in the synchronous sensing data is the same as the shape signature of any geometric primitive of a semantic landmark in the map (i.e., the hash values ​​are equal), then the semantic landmark is considered a match. For example, if a plane primitive with a normal vector of 0,0,1 and a height of 1.2 meters above the ground is extracted from the synchronous sensing data, and a semantic landmark in the topology map contains this plane primitive, then a match is determined.

[0035] S3: When it is confirmed through layer-by-layer comparison of geometric primitives that there are semantic landmarks in the topology map that match the original multi-sensor data or synchronous sensing data, select the semantic landmarks that match the original multi-sensor data or synchronous sensing data from the topology map as matching landmarks to obtain a set of matching landmarks.

[0036] The geometric primitives are compared layer by layer. First, the primary geometric primitives (such as the direction and length of a line segment) are compared. If they match, the composite geometric primitives (such as a rectangle formed by line segments) are compared.

[0037] A semantic landmark is selected as a matching landmark when at least 80% of its geometric primitives can be found in the synchronous sensing data. All matching landmarks constitute the matching landmark set.

[0038] For example, when a robot walks through a corridor, the synchronous perception data extracts two planar primitives from the left and right walls and one planar primitive from the ceiling. If the semantic landmark with index L5 in the topology map contains all three primitives, then L5 is selected into the matching landmark set. If multiple landmarks are matched simultaneously, such as L5 and L8 at an intersection, then both landmarks are included in the set.

[0039] S4: Based on the matching landmark set, the original multi-sensor data is subjected to pose correction processing to obtain multiple corrected original multi-sensor data for use by the robot path planning module. The pose correction processing is to fuse the pose constraints in the matching landmarks that are different from those in the original multi-sensor data into the original multi-sensor data.

[0040] The pose correction process employs an extended Kalman filter framework. The raw multi-sensor data includes the robot's current pose estimate: ,

[0041] in, and Two-dimensional plane coordinates; The orientation angle. Each matched landmark provides a set of pose constraints, i.e., the fixed pose of the landmark in the global coordinate system. superscript This represents the vector transpose. The observed pose in the raw multi-sensor data. and The residuals between the observations are used as the observation update. Specifically, for each landmark in the matching landmark set, the residuals are calculated as follows: ,

[0042] in, The composite operation is performed after inverting the pose, where, The operation is defined as: Let , and Both are 3×3 rotation matrices, representing the rotation part of the global pose of the landmark and the rotation part of the observation pose, respectively; and Both are 3×1 translation vectors, representing the translation part of the global pose of the landmark and the translation part of the observed pose, respectively.

[0043] but That is, first calculate the relative rotation matrix. Then calculate the relative translation vector. .

[0044] It contains Euler angles of relative rotation and translation vectors, The pseudo-measured values ​​are input to the extended Kalman filter to update the state vector x. The corrected original multi-sensor data refers to the updated pose estimate. The system generates new observation data by applying the pose transformation to the original point cloud and image. Multiple corrected data sets form multiple hypotheses, such as by using different landmark combinations, selecting two landmarks from the set each time or different filter parameters, with the process noise covariance Q ranging from 0.01I to 0.1I, producing multiple correction results for the path planning module to select the optimal one based on the subsequent matching cost.

[0045] S41: For each preset condition in the preset condition group, in response to determining that the original multi-sensor data or synchronous sensing data meets the preset condition, the original multi-sensor data is subjected to pose correction processing according to the semantic category and observation number of each matching landmark included in the topology map.

[0046] The preset condition group contains three conditions:

[0047] Condition one is that the robot's current speed is greater than 0.5 meters per second;

[0048] Condition 2 is that the integral of the angular velocity of the inertial measurement unit exceeds 30 degrees;

[0049] Condition 3 is that the number of effective feature points in the lidar point cloud is less than 100.

[0050] The data enters this branch when either the raw multi-sensor data or the synchronous sensing data meets any of the conditions. Semantic categories include, for example, doors, pillars, shelves, and corners. The number of observations refers to the number of times the landmark has been matched in history.

[0051] During pose correction, different weights are assigned to landmarks of different semantic categories: doors and corners have a higher weight of 1.0, pillars have a weight of 0.8, and shelves have a weight of 0.6. The more observations, the higher the weight. The formula for calculating the weight w is: ,

[0052] in, For the first Number of observations for each landmark This represents the maximum number of observations in the current set of matched landmarks. The weighted residuals are then used for the extended Kalman filter update.

[0053] For example, if a robot quickly passes by a row of shelves at a speed of 1.2 meters per second, triggering condition one, it will match three shelf landmarks with observation counts of 5, 8, and 10 respectively. The weights will be 0.8 + 0.25 / 10 = 0.9, 0.8 + 0.28 / 10 = 0.96, and 0.8 + 0.210 / 10 = 1.0 respectively. The pose will be updated after weighted averaging.

[0054] S42: In response to the original multi-sensor data or synchronous sensing data satisfying the keyframe condition, based on the original multi-sensor data, determine the joint covariance matrix of each matching landmark in the matching landmark set to obtain a covariance matrix set; perform pose correction processing on the original multi-sensor data according to the covariance matrix set and the matching landmark set.

[0055] Keyframe conditions are defined as follows: the robot has moved a distance exceeding 0.2 meters, rotated an angle exceeding 10 degrees, or the time interval exceeding 0.5 seconds since the last keyframe. When this condition is met, the current frame is marked as a keyframe. The joint covariance matrix represents the uncertainty correlation among the various landmarks in the matching landmark set. Let the matching landmark set contain n landmarks, and the pose covariance matrix of each landmark be... Landmark With landmarks The cross-variance between them is .

[0056] Joint covariance matrix It is a 3n×3n block matrix, the first... block is , No. block is Off-diagonal blocks are calculated through relative measurements between landmarks. This yields a set of covariance matrices, which is calculated for each keyframe. For pose correction, weighted least squares optimization is used, and the objective function is shown in the following equation:

[0057] in, For the first The pose residual vector of each landmark; Let be the relative pose residual between landmarks i and j. Solve this optimization to obtain the optimal pose correction. Typical values ​​are: the diagonal elements of the covariance matrix are initially set to a position variance of 0.01 square meters and an angle variance of 1 degree squared, which is approximately 0.0003 radians squared; the cross-covariance is recursively calculated through the robot motion model and predicted using an extended Kalman filter.

[0058] S5: When it is confirmed through layer-by-layer comparison of geometric primitives that there are no semantic landmarks in the topology map that match the original multi-sensor data or synchronous sensing data, the grid map interface is displayed for the user to set dynamic obstacle areas; the dynamic obstacle areas set by the user are binarized and input into the topology map.

[0059] After the geometric primitives are compared layer by layer, if each geometric primitive in the synchronous sensing data cannot match any geometric primitive of a semantic landmark in the topology map, it is determined that no matching landmark exists. At this time, a grid map window pops up on the robot's host computer display interface. The grid map is represented by occupied grids, each grid being 0.05 meters × 0.05 meters in size.

[0060] During implementation, users select rectangular or polygonal areas on the raster map using a mouse or touchscreen, marking them as dynamic obstacle areas, such as temporarily stacked boxes of goods or forklifts. Binarization processes assign a value of 1 to the raster values ​​within the marked areas to represent obstacles, and a value of 0 to unmarked areas to represent free space. The processed binarized raster map is then input into the topology map as new semantic landmarks. Specifically, the center point set of all raster cells within the area is extracted as point cloud geometric primitives, assigned the semantic category of "dynamic obstacle," initialized to 1 observation count, and then stored on the topology map server.

[0061] S6: Input the corrected original multi-sensor data into the local factor graph optimizer and perform the following steps: extract edge features and planar features from the corrected LiDAR point cloud frame; extract ORB feature points from the corrected image frame; jointly initialize the inertial measurement unit pre-integration results with the LiDAR edge features, planar features, and image ORB feature points to construct a local sliding window; based on the co-view relationship of multiple keyframes in the local sliding window, perform local bundle adjustment to update the robot pose, and output the optimized pose sequence for the robot path planning module to select.

[0062] The local factor graph optimizer uses the open-source factor graph optimization library GTSAM (Georgia Tech Smoothing and Mapping). In practice, it first extracts edge features (i.e., points with curvature greater than 0.1) and planar features (i.e., points with curvature less than 0.01) from the normal vector estimation and curvature calculation in the point cloud library PCL in the corrected point cloud frame.

[0063] Edge features were computed using the pcl::Feature class, and planar features were computed using pcl::NormalEstimation. ORB feature points were extracted from the corrected image frames using OpenCV's ORB feature extractor, with a feature point count of 500, a scale factor of 1.2, and a pyramid layer count of 8. Inertial measurement unit (IMU) pre-integration employed standard pre-integration algorithms from the literature, such as IMU pre-integration, integrating acceleration and angular velocity between adjacent keyframes to obtain the relative motion increment. .

[0064] Joint initialization employs a tightly coupled vision-inertial-lidar method, estimating gravity direction, accelerometer bias, and initial velocity by solving a system of linear equations, specifically using integrated navigation factors from GTSAM. The local sliding window size is set to 10 keyframes. Within the window, a factor map is constructed, including factors such as: reprojection error factor for edge features, planar distance factor for planar features, visual reprojection factor for ORB feature points, inertial measurement unit pre-integration factor, and pose prior factor.

[0065] Local bundle adjustment employs the Levenberg-Marquardt algorithm, such as the Levenberg-Marquardt Optimizer of GTSAM, to iteratively solve and update the pose and feature point depth of all keyframes within the window. The optimized pose sequence is then published to the path planning module, such as the TEB planner or DWA planner, at a frequency of 100Hz.

[0066] S71: Collect multiple segments of continuous sensing data from multiple robot runs. Each segment of continuous sensing data includes lidar scanning sequences, image sequences, and inertial measurement unit data, resulting in the raw sensing dataset.

[0067] During the topology map generation phase, the robot is controlled to perform multiple traversal runs in the target environment, such as circling three times along all aisles in a warehouse, collecting a set of continuous sensing data for each circumference. The LiDAR scanning sequence uses a 16-line or 32-line LiDAR, such as the Velodyne VLP-16, with a scanning frequency of 10Hz, and each frame contains approximately 30,000 points.

[0068] The image sequences were captured using a global shutter camera such as an Intel RealSense D435, with a resolution of 1280×720 and a frequency of 30Hz. The inertial measurement unit (IMU) data frequency was 200Hz, containing three-axis acceleration and three-axis angular velocity. The raw sensing dataset was stored in rosbag format with timestamp indexes. For example, if 5 minutes of data were collected in a single run, the raw sensing dataset would contain 3000 LiDAR frames, 9000 image frames, and 60000 IMU data points.

[0069] S72: Perform offline joint calibration on each segment of continuous sensing data in the original sensing dataset to generate a calibration parameter set.

[0070] Offline joint calibration employs Kalibr calibration tools, such as the open-source ETH Zurich, for multi-sensor spatiotemporal parameter estimation. For each data segment, the device's motion trajectory is first acquired using inertial measurement unit (IMU) data, and then the extrinsic rotation matrix of the camera relative to the IMU is obtained through vision-inertial calibration. Translation vector and time offset Next, the extrinsic parameters of the lidar relative to the inertial measurement unit are obtained using a lidar-inertial calibration algorithm, such as lidar_align in the open-source package. and .

[0071] All calibration parameters constitute a calibration parameter set, including: camera intrinsic parameter matrix K, distortion coefficients k1, k2, p1, p2, 6-DOF transformation between lidar and inertial measurement unit, 6-DOF transformation between camera and inertial measurement unit, and time offset between each sensor, typically between ±50 milliseconds.

[0072] S73: Based on the calibration parameter set, project each modal data in each segment of continuous sensing data onto a unified timestamp to obtain a synchronous sensing dataset.

[0073] Using the inertial measurement unit's time as a reference, all sensor data are linearly interpolated and resampled. For LiDAR data, the two most recent timestamps are found. and And its corresponding point cloud data, calculated through linear interpolation. The point cloud coordinates at time [time]. The interpolation formula is: .

[0074] in, Indicates the timestamp The three-dimensional coordinate vector of the lidar point cloud collected in real time; Indicates the timestamp The coordinates of the lidar point cloud at any given time; For the timestamp of the inertial measurement unit; and The timestamps of two adjacent frames of the lidar are satisfied. .

[0075] For image data, linear interpolation is also performed to obtain the image at the corresponding time. The projection operation involves using a calibration parameter set to project the interpolated point cloud from the LiDAR coordinate system to the camera coordinate system, and then transforming the data from the camera coordinate system to the inertial measurement unit coordinate system using a transformation matrix. In the final synchronized sensing dataset, each timestamp corresponds to a set of strictly aligned point cloud frames, image frames, and inertial measurement unit data.

[0076] S74: Extract three-dimensional geometric primitives and semantic labels from each frame of data in the synchronous sensing dataset to obtain a semantic feature set.

[0077] For the point cloud in each frame of synchronous sensing data, a region growing segmentation algorithm, such as the pcl::RegionGrowing class in the PCL library, is used to extract 3D geometric primitives: planar primitives are segmented based on normal vector consistency and distance thresholds, with the normal vector angle threshold set to 15 degrees and the distance threshold set to 0.05 meters; for each segmented planar point cloud cluster, the least squares method is used to fit the plane equation, obtaining the plane equation coefficients a, b, c, d, satisfying ax + by + cz + d = 0, where the normal vector (a, b, c) is a unit vector. Cylindrical primitives are fitted with their axis and radius using a random sampling consistency algorithm, employing the pcl::SACSegmentation class from the point cloud library with the model parameters set to SACMODEL_CYLINDER.

[0078] For the same frame of image, a deep learning model such as YOLOv8 or SegFormer is used for semantic segmentation to obtain a semantic label for each pixel, such as wall, floor, door, shelf, column, etc. Then, the semantic labels on the image are projected onto the point cloud, assigning a semantic label to each 3D geometric primitive. For example, if a fitted planar primitive corresponds to a wall region in the image, its semantic label is "wall". The semantic feature set consists of the parameters of each geometric primitive, including the plane equation coefficients a, b, c, d, the cylinder axis and radius R, the semantic label, and the corresponding pose node. The pose node represents the robot's pose at that frame time, obtained by integration using laser odometry or inertial measurement unit, and is stored in a structured data format such as JSON.

[0079] S75: Based on the semantic feature set, construct a pose graph and perform global graph optimization to generate a topology map.

[0080] Specifically, this step includes the following sub-steps:

[0081] S751: Initialize the empty pose graph.

[0082] Create an empty graph data structure in computer memory, using the `unordered_map` function from the C++ standard library to represent the set of nodes, with node IDs being integers, and a `vector` to represent the set of edges. This empty pose graph contains no nodes or edges. This can be implemented using the `Graph` class from the `g2o` or `GTSAM` libraries.

[0083] S752: Add each semantic feature in the semantic feature set and its corresponding pose node to the empty pose graph to form a node set.

[0084] Traverse the semantic feature set and create a pose node for each semantic feature. The pose node contains a unique ID, such as an auto-incrementing integer ID, 3D position coordinates (x, y, z), and a quaternion. Each semantic feature is also a node type, but it is associated with a pose node. For example, a semantic feature of a wall plane is associated with the robot's pose node when that wall was captured. All these nodes are inserted into the empty pose graph to form a node set. The number of nodes equals the number of semantic features plus the number of corresponding poses; typically, a scene has several thousand nodes.

[0085] S753: For the relative pose measurement between every two pose nodes in the node set, add edge constraints to generate an initial pose graph.

[0086] Relative pose measurement is obtained through pre-integration using the robot's odometry or inertial measurement unit. For two temporally adjacent pose nodes i and j, the relative pose is calculated as follows: ,

[0087] in and Let be the homogeneous transformation matrices of nodes i and j in the world coordinate system, respectively. Then, add a directed edge to the pose graph connecting nodes i and j. The edge's information matrix is ​​the inverse of the covariance matrix, with the initial covariance set to the diagonal matrix diag(0.01, 0.01, 0.01, 0.0003). After adding all edges, the initial pose graph is obtained. This graph can be represented using data structures from the g2o (General Graph Optimization) or GTSAM libraries.

[0088] S754: For each semantic feature in the semantic feature set, semantic tags belonging to the same semantic category are clustered to generate landmark clusters, resulting in a landmark cluster set.

[0089] In the semantic feature set, spatial clustering is performed on features with the same semantic labels. The density-based clustering algorithm DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is adopted.

[0090] This can be implemented using `sklearn.cluster.DBSCAN` from the scikit-learn library or `pcl::EuclideanClusterExtraction` from PCL. Set the neighborhood radius ε = 0.2 meters and the minimum number of samples minPts = 5. For example, feature point clouds with the semantic label "pillar" are clustered into a landmark cluster, representing multiple observations on the same pillar. The output of the clustering is a set of landmark clusters, where each cluster contains multiple semantic features and their corresponding pose observations.

[0091] S755: Calculate the centroid position and orientation for each landmark cluster in the landmark cluster set to obtain the centroid-orientation pair set.

[0092] For each landmark cluster, if the cluster consists of planar features, its centroid is the average of the coordinates of all points on the plane, and its orientation is the normal vector of the plane. If the cluster consists of cylindrical features, its centroid is the average of the coordinates of all points on the cylinder's axis, and its orientation is the direction of the cylinder's axis. Let there be N 3D points in the cluster. Each point is a three-dimensional vector, and the centroid is calculated as follows: .

[0093] Orientation is obtained through principal component analysis. Specifically, the Eigen::JacobiSVD class in the Eigen library can be used to perform singular value decomposition on the covariance matrix of the points, and the direction of the first principal component is taken as the orientation unit vector d. Each landmark cluster obtains a centroid-orientation pair (c,d).

[0094] S756: Input the constraints between each centroid-orientation pair in the centroid-orientation pair set and the corresponding pose node into the initial pose graph to update the initial pose graph.

[0095] For each centroid-orientation pair (c, d), find the set of all pose nodes that have observed this landmark cluster. Add a landmark observation factor to each observation, which associates the pose node with the global coordinates of the landmark. Let the pose node... (Transformation from the world to the robot's body), the observed coordinates of the landmark in the robot's local coordinate system are: The residual is: ,

[0096] in, For the robot's pose node, represents the 4×4 homogeneous transformation matrix from the robot's body coordinate system to the world coordinate system; The coordinates of the centroid of the landmark in the robot's local coordinate system; The global coordinates of the landmark to be optimized.

[0097] All residuals are added as new edges to the initial pose graph, while landmarks are introduced as fixed nodes. The updated pose graph adds landmark nodes and observation edges.

[0098] S757: Perform graph optimization iterative solution on each frame of data in the synchronous sensing dataset to generate an optimized pose sequence.

[0099] A nonlinear least squares solver, such as the Levenberg-Marquardt optimizer in GTSAM, is used for global optimization of the updated pose graph. Optimization variables include the six-DOF poses (position x, y, z and orientation quaternions) of all pose nodes and the 3D coordinates of all landmark nodes. The objective function is the weighted sum of squares of all edge constraints. Iterative solutions are applied until the error variation is less than 1e-6 or the maximum number of iterations (50) is reached. After optimization, each pose node receives a corrected pose, which is then sorted by timestamp to form an optimized pose sequence. The `optimize` method of the `LevenbergMarquardtOptimizer` class in GTSAM can be directly called.

[0100] S758: Perform principal component analysis on each landmark cluster in the landmark cluster set to generate principal direction vectors, thus obtaining the principal direction vector set.

[0101] For each landmark cluster, an N×3 matrix is ​​constructed from the coordinates of all its 3D points, where N is the number of points. Principal Component Analysis (PCA) is used, employing either `Eigen::PCA` from the `Eigen` library or `sklearn.decomposition.PCA` from `scikit-learn`. Specifically, the eigenvalues ​​and eigenvectors of the covariance matrix are calculated; the eigenvector corresponding to the largest eigenvalue is the principal direction vector. For example, for a row of shelf landmark clusters, the principal direction vector points in the direction the shelf extends. The principal direction vector set stores the principal direction vector for each landmark cluster, obtained after normalization. Parameter settings: The covariance matrix is ​​calculated after data decentering, and the eigenvalues ​​are solved using the Jacobi iteration method.

[0102] S759: Determine the cumulative error gradient of each pose node in the node set to obtain the gradient set.

[0103] The cumulative error gradient refers to the partial derivative of the objective function with respect to each pose node during the optimization process. In the last iteration of the optimizer, the residual Jacobian matrix of each pose node is obtained, and the gradient is: ,

[0104] in, The residual vector is formed by concatenating the residuals of all edge constraints; The residual Jacobian matrix for each pose node.

[0105] Calculate the gradient magnitude of each pose node. This yields the gradient set. A larger gradient indicates a less reliable pose estimation for that node. In GTSAM, this can be obtained by calling the gradient function of gaussNewtonOptimizer.

[0106] S7510: For every two main direction vectors in the main direction vector set, in response to determining that the dot product of the two main direction vectors is greater than or equal to the similarity threshold, the two landmark clusters corresponding to the two main direction vectors are merged to update the semantic feature set.

[0107] The similarity threshold is set to 0.9, meaning that two main direction vectors are considered to have the same direction if the angle between them is less than approximately 25 degrees. The similarity threshold is calculated for every two main direction vectors. and The dot product, if If so, the two corresponding landmark clusters will be merged into a new landmark cluster.

[0108] The merging method involves placing all semantic features from the two clusters into a new cluster and recalculating the centroid position and orientation. After merging, the semantic feature set is updated, the original two clusters are deleted, and the new cluster is added. For example, two independently observed sections of the same long wall are merged into a single landmark cluster representing the entire wall surface. This step can be implemented using an iterative hierarchical clustering method.

[0109] S7511: Rigidly register each pose node in the optimized pose sequence with each semantic feature in the merged semantic feature set, and input it into the updated pose graph.

[0110] Rigid registration refers to transforming each semantic feature from the robot's local coordinate system to the global coordinate system using the pose node with the corresponding timestamp in the optimized pose sequence. Let the semantic feature be represented in the local coordinate system as... For example, local parameters of the plane equation, pose nodes. (4×4 homogeneous transformation matrix), then the global representation is: .

[0111] For point cloud features, coordinate transformation is performed directly; for planar features, the normal vector needs to be rotated to the global direction. All registered semantic features are input as new fixed nodes into the updated pose graph, replacing the original landmark nodes.

[0112] S7512: Based on a preset semantic category group, the pose nodes and semantic features included in the updated pose graph are labeled to generate a topology map.

[0113] The pre-defined semantic category groups include six categories: walls, floors, doors, pillars, shelves, and obstacles. The algorithm iterates through all nodes in the updated pose graph, labeling each node with its semantic tag. Pose nodes are labeled as path points, and semantic feature nodes are labeled with their respective categories, such as labeling walls as "walls". After labeling, the graph structure is serialized into a file format such as JSON or YAML and stored in a topology map server. This topology map can be used for subsequent real-time robot localization and perception.

[0114] Furthermore, the storage medium in the embodiments of this application stores program instructions capable of implementing all the above methods. These program instructions can be stored in the storage medium in the form of a software product, including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods described in the various embodiments of this application. 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, or terminal devices such as computers, servers, mobile phones, and tablets.

[0115] The above description is merely an explanation of some preferred embodiments of the present invention and the technical principles employed. Those skilled in the art should understand that the scope of the invention as described in the embodiments of the present invention is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described inventive concept. For example, technical solutions formed by substituting the above-described features with (but not limited to) technical features with similar functions disclosed in the embodiments of the present invention.

Claims

1. A robot environment perception method based on multi-sensor fusion, comprising: Spatiotemporal synchronization processing is performed on raw multi-sensor data to generate synchronized sensing data; Determine whether there are semantic landmarks in the topology map stored in the topology map server that match the original multi-sensor data or the synchronous sensing data. The matching semantic landmark is a semantic landmark in the semantic landmark stored in the topology map that corresponds to the same geometric primitive in the original multi-sensor data or the synchronous sensing data. When there are semantic landmarks in the topology map that match the original multi-sensor data or the synchronous sensing data, select the semantic landmarks that match the original multi-sensor data or the synchronous sensing data from the topology map as matching landmarks to obtain a set of matching landmarks; Based on the matching landmark set, pose correction processing is performed on the original multi-sensor data to obtain multiple corrected original multi-sensor data.

2. The method according to claim 1, characterized in that, The pose correction processing of the original multi-sensor data based on the matching landmark set includes: For each preset condition in the preset condition group, in response to determining that the original multi-sensor data or the synchronous sensing data meets the preset condition, the original multi-sensor data is subjected to pose correction processing according to the semantic category and observation count of each matching landmark included in the topology map.

3. The method according to claim 1, characterized in that, The pose correction processing of the original multi-sensor data based on the matching landmark set includes: In response to the original multi-sensor data or the synchronous sensing data satisfying the keyframe condition, the joint covariance matrix of each matching landmark in the matching landmark set is determined based on the original multi-sensor data, thus obtaining a set of covariance matrices. Based on the covariance matrix set and the matching landmark set, pose correction processing is performed on the original multi-sensor data.

4. The method according to claim 1, characterized in that, The method further includes: When there are no semantic landmarks in the topology map that match the original multi-sensor data or the synchronous sensing data, the grid map interface is displayed for the user to set dynamic obstacle areas. The dynamic obstacle area set by the user is binarized and then input into the topology map.

5. The method according to claim 1, characterized in that, The method further includes: The corrected raw multi-sensor data is input into the local factor graph optimizer, and the following steps are performed: Extract edge and planar features from the corrected lidar point cloud frame; extract ORB feature points from the corrected image frame; The pre-integration results of the inertial measurement unit are jointly initialized with the edge features, planar features, and ORB feature points of the image by the lidar to construct a local sliding window; Based on the co-view relationship of multiple keyframes in the local sliding window, local bundle adjustment is performed to update the robot pose, and the optimized pose sequence is output for selection by the robot path planning module.

6. The method according to claim 1, characterized in that, The topology map is obtained through the following steps: Collect multiple segments of continuous sensing data from multiple robot runs. Each segment of continuous sensing data includes lidar scanning sequences, image sequences, and inertial measurement unit data to obtain the raw sensing dataset. Offline joint calibration is performed on each segment of continuous sensing data in the original sensing dataset to generate a calibration parameter set; Based on the calibration parameter set, the modal data in each segment of continuous sensing data are projected onto a unified timestamp to obtain a synchronous sensing dataset; Three-dimensional geometric primitives and semantic labels are extracted from each frame of data in the synchronous sensing dataset to obtain a semantic feature set; Based on the semantic feature set, a pose graph is constructed and global graph optimization is performed to generate a topology map.

7. The method according to claim 6, characterized in that, The step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map includes: Initialize the empty pose graph; Each semantic feature in the semantic feature set and its corresponding pose node are added to the empty pose graph to form a node set; For the relative pose measurement between every two pose nodes in the node set, edge constraints are added to generate an initial pose graph.

8. The method according to claim 7, characterized in that, The step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map further includes: For each semantic feature in the semantic feature set, the semantic tags belonging to the same semantic category are clustered to generate landmark clusters, thus obtaining a landmark cluster set; For each landmark cluster in the landmark cluster set, calculate the centroid position and orientation to obtain a centroid-orientation pair set; The constraints between each centroid-orientation pair in the centroid-orientation pair set and its corresponding pose node are input into the initial pose graph to update the initial pose graph.

9. The method according to claim 8, characterized in that, The step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map further includes: Graph optimization iterative solution is performed on each frame of data in the synchronous sensing dataset to generate an optimized pose sequence; Principal component analysis is performed on each landmark cluster in the landmark cluster set to generate principal direction vectors, thus obtaining a set of principal direction vectors. The cumulative error gradient of each pose node in the node set is determined to obtain the gradient set; For every two main direction vectors in the set of main direction vectors, in response to determining that the dot product of the two main direction vectors is greater than or equal to a similarity threshold, the two landmark clusters corresponding to the two main direction vectors are merged to update the semantic feature set.

10. The method according to claim 9, characterized in that, The step of constructing a pose graph and performing global graph optimization based on the semantic feature set to generate a topology map further includes: Each pose node in the optimized pose sequence is rigidly registered with each semantic feature in the merged semantic feature set and then input into the updated pose graph. Based on a preset semantic category group, each pose node and semantic feature included in the updated pose graph is labeled to generate a topology map.

Images