A substation inspection slam method based on ground description

Abstract

The application belongs to the technical field of robot navigation and positioning, and proposes a substation inspection SLAM method based on a ground description: a multi-layer concentric circle division model is established; the multi-layer concentric circle division model is used for ground segmentation, and a multi-dimensional ground description is calculated; a ground bin information map is established, and the calculated multi-dimensional ground description is stored; non-ground point clouds are obtained based on the multi-layer concentric circle division model, and the non-ground point clouds are clustered to obtain an effective clustering set; based on the geometric feature judgment of the ground bin information and the clustering attribute, it is judged whether the effective clustering is a dynamic target candidate, and the dynamic target candidate is subjected to time sequence consistency judgment to identify the dynamic target and remove it from the non-ground point cloud; based on the multi-dimensional ground description, the candidate loop frame in loop detection is constrained, the optimization of loop detection is completed, and the optimal pose of the inspection robot is obtained.

Description

Technical Field

[0001] This invention relates to the field of robot navigation and positioning technology, and in particular to a substation inspection SLAM method based on ground descriptors, which is applicable to autonomous navigation and inspection tasks in complex industrial environments such as substations. Background Technology

[0002] With the advancement of smart grid construction, the demand for automation and intelligence in substation inspection operations is increasing. Traditional substation inspections mainly rely on manual labor, which suffers from low efficiency, high safety risks, and significant susceptibility to environmental influences. In recent years, inspection robots based on SLAM (Simultaneous Localization and Mapping) technology have been widely used in substation environments.

[0003] The existing SLAM systems mainly have the following problems:

[0004] 1. Insufficient utilization of ground features: Traditional SLAM systems mainly rely on vertical structures such as buildings and obstacles for positioning, which limits the utilization of ground features. In substations in Northwest China with complex ground conditions such as sand and gravel reflection, positioning drift is prone to occur.

[0005] 2. Dynamic target interference: There are dynamic targets such as personnel and work vehicles in the substation environment. Traditional SLAM systems have difficulty effectively distinguishing between static and dynamic features, resulting in ghosting in map construction and affecting inspection work.

[0006] 3. Low ground segmentation accuracy: Existing ground segmentation algorithms have low segmentation accuracy under complex terrain and different ground materials, which affects the extraction quality of ground descriptors.

[0007] 4. High real-time requirements: Substation inspection requires real-time positioning and navigation. Existing SLAM systems have high computational complexity and cannot meet real-time requirements.

[0008] 5. Substations have dense equipment and complex metal structures, making it easy for lidar to generate a lot of noise and reflection interference; traditional visual SLAM systems are easily affected by factors such as changes in lighting and equipment obstruction in substation environments. Summary of the Invention

[0009] The purpose of this invention is to provide a substation inspection SLAM method based on ground descriptors to solve the problems of low positioning accuracy and poor robustness of substation inspection robots in complex environments in the prior art.

[0010] The technical solution adopted by this invention to solve its technical problem is as follows:

[0011] A substation inspection SLAM method based on ground descriptors is applied to an inspection robot equipped with a multi-line lidar and an inertial measurement unit. The inspection robot is configured with a computing unit to execute the method, which includes the following steps:

[0012] Acquire lidar point cloud data of the substation environment, and establish a multi-layer concentric circle partitioning model based on the point cloud density of the lidar point cloud data;

[0013] The ground is segmented using a multi-layer concentric circle partitioning model, and a multi-dimensional ground descriptor is calculated. The ground descriptor includes ground normal vector, elevation, flatness, roughness, and confidence level.

[0014] Establish a ground bin information map and store the calculated multidimensional ground descriptors;

[0015] Non-ground point clouds are obtained based on a multi-layer concentric circle partitioning model, and the non-ground point clouds are clustered to obtain an effective cluster set.

[0016] Based on the geometric features of ground bin information and clustering attributes, it is determined whether the effective cluster is a candidate for dynamic target, and the temporal consistency of the candidate for dynamic target is judged to identify the dynamic target and remove it from the non-ground point cloud.

[0017] By constraining candidate loop closure frames during loop closure detection using multidimensional ground descriptors, loop closure detection is optimized, and the optimal pose of the inspection robot is obtained.

[0018] In some embodiments, the established multi-layer concentric circle partitioning model is a four-layer concentric circle partitioning model;

[0019] The ground segmentation using a multi-layer concentric circle partitioning model includes: using a four-layer concentric circle partitioning model to divide the multi-line lidar scanning area into four concentric circle regions according to angle and distance;

[0020] The first layer of concentric circular regions has a radial range of 0-5m and is divided into 16 sectors, each sector containing 2 annular sub-regions. The second layer of concentric circular regions has a radial range of 5-10m and is divided into 32 sectors, each sector containing 4 annular sub-regions. The third layer of concentric circular regions has a radial range of 10-15m and is divided into 54 sectors, each sector containing 4 annular sub-regions. The fourth layer of concentric circular regions has a radial range of 15-20m and is divided into 32 sectors, each sector containing 4 annular sub-regions.

[0021] In some embodiments, after performing ground segmentation using a multi-layer concentric circle partitioning model and before calculating the multidimensional ground descriptor, the method further includes:

[0022] The lidar point cloud data is distributed to the corresponding sectors through polar coordinate transformation.

[0023] In some embodiments, after allocating the lidar point cloud data to the corresponding sectors via polar coordinate transformation, the method further includes:

[0024] Set a threshold for the number of points in a sector, and filter the number of points in each sector, classifying sectors with fewer than the threshold into non-ground point clouds.

[0025] For sectors that meet the point count requirement, sort them in ascending order of Z coordinate, select the first few points with the smallest Z coordinate as seed points, and obtain the seed point set;

[0026] For each sector's seed point set, calculate the covariance matrix and perform singular value decomposition to solve for the normal vector of the ground plane;

[0027] Based on the seed point, the ground segment is iteratively expanded. A distance threshold is set, and points whose distance to the current plane is less than the threshold are included in the ground segment until the distance of the newly added point to the plane exceeds the threshold. This process results in a ground point set and a non-ground point set.

[0028] In some embodiments, calculating the multidimensional ground descriptor means: calculating the normal vector, elevation, flatness, roughness, and confidence of each sector based on the segmented set of ground points;

[0029] The flatness is calculated using the residuals of the plane fitting, the roughness is calculated using the standard deviation of the Z coordinates of the ground points, and the confidence level is determined based on the number of ground points and the accuracy of the plane fitting.

[0030] In some embodiments, obtaining non-ground point clouds based on a multi-layer concentric circle partitioning model and clustering the non-ground point clouds to obtain an effective cluster set refers to:

[0031] For the non-ground point set obtained from the segmentation, the DBSCAN algorithm is used for clustering, and the clustering parameters are set, with the neighborhood radius set to 0.3m and the minimum number of points set to 3.

[0032] Introducing a KD-tree into the neighborhood calculation step accelerates neighborhood search;

[0033] The clustering results are filtered, and clusters with fewer than 3 points are removed to obtain a set of valid clusters.

[0034] In some embodiments, the determination of whether a valid cluster is a dynamic target candidate based on the geometric features of ground bin information and clustering attributes means:

[0035] For each valid cluster in the valid cluster set, calculate the cluster centroid and normal vector;

[0036] Traverse the ground bin information map and find the most relevant ground bin below the cluster;

[0037] The ground is classified according to the roughness of the most relevant ground bins. The corresponding geometric threshold is used to determine whether the clustering conforms to the geometric characteristics of dynamic targets from three dimensions: size, height, and normal vector direction, and to preliminarily identify dynamic target candidates.

[0038] In some embodiments, the step of performing temporal consistency judgment on dynamic target candidates, identifying dynamic targets, and removing them from the non-terrestrial point cloud refers to:

[0039] Maintain a sliding window to store the clustering results of the most recent 5 historical point clouds;

[0040] For clusters that are identified as candidates for dynamic targets based on geometric features, a KD tree is used to perform a fast nearest neighbor search in the most recent 5 frames of historical point clouds to find if there is a historical cluster whose Euclidean distance to the current cluster center is within 0.5m. If no such cluster is found, and the minimum distance in all historical frames is greater than 1.0m, then the cluster is identified as a dynamic target and removed from the non-ground point cloud.

[0041] In some embodiments, constraining candidate loop closure frames during loop closure detection based on multidimensional ground descriptors to optimize loop closure detection and obtain the optimal pose of the inspection robot includes the following steps:

[0042] Key frames are extracted based on IMU data from multi-line lidar and lidar data from inertial measurement unit. For the current key frame, the translation distance and time difference between it and historical key frames are calculated. Historical key frames with a translation distance of less than 10m and a time difference of more than 20 seconds are selected as candidate loopback frames.

[0043] Extract the ground descriptors of candidate loop closure frames and the current keyframe, compare the ground flatness and normal vectors of the two, design a scoring function, and select the top 5 candidate loop closure frames with the highest scores as objects for further matching.

[0044] For the filtered candidate loop closure frames, extract edge point and planar point features, and calculate the number of feature matching pairs between the current keyframe and the candidate loop closure frames. If the number of matching pairs is less than the threshold, exclude the candidate loop closure frame and complete the coarse feature matching of the candidate loop closure frames.

[0045] For candidate loop closure frames that pass the coarse feature matching, voxel filtering is used to downsample the point cloud. The pose obtained from the coarse feature matching is used as the initial value, and the relative pose is optimized using the point-to-surface ICP algorithm. The upper limit of the number of iterations and the threshold of pose transformation are set. When the iteration stopping condition is met, the average error after ICP matching is calculated. If the average error is less than the threshold, the loop closure is determined to be valid and is added as a loop closure factor to the factor graph optimization.

[0046] The effective closure factor, IMU pre-integration factor, and lidar odometry factor are incorporated into the factor graph, and the optimal solution of the factor graph is solved using Ceres Solver to optimize the global pose.

[0047] The beneficial effects of this invention are: by designing a specialized ground descriptor and optimizing the SLAM algorithm, high-precision and robust autonomous localization and map building in substation environments are achieved, resulting in the following technical advantages:

[0048] 1. Improve the utilization rate of ground features: This invention extracts multi-dimensional ground descriptors such as ground normal vector, elevation, flatness, and roughness, and makes full use of ground features for positioning. Compared with the traditional SLAM method that relies on vertical structure positioning, it can effectively reduce positioning drift and improve positioning accuracy in substations in Northwest China where the ground is sandy and gravelly and the reflection conditions are complex. After testing on the KITTI dataset 07 sequence, the trajectory error of this invention can be controlled within 0.5m in complex environments.

[0049] 2. Effective removal of dynamic target interference: This invention can identify dynamic targets based on ground normal vectors, combined with geometric feature judgment and temporal consistency verification, and can accurately identify dynamic targets such as workers and inspection vehicles in the substation environment, avoiding map ghosting problems caused by dynamic targets and improving the accuracy of map construction. By introducing optimization measures such as KD tree to accelerate neighborhood calculation, voxel filtering to reduce the number of point clouds, and screening candidate loopback frames to reduce the amount of computation, the computational complexity of the system is reduced. The point cloud processing frame rate of the system can reach more than 10Hz, which can meet the real-time positioning and navigation needs of substation inspection robots.

[0050] 3. Enhanced environmental adaptability: This invention dynamically updates the elevation and flatness thresholds based on historical ground data and adaptively adjusts the dynamic target judgment criteria according to ground roughness, enabling the system to adapt to environmental changes in different areas of the substation and maintain stable performance in different scenarios such as densely populated equipment areas, open areas, and areas with dynamic targets. Attached Figure Description

[0051] Figure 1 This is a flowchart of a substation inspection SLAM method based on ground descriptors according to Embodiment 1 of the present invention;

[0052] Figure 2 This is a comparison diagram of the trajectory routes of the KITTI dataset 07 sequence in Embodiment 4 of the present invention. Detailed Implementation

[0053] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0054] Example 1

[0055] This embodiment provides a substation inspection SLAM method based on ground descriptors, applied to an inspection robot equipped with a multi-line lidar and inertial measurement unit. The inspection robot is configured with a computing unit to execute the method. See the flowchart below. Figure 1 The method may include the following steps:

[0056] S1. Obtain lidar point cloud data of the substation environment, and establish a multi-layer concentric circle partitioning model based on the point cloud density of the lidar point cloud data.

[0057] S2. Use a multi-layer concentric circle partitioning model to segment the ground and calculate a multi-dimensional ground descriptor, which includes ground normal vector, elevation, flatness, roughness and confidence.

[0058] S3. Establish a ground bin information map and store the calculated multidimensional ground descriptors;

[0059] S4. Obtain non-ground point clouds based on the multi-layer concentric circle partitioning model, and cluster the non-ground point clouds to obtain an effective cluster set;

[0060] S5. Based on the geometric feature judgment of ground bin information and clustering attributes, determine whether the effective cluster is a dynamic target candidate, and perform temporal consistency judgment on the dynamic target candidate to identify the dynamic target and remove it from the non-ground point cloud.

[0061] S6. Constrain the candidate loop closure frames during loop closure detection based on the multi-dimensional ground descriptor to optimize the loop closure detection and obtain the optimal pose of the inspection robot.

[0062] In practical applications, it is necessary to first set up the hardware environment and configure the software environment.

[0063] First, regarding the hardware environment setup, the hardware components in this embodiment mainly include the inspection robot body, multi-line LiDAR, inertial measurement unit (IMU), computing unit, and power module, wherein:

[0064] Inspection robot body: A wheeled inspection robot is selected, which has good motion performance and stability and can adapt to flat ground, slight slope and other terrains in the substation. The robot body is equipped with a motion control module, which can receive the pose information output by the SLAM method to achieve autonomous navigation.

[0065] Multi-line lidar: 32-line or 64-line lidar is selected, with a horizontal field of view of 360° and a vertical field of view selected according to actual needs (e.g., -15°~15°). The ranging range is 0.1~200 meters, and the scanning frequency is 10~20Hz. It can quickly acquire point cloud data of the substation environment, providing data support for ground segmentation and descriptor extraction.

[0066] Inertial Measurement Unit (IMU): A high-precision IMU with a sampling frequency of 200~500Hz is selected. It is equipped with an accelerometer and a gyroscope, which can measure the robot's acceleration and angular velocity, provide motion constraints for the SLAM method, assist in pose estimation, and reduce positioning drift.

[0067] Computing Unit: An industrial-grade embedded computer is selected, equipped with a multi-core processor (such as an Intel Core i7 or an ARM processor with equivalent performance), 8GB or more of memory and 128GB or more of solid-state drive, which can meet the real-time computing requirements of SLAM method, and also supports Linux operating system such as Ubuntu, which is convenient for installing and running related algorithm libraries (such as PCL, ROS, etc.).

[0068] Power module: It adopts a high-capacity lithium battery pack with a voltage of 12V or 24V. The capacity is determined according to the power consumption and working time requirements of the inspection robot (such as 100Ah or more). It has overcharge, over-discharge and short circuit protection functions to ensure stable power supply to the system.

[0069] In addition, regarding the software environment configuration, the software environment in this embodiment is built based on the Ubuntu 20.04 operating system and Robot Operating System (ROS Noetic), and mainly includes the following software modules and libraries:

[0070] Point Cloud Processing Library: Install the Point Cloud Library (PCL 1.11), which is used for point cloud filtering, segmentation, clustering, feature extraction and other operations, providing basic algorithm support for ground segmentation and descriptor extraction modules.

[0071] SLAM Basic Framework: Based on the LIO-SAM algorithm framework, secondary development is carried out. The LIO-SAM algorithm integrates LiDAR and IMU data, which has high positioning accuracy and real-time performance, and is suitable as the basic algorithm framework for this embodiment.

[0072] Mathematical computation library: Install Eigen 3.3 for matrix operations, linear algebra solutions, etc., providing mathematical computation support for algorithms such as singular value decomposition and ICP matching; install Ceres Solver 2.0 for nonlinear optimization, realizing back-end pose solving for factor graph optimization.

[0073] Visualization tools: Install RViz for real-time visualization of point cloud data, robot pose, map building results, etc., to facilitate debugging and verification of system performance; install Matplotlib for plotting experimental data curves and analyzing system metrics such as positioning accuracy and loop closure detection success rate.

[0074] Drivers: Install the ROS drivers for the LiDAR and IMU to ensure that the LiDAR and IMU can work properly, and publish point cloud data and IMU data through ROS topics for SLAM methods to receive and process.

[0075] It should be noted that, given the aforementioned hardware and software environment setup, the first step is to implement ground segmentation and multidimensional ground descriptor extraction. In this embodiment, ground segmentation and multidimensional ground descriptor extraction may include the following steps:

[0076] Step 1: Point Cloud Data Reception and Preprocessing: Subscribe to the point cloud data published by the LiDAR through ROS topics, and perform distortion correction on the original point cloud to eliminate errors caused by motion distortion of the LiDAR; at the same time, perform voxel filtering on the distortion-corrected point cloud to reduce the point cloud density and reduce the amount of computation. The filtering parameters are adjusted according to the resolution of the LiDAR and the actual environment, such as setting the voxel size to 0.05m×0.05m×0.05m.

[0077] Step 2: Concentric Circle Region Division and Point Cloud Allocation: Based on the four-layer concentric circle region division model designed in this embodiment, the radial range, number of sectors, and number of annular sub-regions of each region are set. The preprocessed point cloud is then allocated to the corresponding sectors through polar coordinate transformation. For example, the radial range of the first layer region (zone 1) is set to 0~5m, divided into 16 sectors, each sector containing 2 annular sub-regions; the radial range of the second layer region (zone 2) is set to 5~10m, divided into 32 sectors, each sector containing 4 annular sub-regions, and so on.

[0078] Step 3: Sector Point Count Filtering and Seed Point Selection: Set a point count threshold (e.g., 5 points), filter the points for each sector, and classify sectors with fewer than the threshold into non-ground point clouds; for sectors that meet the point count requirements, sort them in ascending order of Z coordinate, and select the first few points with the smallest Z coordinate (e.g., the first 10% of points) as seed points to obtain the low-order seed point set.

[0079] Step 4: Solving for ground plane parameters and expanding ground segments: For the seed point set of each sector, calculate the covariance matrix and perform singular value decomposition to solve for the normal vector of the ground plane; based on the seed points, iteratively expand the ground segments, set a distance threshold (e.g., 0.03m), and include points whose distance to the current plane is less than the threshold into the ground segments until the distance of newly added points to the plane exceeds the threshold, thus separating ground points and non-ground points.

[0080] Step 5: Ground Descriptor Calculation and Storage: Based on the segmented ground point set, calculate parameters such as normal vector, elevation, flatness, roughness, and confidence level for each sector. Flatness is calculated using the residual of plane fitting, roughness is calculated using the standard deviation of the Z coordinates of the ground points, and confidence level is determined comprehensively based on the number of ground points and the accuracy of plane fitting. Store the calculated ground descriptors in the ground bin information map and publish them through ROS topics for use by subsequent modules.

[0081] Therefore, in this embodiment, the established multi-layer concentric circle partitioning model is a four-layer concentric circle partitioning model. The ground segmentation using the multi-layer concentric circle partitioning model includes: using the four-layer concentric circle partitioning model, dividing the multi-line lidar scanning area into four concentric circle regions according to angle and distance; wherein, the radial range of the first layer concentric circle region is 0~5m, divided into 16 sectors, each sector containing 2 annular sub-regions; the radial range of the second layer concentric circle region is 5~10m, divided into 32 sectors, each sector containing 4 annular sub-regions; the radial range of the third layer concentric circle region is 10~15m, divided into 54 sectors, each sector containing 4 annular sub-regions; and the radial range of the fourth layer concentric circle region is 15~20m, divided into 32 sectors, each sector containing 4 annular sub-regions.

[0082] Therefore, after performing ground segmentation using a multi-layer concentric circle partitioning model, and before calculating the multi-dimensional ground descriptor, the method further includes: allocating the lidar point cloud data to corresponding sectors through polar coordinate transformation; after allocating the lidar point cloud data to corresponding sectors through polar coordinate transformation, the method further includes:

[0083] A threshold for the number of points in a sector is set, and each sector is filtered for points, with sectors having fewer points than the threshold being classified as non-ground point clouds. For sectors that meet the point count requirement, they are sorted in ascending order of Z-coordinate, and the first few points with the smallest Z-coordinates are selected as seed points to obtain a seed point set. For the seed point set of each sector, the covariance matrix is ​​calculated and singular value decomposition is performed to solve for the normal vector of the ground plane. Based on the seed points, the ground segment is iteratively expanded, a distance threshold is set, and points whose distance to the current plane is less than the threshold are included in the ground segment until the distance of newly added points to the plane exceeds the threshold, thus obtaining a ground point set and a non-ground point set.

[0084] It should be noted that the calculation of the multidimensional ground descriptor refers to: calculating the normal vector, elevation, flatness, roughness, and confidence level of each sector based on the segmented set of ground points; the flatness is calculated by the residual of the plane fitting, the roughness is calculated by the standard deviation of the Z coordinate of the ground points, and the confidence level is determined based on the number of ground points and the accuracy of the plane fitting.

[0085] Next, dynamic clustering removal based on ground normal vectors can be performed. In this embodiment, the steps for dynamic clustering removal based on ground normal vectors are as follows:

[0086] Step 1: Non-Ground Point Cloud Clustering: Obtain non-ground point clouds from the ground segmentation module, and use the DBSCAN algorithm for clustering. Set clustering parameters (e.g., neighborhood radius is set to 0.3m, minimum number of points is set to 3). Introduce KD tree in the neighborhood calculation step to accelerate neighborhood search and improve clustering efficiency. Filter the clustering results and remove clusters with fewer than 3 points to obtain a valid cluster set.

[0087] Step 2, Geometric Feature Judgment: For each valid cluster, calculate the cluster's center point and normal vector. Traverse the ground bin information map to find the most relevant ground bin below the cluster. Classify the ground according to the roughness of the ground bin, and use the corresponding geometric threshold to judge whether the cluster meets the geometric features of the dynamic target from three dimensions: size, height, and normal vector direction. For example, for flat ground, if the normal vector deflection angle of the cluster is greater than 40°, and the size is within the range of human or vehicle size, and the elevation difference with the ground bin is greater than 0.1m, it is initially judged as a candidate for dynamic target.

[0088] Step 3, Temporal Consistency Judgment: Maintain a sliding window to store the clustering results of the most recent 5 historical point cloud frames; for clusters that are judged to be candidates for dynamic targets based on geometric features, use a KD tree to perform a fast nearest neighbor search in the most recent 5 historical point cloud frames to find whether there is a historical cluster whose Euclidean distance to the current cluster center is within 0.5m; if not found, and the minimum distance of all historical frames is greater than 1.0m, then it is determined to be a dynamic target and removed from the point cloud data.

[0089] Then, loop closure detection optimization under ground information constraints can be performed. In this embodiment, loop closure detection optimization under ground information constraints can be achieved through the following steps:

[0090] Step 1: Keyframe Extraction and Pose Selection: Based on IMU data and LiDAR data, extract keyframes. The keyframe extraction interval is adjusted according to the robot's movement speed (e.g., extract one keyframe every 0.5m or every 1 second). For the current keyframe, calculate its translation distance and time difference with historical keyframes, and select historical keyframes with a translation distance of less than 10m and a time difference of more than 20 seconds as candidate loopback frames.

[0091] Step 2, coarse feature matching: Extract the ground descriptors of candidate loop closure frames and the current keyframe, compare the ground flatness and normal vectors of the two, design a scoring function (such as a weighted sum of the absolute value of the flatness difference and the cosine of the angle between the normal vectors), and select the top 5 candidate loop closure frames with the highest scores as objects for further matching; for the selected candidate loop closure frames, extract edge point and planar point features, calculate the number of feature matching pairs between the current frame and the candidate frames, and if the number of matching pairs is less than a threshold (such as 30 pairs), then exclude the candidate frame.

[0092] Step 3: Fine Matching and Loop Closure Determination: For candidate loop closure frames that pass the coarse feature matching, voxel filtering is used to downsample the point cloud (voxel size is set to 0.1m×0.1m×0.1m). Using the pose obtained from the coarse matching as the initial value, the point-to-surface ICP algorithm is used to optimize the relative pose. An upper limit for the number of iterations (e.g., 50 times) and a pose transformation threshold (e.g., 0.001m) are set. When the iteration stopping condition is met, the average error after ICP matching is calculated. If the average error is less than the threshold (e.g., 0.05m), the loop closure is determined to be valid and is added as a loop closure factor to the factor graph optimization.

[0093] Step 4, Global Pose Optimization: The effective loop closure factor, IMU pre-integration factor, and LiDAR odometry factor are incorporated into the factor graph. The optimal solution of the factor graph is solved using Ceres Solver to correct the global pose and offset the cumulative drift. The optimized global pose is published through the ROS topic for use by the motion control module of the inspection robot to achieve autonomous navigation.

[0094] Example 2

[0095] Based on Example 1, this example performs performance testing and optimization on a substation inspection SLAM method based on ground descriptors. When setting up the test environment, the actual substation environment is selected as the test site. The test site should include areas with dense equipment, open areas, and areas with dynamic targets (such as staff and inspection vehicles), covering typical substation scenarios. Several marker points with known coordinates are set in the test site to evaluate the positioning accuracy of the system.

[0096] Different performance test metrics can include positioning accuracy, dynamic target removal rate, loop closure detection success rate, and real-time performance, among which:

[0097] For positioning accuracy: By comparing the robot's estimated pose with the actual coordinates of the marker points, the positioning error (such as translation error and rotation error) is calculated, and the positioning accuracy of the system in different scenarios is evaluated.

[0098] For dynamic target removal rate: Arrange staff to walk and inspection vehicles to move within the test site, and count the proportion of dynamic targets successfully identified and removed by the system out of the total number of dynamic targets to evaluate the performance of the dynamic target recognition module.

[0099] Regarding the loop closure detection success rate: Multiple loop closure points are set in the robot's inspection path, and the proportion of the number of times the system successfully detects loop closures out of the total number of loop closures is counted to evaluate the performance of the loop closure detection module.

[0100] Regarding real-time performance: record the time it takes for the system to process lidar and IMU data, calculate the system's frame rate (such as point cloud processing frame rate and pose estimation frame rate), and evaluate whether the system's real-time performance meets the requirements of substation inspection (such as a frame rate of not less than 10Hz).

[0101] Finally, based on the performance test results, the system parameters are adjusted and optimized. For example, if the positioning accuracy is low, parameters such as the distance threshold for ground segmentation and the number of iterations for ICP matching can be adjusted; if the dynamic target removal rate is not high, the classification threshold for ground roughness and the size determination range for dynamic targets can be optimized; if the real-time performance is insufficient, parameters such as the voxel size for point cloud filtering and the keyframe extraction interval can be adjusted to improve the real-time performance of the system while ensuring system performance.

[0102] Example 3

[0103] Based on Examples 1 and 2, this example provides a detailed explanation of three aspects: ground segmentation and descriptor extraction, dynamic clustering removal based on ground normal vectors, and loop closure detection optimization under ground information constraints.

[0104] First, for ground segmentation and descriptor extraction, the density of LiDAR point clouds decreases with distance, i.e., points are denser at close range and sparser at far range, making it impossible to adapt to uniform segmentation accuracy. In addition, outdoor substations in Northwest China have a large proportion of open areas and complex ground conditions (flat cement, gravel, grass). Traditional RANSAC algorithms using a single global model are difficult to handle complex terrains and require adaptive local ground fitting.

[0105] Therefore, in this embodiment, in order to match the point cloud density distribution, the multi-line lidar scanning area is divided into multiple concentric circle regions according to the angle and distance using a concentric circle model. Each region consists of several rings, and each ring is further divided into several sectors.

[0106] When assigning the input point cloud to the model, let the input points of the input point cloud be... The radial distance of the point is calculated through polar coordinate transformation. and angle Here, you can... The radial interval falling into the region index ,area The radial range is ,but In the region Within, the ring is determined by radially subdividing the intervals. The index, whose radial range is ,but Angle Evenly divided into Each sector has an angle range of [number] sectors. ,but .

[0107] The points are assigned to the corresponding sectors after the coordinate mapping described above. In the diagram, let the set of points within this region be denoted as . .

[0108] For each sector Point filtering is performed; sectors with too few points lack statistical significance and are directly classified as non-terrestrial point clouds to avoid misclassification. Considering the height and regional characteristics of the inspection robot's sensors, other sectors are selected as seed points by arranging them in ascending Z-coordinate order. Window start point. Expand window to maximum Satisfy the height difference threshold within the window: Here, we obtain the low-order seed point set. Ground plane parameters are solved by singular value decomposition (SVD) of the covariance matrix of the point set: Let the candidate ground point set be... Mean vector Denotes the geometric center of the point set. Let represent the number of points in the point set, where the formula for calculating the set center is: , , .

[0109] In this embodiment, the ground normal vector can be obtained by calculating the covariance matrix based on the mean vector and then calculating the minimum singular value. The covariance matrix is , where each element is:

[0110] ,

[0111] ,

[0112] ,

[0113] ,

[0114] Then, singular value decomposition can be performed on the covariance matrix. ,in It is a left singular vector matrix. It is a singular diagonal matrix, and , This is the transpose of the right singular vector matrix. The normal vector of the ground plane. Corresponding minimum singular value The corresponding left singular vector, i.e.: Iteratively expand the ground segment based on the seed point, if its distance to the current plane is... Then the distance is included in the ground segment. When the distance from a newly added point to the plane exceeds a threshold, the accumulation stops, and the point is segmented into ground points based on the accumulation status. Non-ground points Ultimately, we obtained, .

[0115] In this embodiment, parameters are calculated and features are quantified based on the ground point set. Verticality is used to determine uprightness, elevation is used to determine the distance between the point cloud and the sensor, and a flatness threshold is used to comprehensively determine the ground morphology. Finally, points that meet the screening criteria are selected. Classified as ground-level point clouds.

[0116] Unlike traditional SLAM methods that use ground segmentation algorithms and only output ground / non-ground binary classification results without retaining detailed geometric and statistical features, this embodiment extracts ground descriptor information after ground segmentation to construct subsequent constraints, improving robustness in the complex environment of substations. Furthermore, this embodiment constructs ground descriptors by filling ground point cloud information into GroundBininfo, including: normal vector, elevation, flatness, roughness, and confidence, forming a reusable descriptor layer. The specific form is as follows:

[0117] ,

[0118] Therefore, this embodiment achieves point cloud discretization through spatial partitioning, and calculates multi-dimensional features such as normal vectors and elevations based on Z-value sorting and seed point iterative fitting of the ground plane. Reliable ground grids are selected by threshold judgment and their information is stored to provide prior information constraints for subsequent dynamic point cloud removal and loop closure detection optimization. Finally, the threshold is adaptively updated using historical data to achieve robust ground-non-ground point cloud segmentation.

[0119] Then, for dynamic clustering removal based on ground normal vectors, in this embodiment, to ensure clustering integrity, the input point cloud cluster set is first processed. Each cluster in Determine if the number of points meets the minimum threshold, remove all clusters with fewer than 3 points to obtain the effective cluster set. ,in, .

[0120] In this embodiment, each effective cluster after preprocessing can be dually verified by geometric feature judgment and temporal feature judgment, making full use of the prior information obtained from the ground descriptor, and adjusting the discrimination criteria according to the specific ground conditions, which significantly improves the purity of SLAM map, suppresses dynamic occlusion error, and reduces the computational load of subsequent feature extraction.

[0121] It should be noted that, in this embodiment, dynamic clustering removal based on ground normal vectors may include geometric feature judgment based on ground bins and clustering attributes, as well as dynamic judgment based on temporal consistency.

[0122] Specifically, for geometric feature judgment based on ground bins and clustering attributes, this embodiment is designed based on the significant geometric differences between dynamic and static targets. Dynamic targets, such as pedestrians and vehicles, typically have fixed dimensions, and the angle between the shape and orientation of dynamic targets and the ground normal vector direction may exceed a threshold. Furthermore, the roughness of the ground also affects the accuracy of the judgment, requiring dynamic adjustment of the rules. Therefore, the most relevant ground unit below the cluster is first identified. Then, based on the characteristics of this ground unit, the judgment threshold is dynamically adjusted. Finally, the clustering is verified from three dimensions: size, height, and normal vector direction, to determine whether it conforms to the geometric rules of dynamic targets. Therefore, the specific implementation process of geometric feature judgment based on ground bins and clustering attributes is as follows:

[0123] First, the DBSCAN algorithm is used to cluster the non-terrestrial point cloud. To improve the real-time performance of the neighborhood calculation step, a KD-tree is innovatively introduced to accelerate neighborhood computation, progressively selecting core points and iteratively expanding the clusters. The final output is a set of clusters. The cluster's geometric properties (size, center height, normal vector) are combined with the local features (normal vector, elevation, roughness) of the most relevant ground bin below the cluster to determine whether the cluster conforms to the geometric characteristics of the dynamic target. First, the clusters are calculated. center point : , , .

[0124] The normal vectors of the clusters are obtained by SVD decomposition using the method described above. .

[0125] Iterate through all reliable bins, calculate the distance, and use the angle factor to filter the most relevant bins based on the overall score. The distance is: ,in, Here are the coordinates of the center point of bin; the angle factor is: ,in, The angle between the line connecting the cluster center and the bin center and the z-axis; the overall score is: Here, the bin with the highest score can be selected as the most relevant ground bin for that point cloud cluster.

[0126] In this embodiment, in order to adapt to the complex ground conditions of substations in Northwest China, the ground can be classified into flat ground, relatively rough ground, and rough ground based on the roughness extracted from the ground descriptor. For scenarios with high ground roughness, a more lenient discrimination standard is used. The classification table of different roughness grounds is shown in Table 1.

[0127]

[0128] In this embodiment, the geometric threshold is designed based on the actual height and width of common engineering vehicles and workers. The length, width, and height of the clustered point cloud are calculated using boundary coordinates to determine whether the shape of the point cloud "resembles a person / engineering vehicle." Human dimensions: 1.0~2.0 meters high, 0.1~1.2 meters wide and thick (with sensitivity adaptive); vehicle dimensions: 0.5~2.5 meters high, 1.5~1.2 meters wide and thick. Furthermore, to distinguish it from cables and distribution networks that lie close to the ground, focusing only on protruding objects with obvious features above the ground, the dynamic point cloud clusters must have an elevation difference greater than 0.1 meters from the ground bin.

[0129] For dynamic judgment based on temporal consistency, static objects such as large substations and transmission towers maintain their fixed spatial positions, so their point cloud clusters consistently appear in relatively constant spatial locations across multiple frames. Conversely, the positions of dynamic objects (such as pedestrians and vehicles) change over time, and their corresponding target clusters "disappear" or shift significantly in historical frames. If no matching points (within 50cm) that "basically overlap" with the current cluster can be found in the recent historical point cloud frames, the cluster is considered "temporally inconsistent" and judged as dynamic.

[0130] Therefore, in this embodiment, for the current input clustered point cloud Maintain a sliding window to store historical frames: Furthermore, this embodiment uses a KD-tree to perform a two-step nearest neighbor fast search within the most recent 5 frames of historical point clouds to find the point that is spatially closest to the current cluster center. First, it checks if there is a point whose Euclidean distance to the current point cloud is within 0.5 meters. If found, the current cluster is considered to be a static target as it highly overlaps with the historical frame point cloud. If the radius search fails, a single nearest neighbor distance search is used, and the minimum distance is recorded. If no historical point is found within a radius of 0.5 meters in all historical frames to be examined, and the minimum distance in all historical frames is greater than 1.0 meter, then the current cluster is finally determined to be a dynamic object.

[0131] Finally, for the optimization of loop closure detection under ground information constraints, in this embodiment, in order to adapt to multiple loop closures in the inspection path, the present invention introduces ground information (ground normal vector, ground roughness) as constraints in the loop closure detection part of the LIO SAM algorithm, and filters candidate loop closure frames to reduce the computational amount of loop closure frame matching while avoiding misjudgment and improving the accuracy of loop closure detection.

[0132] Therefore, in this embodiment, pose filtering is first performed based on the current keyframe to exclude historical frames that cannot overlap. Let the current keyframe be Fcurr, its estimated pose be Tcurr, and the set of historical keyframes be... Calculate the translation distance between the current frame and the historical frames sequentially: Only retain Historical frames shorter than 10 meters are considered to potentially overlap with the current frame spatially. A minimum time threshold is set to avoid repeatedly matching adjacent frames within a short period. The time limit is 20 seconds, only retaining frames with a time difference exceeding 20 seconds from the current frame. Candidate loopback frames are used to reduce redundant calculations.

[0133] It should be noted that in this embodiment, during coarse feature matching, given the lack of degraded corner and face features in the point cloud of the scene, ground flatness and ground normal vectors from historical frame ground descriptors are extracted first and matched with the current keyframe. The principle is based on ground consistency; the ground between loopback frames is generally consistent, meaning the ground flatness and normal vector directions are relatively consistent. A scoring function is designed by comparing ground information to select only the top few frames with the most similar descriptor structures as input for feature matching. Then, feature matching is repeated on the selected candidate frames with the current estimated pose. pose of candidate frames Calculate the transformed nearest neighbor distance for the initial value, and count the number of matching pairs whose distance is less than the threshold. (Number of edge point matches) (Number of matching points in a plane). If If the candidate frame does not match the current frame structure, it is excluded; otherwise, the fine matching stage begins.

[0134] It should be noted that this embodiment uses voxel filtering to downsample the point cloud in the fine matching process, reducing the number of point clouds and accelerating the iterative ICP speed. Based on the coarse matching obtained above... The initial values ​​are optimized using point-to-surface ICP (Inter-Positioning Calculation). The objective function is to minimize the position of the points in the current frame. With the plane after the candidate frame transformation The distance, that is: ,in Given the rotation matrix and relative translation to be optimized, , The normal vector of the candidate frame plane and the constant term of the plane equation are used. The iteration stops when the pose transformation between two adjacent iterations is less than the threshold or the number of iterations reaches the upper limit, thus obtaining the optimized relative pose. .

[0135] Finally, the average error after ICP matching is calculated: .

[0136] In this embodiment, if the average error is less than a threshold, the loop closure is determined to be valid, and this is used as the loop closure factor (the relative pose constraints between the current frame and the loop closure frame satisfy...). A sliding window is added to the backend of factor graph optimization to correct the global pose. In pose optimization, the sum of the residuals of all factors is minimized using the Gauss-Newton method to solve for the optimal pose, which counteracts the cumulative drift and ensures global consistency.

[0137] Example 4

[0138] Based on Example 3, this example verifies a substation inspection SLAM method based on ground descriptors. See [link to example]. Figure 2 The KITTI dataset 07 sequence trajectory comparison diagram shown in this embodiment demonstrates that, after testing with the KITTI dataset 07 sequence, the trajectory error of this embodiment can be controlled within 0.5m under complex environments.

[0139] Furthermore, this embodiment achieves dual optimization in global positioning accuracy and inter-frame motion consistency compared to the original LIO SAM algorithm. The comparison data between the method of this embodiment and the original LIO SAM algorithm are shown in Table 2. The simultaneous reduction of absolute pose error (APE) and relative pose error (RPE) verifies the comprehensive effectiveness of the improved scheme.

[0140]

[0141] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A substation inspection SLAM method based on ground descriptors, applied to an inspection robot equipped with a multi-line lidar and an inertial measurement unit, wherein the inspection robot is configured with a computing unit to execute the method, characterized in that, Includes the following steps: Acquire lidar point cloud data of the substation environment, and establish a multi-layer concentric circle partitioning model based on the point cloud density of the lidar point cloud data; The ground is segmented using a multi-layer concentric circle partitioning model, and a multi-dimensional ground descriptor is calculated. The ground descriptor includes ground normal vector, elevation, flatness, roughness, and confidence level. Establish a ground bin information map and store the calculated multidimensional ground descriptors; Non-ground point clouds are obtained based on a multi-layer concentric circle partitioning model, and the non-ground point clouds are clustered to obtain an effective cluster set. Based on the geometric features of ground bin information and clustering attributes, it is determined whether the effective cluster is a candidate for dynamic target, and the temporal consistency of the candidate for dynamic target is judged to identify the dynamic target and remove it from the non-ground point cloud. By constraining candidate loop closure frames during loop closure detection using multidimensional ground descriptors, loop closure detection is optimized, and the optimal pose of the inspection robot is obtained.

2. The substation inspection SLAM method based on ground descriptors according to claim 1, characterized in that, The established multi-layer concentric circle partitioning model is a four-layer concentric circle partitioning model; The ground segmentation using a multi-layer concentric circle partitioning model includes: using a four-layer concentric circle partitioning model to divide the multi-line lidar scanning area into four concentric circle regions according to angle and distance; The first layer of concentric circular regions has a radial range of 0-5m and is divided into 16 sectors, each sector containing 2 annular sub-regions. The second layer of concentric circular regions has a radial range of 5-10m and is divided into 32 sectors, each sector containing 4 annular sub-regions. The third layer of concentric circular regions has a radial range of 10-15m and is divided into 54 sectors, each sector containing 4 annular sub-regions. The fourth layer of concentric circular regions has a radial range of 15-20m and is divided into 32 sectors, each sector containing 4 annular sub-regions.

3. The substation inspection SLAM method based on ground descriptors according to claim 2, characterized in that, After performing ground segmentation using a multi-layer concentric circle partitioning model, and before calculating the multidimensional ground descriptor, the following steps are also included: The lidar point cloud data is distributed to the corresponding sectors through polar coordinate transformation.

4. The substation inspection SLAM method based on ground descriptors according to claim 3, characterized in that, After allocating the lidar point cloud data to the corresponding sectors via polar coordinate transformation, the process also includes: Set a threshold for the number of points in a sector, and filter the number of points in each sector, classifying sectors with fewer than the threshold into non-ground point clouds. For sectors that meet the point count requirement, sort them in ascending order of Z coordinate, select the first few points with the smallest Z coordinate as seed points, and obtain the seed point set; For each sector's seed point set, calculate the covariance matrix and perform singular value decomposition to solve for the normal vector of the ground plane; Based on the seed point, the ground segment is iteratively expanded. A distance threshold is set, and points whose distance to the current plane is less than the threshold are included in the ground segment until the distance of the newly added point to the plane exceeds the threshold. This process results in a ground point set and a non-ground point set.

5. The substation inspection SLAM method based on ground descriptors according to claim 4, characterized in that, The calculation of the multidimensional ground descriptor refers to: calculating the normal vector, elevation, flatness, roughness, and confidence of each sector based on the segmented set of ground points; The flatness is calculated using the residuals of the plane fitting, the roughness is calculated using the standard deviation of the Z coordinates of the ground points, and the confidence level is determined based on the number of ground points and the accuracy of the plane fitting.

6. The substation inspection SLAM method based on ground descriptors according to claim 4, characterized in that, The process of obtaining non-ground point clouds based on a multi-layer concentric circle partitioning model and then clustering these non-ground point clouds to obtain an effective cluster set refers to: For the non-ground point set obtained from the segmentation, the DBSCAN algorithm is used for clustering, and the clustering parameters are set, with the neighborhood radius set to 0.3m and the minimum number of points set to 3. Introducing a KD-tree into the neighborhood calculation step accelerates neighborhood search; The clustering results are filtered, and clusters with fewer than 3 points are removed to obtain a set of valid clusters.

7. A substation inspection SLAM method based on ground descriptors according to claim 6, characterized in that, The geometric feature judgment based on ground bin information and clustering attributes to determine whether a valid cluster is a dynamic target candidate refers to: For each valid cluster in the valid cluster set, calculate the cluster centroid and normal vector; Traverse the ground bin information map and find the most relevant ground bin below the cluster; The ground is classified according to the roughness of the most relevant ground bins. The corresponding geometric threshold is used to determine whether the clustering conforms to the geometric characteristics of dynamic targets from three dimensions: size, height, and normal vector direction, and to preliminarily identify dynamic target candidates.

8. A substation inspection SLAM method based on ground descriptors according to claim 7, characterized in that, The step of performing temporal consistency judgment on dynamic target candidates, identifying dynamic targets, and removing them from the non-ground point cloud refers to: Maintain a sliding window to store the clustering results of the most recent 5 historical point clouds; For clusters that are identified as candidates for dynamic targets based on geometric features, a KD tree is used to perform a fast nearest neighbor search in the most recent 5 frames of historical point clouds to find if there is a historical cluster whose Euclidean distance to the current cluster center is within 0.5m. If no such cluster is found, and the minimum distance in all historical frames is greater than 1.0m, then the cluster is identified as a dynamic target and removed from the non-ground point cloud.

9. A substation inspection SLAM method based on ground descriptors according to any one of claims 1-8, characterized in that, The step of constraining candidate loop closure frames during loop closure detection based on multidimensional ground descriptors to optimize loop closure detection and obtain the optimal pose of the inspection robot includes the following steps: Key frames are extracted based on IMU data from multi-line lidar and lidar data from inertial measurement unit. For the current key frame, the translation distance and time difference between it and historical key frames are calculated. Historical key frames with a translation distance of less than 10m and a time difference of more than 20 seconds are selected as candidate loopback frames. Extract the ground descriptors of candidate loop closure frames and the current keyframe, compare the ground flatness and normal vectors of the two, design a scoring function, and select the top 5 candidate loop closure frames with the highest scores as objects for further matching. For the filtered candidate loop closure frames, extract edge point and planar point features, and calculate the number of feature matching pairs between the current keyframe and the candidate loop closure frames. If the number of matching pairs is less than the threshold, exclude the candidate loop closure frame and complete the coarse feature matching of the candidate loop closure frames. For candidate loop closure frames that pass the coarse feature matching, voxel filtering is used to downsample the point cloud. The pose obtained from the coarse feature matching is used as the initial value, and the relative pose is optimized using the point-to-surface ICP algorithm. The upper limit of the number of iterations and the threshold of pose transformation are set. When the iteration stopping condition is met, the average error after ICP matching is calculated. If the average error is less than the threshold, the loop closure is determined to be valid and is added as a loop closure factor to the factor graph optimization. The effective closure factor, IMU pre-integration factor, and lidar odometry factor are incorporated into the factor graph, and the optimal solution of the factor graph is solved using CeresSolver to optimize the global pose.

Images