Power grid pipe corridor inspection-oriented positioning mapping method and electronic device

Abstract

The application discloses a positioning and mapping method for power grid pipe corridor inspection and electronic equipment. The method comprises the following steps: performing feature extraction on the current frame original point cloud of the power grid pipe corridor to obtain point cloud cluster data; performing integral processing on the current frame inertial measurement data of the power grid pipe corridor to obtain the predicted pose of the current frame; taking the predicted pose as the initial pose, and obtaining the laser observation pose by iterative optimization based on the point cloud cluster data; performing fusion optimization on the predicted pose and the laser observation pose by using an iterative extended Kalman filter to obtain the current key frame pose; and generating the positioning result of the mobile robot platform and the mapping result of the power grid pipe corridor based on the current key frame pose. The application solves the technical problems of serious positioning drift of the mobile robot and insufficient mapping accuracy caused by the accumulation error of a single sensor in the related art under the blocked global satellite navigation system and the feature degradation scene.

Description

Technical Field

[0001] This invention relates to the field of power facility inspection technology, and more specifically, to a positioning and mapping method and electronic equipment for power grid corridor inspection. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology is the core of mobile robots' autonomous navigation in unknown environments. However, in closed or semi-closed scenarios such as high-voltage power cable tunnels and underground utility tunnels, Global Navigation Satellite System (GNSS) signals are severely attenuated or completely unusable, forming typical GNSS-denied environments. In such environments, solutions relying on a single sensor have significant drawbacks: on the one hand, the Inertial Measurement Unit (IMU) has zero-bias noise, and long-term integration leads to a rapid accumulation of trajectory estimation errors, resulting in severe positioning drift; on the other hand, utility tunnel scenarios are characterized by numerous long straight passages, repetitive support structures, and degraded geometric features, making it difficult for lidar to extract effective geometric constraints in feature-sparse or missing areas, leading to registration failures or jumps. The methods in related technologies fail to fully leverage the complementary advantages of multi-source data at the state estimation level, making it difficult to maintain high-precision pose estimation under features degradation and strong noise interference. This ultimately leads to insufficient positioning accuracy and distorted map construction for mobile robots during utility tunnel inspections, affecting the reliability of subsequent intelligent inspection tasks. As can be seen, in related technologies, single sensors are prone to cumulative errors under scenarios where global satellite navigation systems are blocked or features are degraded, easily resulting in severe positioning drift and insufficient mapping accuracy for mobile robots.

[0003] There is currently no effective solution to the above problems. Summary of the Invention

[0004] This invention provides a positioning and mapping method and electronic device for power grid pipeline inspection, which at least solves the technical problems in related technologies, such as serious positioning drift and insufficient mapping accuracy of mobile robots caused by the cumulative error that easily occurs when a single sensor is blocked or features are degraded in the global satellite navigation system.

[0005] According to one aspect of the present invention, a positioning and mapping method for power grid utility tunnel inspection is provided, comprising: receiving a current frame original point cloud collected by a solid-state lidar sensor mounted on a mobile robot platform during the current sampling period for the power grid utility tunnel, and current frame inertial measurement data collected by an inertial detection unit mounted on the mobile robot platform during the current sampling period for the power grid utility tunnel; performing feature extraction based on the current frame original point cloud to generate point cloud cluster data; performing integration processing based on the current frame inertial measurement data to obtain the predicted pose of the current frame; using the predicted pose as the initial pose, iteratively optimizing the lidar observation pose of the current frame original point cloud in the local map coordinate system based on the point cloud cluster data; performing fusion optimization using an iterative extended Kalman filter based on the predicted pose and the lidar observation pose to obtain the current key frame pose; and generating the positioning result of the mobile robot platform and the mapping result of the power grid utility tunnel based on the current key frame pose.

[0006] According to another aspect of the present invention, a non-volatile storage medium is also provided, the non-volatile storage medium storing a plurality of instructions, the instructions being adapted to be loaded by a processor and executed by any one of the positioning and mapping methods for power grid utility tunnel inspection.

[0007] According to another aspect of the present invention, an electronic device is also provided, including one or more processors and a memory, the memory being used to store one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to implement any one of the positioning and mapping methods for power grid tunnel inspection.

[0008] According to another aspect of the present invention, a computer program product is also provided, including a computer program that, when executed by a processor, implements the steps of the positioning and mapping method for power grid utility tunnel inspection as described in any one of the claims.

[0009] In this embodiment of the invention, the system receives the original point cloud of the current frame collected by the solid-state lidar sensor mounted on the mobile robot platform during the current sampling period for the power grid utility tunnel, and the inertial measurement data of the current frame collected by the inertial detection unit mounted on the mobile robot platform during the current sampling period for the power grid utility tunnel. Based on the original point cloud of the current frame, feature extraction is performed to generate point cloud cluster data. Based on the inertial measurement data of the current frame, integration processing is performed to obtain the predicted pose of the current frame. Based on the point cloud cluster data, using the predicted pose as the initial pose, the laser observation pose of the original point cloud of the current frame in the local map coordinate system is solved through iterative optimization. Based on the predicted pose and the laser observation pose, an iterative extended Kalman filter is used for fusion optimization to obtain the current target position. Keyframe pose: Based on the current keyframe pose, the localization result of the mobile robot platform and the mapping result of the power grid corridor are generated. This achieves the goal of determining the keyframe pose by tightly coupling and fusing the IMU pre-integration predicted pose and the laser point cloud registration observation pose under the framework of iterative extended Kalman filter. Based on the keyframe pose, the mobile robot platform is accurately localized and mapped. This improves the real-time positioning accuracy and mapping accuracy of the mobile robot in the complex environment of the power grid corridor. It also solves the technical problem in related technologies that the mobile robot's positioning drift and mapping accuracy are serious due to the cumulative error caused by the single sensor in scenarios where the global satellite navigation system is blocked and features are degraded. Attached Figure Description

[0010] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings:

[0011] Figure 1 This is a flowchart of a positioning and mapping method for power grid utility tunnel inspection according to an embodiment of the present invention;

[0012] Figure 2 This is an optional robot localization and mapping flowchart according to an embodiment of the present invention;

[0013] Figure 3 This is a flowchart of an optional positioning and mapping method for power grid utility tunnel inspection according to an embodiment of the present invention;

[0014] Figure 4 This is a schematic diagram of a positioning and mapping device for power grid utility tunnel inspection according to an embodiment of the present invention;

[0015] Figure 5 This is a schematic diagram of an electronic device according to an embodiment of the present invention. Detailed Implementation

[0016] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0017] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0018] According to an embodiment of the present invention, a method for positioning and mapping for power grid utility tunnel inspection is provided. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0019] Figure 1 This is a flowchart of a positioning and mapping method for power grid utility tunnel inspection according to an embodiment of the present invention, such as... Figure 1 As shown, the method includes the following steps:

[0020] Step S102: Receive the original point cloud of the current frame collected by the solid-state lidar sensor on the mobile robot platform for the power grid tunnel during the current sampling period, and the inertial measurement data of the current frame collected by the inertial detection unit on the mobile robot platform for the power grid tunnel during the current sampling period.

[0021] The execution entity for steps S102 to S112 can be an edge computing device. Power grid tunnels refer to environments such as high-voltage cable tunnels, underground integrated utility tunnels, and underground mezzanines of substations. Power grid tunnels typically lack satellite signals, making absolute positioning using GPS impossible. Structurally, they are characterized by numerous long, straight passages, repetitive support structures and cable trays, narrow spaces, complex lighting, and dust interference, requiring high-precision maps to correlate inspection tasks and locate defective equipment.

[0022] This step begins with the acquisition and synchronization of raw data from multiple sensors on a mobile robot platform during a power grid tunnel inspection scenario. Specifically, the edge computing device receives the raw point cloud data of the current frame collected by a solid-state LiDAR sensor mounted on the mobile robot platform within the current sampling period, and simultaneously receives the inertial measurement data of the current frame collected by an inertial measurement unit (IMU) mounted on the same mobile robot platform within the same current sampling period. Here, the solid-state LiDAR is responsible for acquiring the geometric structure information of the tunnel space, forming a raw point cloud sequence, while the IMU is responsible for acquiring the robot's attitude, angular velocity, acceleration, and other motion state information, forming an inertial measurement sequence. By aligning these two types of heterogeneous sensor data to the same current sampling period in the time dimension, a time synchronization and data foundation is laid for subsequent motion distortion correction of the LiDAR point cloud using inertial data and for achieving tight coupling and fusion positioning of LiDAR and inertial data.

[0023] Optionally, sensor data acquisition is performed based on a low-power airborne platform. This low-power airborne platform can be an edge computing device mounted on a mobile carrier such as a drone or quadruped robot, limited by battery capacity, heat dissipation conditions, and payload weight. The low-power airborne platform can receive the current frame's raw point cloud acquired by the solid-state LiDAR sensor mounted on the mobile robot platform (drone or quadruped robot), and the current frame's inertial measurement data acquired by the inertial measurement unit, converting them into an internal unified data structure and caching them by timestamp. Specifically, the method program of this embodiment is deployed on a mobile robot platform (such as a drone or quadruped robot platform) equipped with a solid-state LiDAR sensor and an IMU, and specifically runs on an airborne low-power edge computing device. After the system starts, it reads the inspection task configuration, platform type configuration, LiDAR topic, IMU topic, and the extrinsic parameter matrix from the LiDAR to the IMU. The extrinsic parameter matrix is ​​a 4x4 homogeneous transformation matrix, describing the rigid body transformation relationship from the LiDAR coordinate system to the IMU coordinate system. Upon receiving IMU messages, the data is converted into a data structure supported by the internal IMU, including timestamps, three-axis acceleration, and three-axis angular velocity, and written to the circular buffer queue of the IMU data retrieval unit. The IMU data retrieval unit supports efficient querying by timestamp range, providing time-synchronized IMU data sequences for subsequent distortion correction, motion state recognition, and IMU pre-integration. Upon receiving LiDAR point cloud messages, the pointer to the current frame's raw point cloud is written to the raw point cloud queue, and a condition variable is used to notify the preprocessing thread to retrieve new data. The current frame's raw point cloud contains the three-dimensional coordinates, reflection intensity, and relative timestamps within the point for the power grid corridor during the current sampling period; if the relative timestamps within the point are missing, the point acquisition time can be estimated based on the solid-state LiDAR scanning model. Motion disturbance parameters are read according to the type of mobile robot platform. For example, for UAV platforms, angular velocity thresholds related to hovering jitter, rapid turning, and airflow disturbance are recorded; for quadruped robot platforms, acceleration thresholds related to gait periodic impacts, fuselage pitch vibration, and stop-and-go switching are recorded.

[0024] Step S104: Extract features based on the original point cloud of the current frame to generate point cloud cluster data.

[0025] In this step, raw point cloud data from the sensor is received, and feature extraction is performed on this raw point cloud. Specifically, this process aims to identify and separate a set of points with significant geometric features from the raw point cloud, which includes attributes such as 3D coordinates, reflection intensity, and relative timestamps. By extracting representative geometric features and controlling the amount of data, this step provides a preprocessed and streamlined data foundation for subsequent pose estimation and mapping, ensuring efficient feature matching and state estimation on a low-power airborne platform while preserving critical information sensitive to the structure of the utility tunnel scene.

[0026] In one optional embodiment, feature extraction is performed based on the original point cloud of the current frame to generate point cloud cluster data, including: performing distortion correction on the original point cloud of the current frame based on the inertial measurement data of the current frame to obtain the corrected point cloud; and performing feature processing on the corrected point cloud to obtain point cloud cluster data.

[0027] In this embodiment, the specific execution path of point cloud preprocessing is first clarified. High-frequency attitude change data collected by the inertial detection unit is used to reconstruct the minute displacements and rotations of the lidar within a single scan cycle, thereby correcting the distortion of the original point cloud caused by robot motion into a geometric shape under a unified coordinate system, resulting in a structurally complete corrected point cloud. Subsequently, based on this geometrically accurate point cloud data, significant features such as edges and corners are extracted and clustered to generate discrete point cloud cluster data for subsequent matching. By introducing inertial data to dynamically correct the distortion of the original point cloud, the stretching or compression of the point cloud caused by the mobile robot's own motion during inspection is effectively eliminated. This avoids noise and pseudo-features introduced by distorted point clouds in the feature extraction stage, ensuring that the data used for subsequent feature extraction has a true geometric topological relationship. This significantly improves the convergence speed and accuracy of subsequent iterative optimization to solve the laser observation pose, ultimately achieving high-precision positioning and mapping in the complex environment of the power grid corridor.

[0028] In one optional embodiment, distortion correction is performed on the original point cloud of the current frame based on the inertial measurement data of the current frame to obtain a corrected point cloud. This includes: for each point in the original point cloud of the current frame, querying the inertial measurement data corresponding to each point from the inertial measurement data of the current frame according to the acquisition time (i.e., timestamp) of each point; determining the pose transformation matrix of each point relative to the current frame reference time based on the corresponding inertial measurement data, wherein the pose transformation matrix is ​​used to indicate the spatial position and attitude relationship between the sensor coordinate system of the corresponding point at the acquisition time and the sensor coordinate system of the current frame reference time; and transforming the coordinates of each point from the sensor coordinate system of the corresponding acquisition time to the sensor coordinate system of the current frame reference time based on the pose transformation matrix to obtain the corrected point cloud.

[0029] In this embodiment, for each point in the original point cloud of the current frame, the corresponding inertial measurement data is queried from the inertial measurement data of the current frame based on the acquisition time of each point. This means utilizing the high-frequency sampling advantage of the inertial measurement unit to match the instantaneous angular velocity and linear acceleration data accurate to the microsecond level for each laser point discretely distributed on the time axis in the point cloud. Based on the corresponding inertial measurement data, the pose transformation matrix of each point's acquisition time relative to the current frame reference time is determined. The pose transformation matrix is ​​used to indicate the spatial position and attitude relationship between the sensor coordinate system at the acquisition time of the corresponding point and the sensor coordinate system at the current frame reference time. This step calculates the minute rotation and translation changes that the robot undergoes during the acquisition of a single frame of point cloud by integrating the inertial data, thereby quantifying the relative offset of the sensor coordinate system at different acquisition times. Using the pose transformation matrix, the coordinates of each point are transformed from the sensor coordinate system at the corresponding acquisition time to the sensor coordinate system at the reference time of the current frame, resulting in a corrected point cloud. Essentially, the calculated pose transformation is used to perform reverse compensation on the coordinates of each point cloud, eliminating the spatial distortion of the point cloud caused by robot motion. Subsequently, feature processing is performed on the corrected point cloud to obtain point cloud cluster data, ensuring that the extracted geometric features such as edges and planes reflect the true static structure of the environment rather than motion artifacts. By adopting the above steps and correcting motion distortion point by point, the deformation caused by the acquisition time difference within a single frame point cloud is effectively eliminated, making the subsequent feature extraction based on the point cloud cluster data more accurate, thereby improving the accuracy of laser observation pose solution and ultimately enhancing the reliability of the localization and mapping results after iterative extended Kalman filter fusion optimization.

[0030] Optionally, based on the corresponding inertial measurement data, the pose transformation matrix of each point's acquisition time relative to the current frame reference time is determined. This includes: integrating the inertial measurement data between the current frame reference time and the acquisition time of each point to calculate the rotation increment and translation increment of that point's acquisition time relative to the current frame reference time; and constructing the pose transformation matrix based on the rotation increment and translation increment. Through this method, high-frequency inertial data can be used to perform point-by-point motion distortion correction on laser point clouds, effectively eliminating point cloud deformation under severe motion conditions such as rapid UAV turning, hovering jitter, or quadruped robot gait impacts, thereby significantly improving the accuracy of subsequent point cloud registration and the robustness of the positioning system.

[0031] In one optional embodiment, feature processing is performed on the corrected point cloud to obtain point cloud cluster data, including: determining the local curvature of each point in the corrected point cloud; dividing the corrected point cloud into an edge feature point set and a planar feature point set according to preset edge curvature thresholds and planar curvature thresholds; performing voxel downsampling on the edge feature point set and the planar feature point set respectively to obtain downsampled edge feature points and downsampled planar feature points; filtering the downsampled edge feature points and downsampled planar feature points according to preset feature quantity upper limits to obtain filtered edge feature points and filtered planar feature points; and obtaining point cloud cluster data based on the corrected point cloud, the filtered edge feature points, and the filtered planar feature points.

[0032] In this embodiment, feature processing is performed on the corrected point cloud to obtain point cloud cluster data. Specifically, this includes: first, determining the local curvature of each point in the corrected point cloud, i.e., quantifying the surface curvature by calculating the geometric distribution of points in the neighborhood of the point cloud; then, based on preset edge curvature thresholds and planar curvature thresholds, dividing the corrected point cloud into an edge feature point set and a planar feature point set, where edge feature points refer to points with larger curvature reflecting changes in the edges or boundaries of objects, and planar feature points refer to points with smaller curvature reflecting smooth surfaces; subsequently, voxel downsampling is performed on the edge feature point set and the planar feature point set respectively, i.e., using a three-dimensional mesh to perform voxel downsampling on the point cloud. Spatial discretization removes redundant points within the same grid, yielding downsampled edge and planar feature points to reduce data volume and ensure uniform distribution. Then, based on a preset feature quantity limit, the downsampled edge and planar feature points are further filtered, retaining the most representative key points to obtain filtered edge and planar feature points. This prevents excessive feature points from wasting computational resources or causing registration noise. Finally, based on the corrected point cloud, the filtered edge and planar feature points, a point cloud cluster is obtained. This cluster contains structured features that have undergone distortion correction and refined filtering. Using these steps, motion distortion is eliminated through inertial data correction, ensuring the accuracy of point cloud geometry. Furthermore, curvature partitioning, voxel downsampling, and quantity limit filtering quantitatively extract highly discriminative edge and planar features, effectively avoiding distortion interference caused by rapid motion or vibration in the original point cloud. Simultaneously, it reduces the computational burden of redundant feature points, significantly improving processing efficiency and registration stability in subsequent iterative optimization for solving laser observation poses.

[0033] Optionally, point cloud preprocessing for solid-state non-repeating scanning LiDAR is performed. This involves IMU-based point-by-point motion distortion correction, range clipping, voxel downsampling, feature quantity budget control, and Lidar Odometry and Mapping (LOAM) feature extraction on the original point cloud of the current frame, outputting preprocessed point cloud cluster data. Specifically, this includes:

[0034] S1041, Point Cloud Format Conversion. The preprocessing thread waits for new data in the raw point cloud queue. After acquiring the raw point cloud of the current frame, it converts the raw point cloud of the current frame into a structured point cloud format (PointXYZIRT) containing three-dimensional coordinates (x, y, z), reflection intensity, scan line number, and relative time. For solid-state LiDAR sensors, the non-repeating scan point cloud is automatically adapted based on pre-registered sensor parameters.

[0035] S1042, IMU-based point-by-point motion distortion correction. The distortion corrector (LidarDistortionCorrector) is invoked, and the inertial measurement data sequence within the time range corresponding to the original point cloud of the current frame is queried through the IMU data retrieval tool. For each point within the frame, the IMU attitude at the corresponding time is found based on its relative timestamp. The rotation increment and translation increment of that point relative to the frame reference time are calculated, and the coordinates of that point are transformed from the acquisition time coordinate system to the frame reference time coordinate system, achieving point-by-point motion distortion correction. Specifically, for the i-th laser point within the frame... Its corrected coordinates for: ,in, The pose of the i-th point at the acquisition time is obtained from IMU integration. The pose at the reference frame time.

[0036] S1043, Motion Disturbance Identification. Calculate motion disturbance indices based on the IMU angular velocity variance, acceleration variance, and attitude change within the corresponding time window of the current frame. If identified as a rapid UAV turn, hovering jitter, or quadruped robot gait impact zone, increase the priority of point-by-point distortion correction for that frame, and add inertial prediction weights or restrict abnormal laser registration updates in subsequent registration processes.

[0037] S1044, Distance Clipping and Computation Budget Control. Based on the minimum and maximum usage distances (lidar_use_min_dist and lidar_use_max_dist) configured parameters, it filters out points that are too close or too far from the distance sensor, removing near-field noise and far-field sparse points. It dynamically adjusts the voxel downsampling size and the maximum number of feature points per frame based on the onboard computing platform load and the processing time of the previous frame, ensuring real-time processing capabilities on the low-power platform.

[0038] S1045, LOAM Feature Extraction. Calculate the curvature value of each point in the distorted point cloud. Based on curvature thresholds, divide the points into edge feature points (corner, curvature greater than the first curvature threshold corner_thres) and planar feature points (planar, curvature less than the second curvature threshold planar_thres, where the second curvature threshold is less than or equal to the first curvature threshold). Perform voxel raster downsampling on the edge and planar feature points respectively, and filter according to the upper limit of the number of edge and planar features to form point cloud cluster data (PointcloudCluster) containing edge features, planar features, the original distorted point cloud, motion perturbation indicators, and timestamps, and write it to the preprocessing output queue (cloud_cluster_deque).

[0039] Step S106: Perform integration processing based on the inertial measurement data of the current frame to obtain the predicted pose of the current frame.

[0040] In this step, the edge computing device acquires the inertial measurement unit (IMU) data sequence corresponding to the current frame and performs time-based cumulative integration using high-frequency acceleration and angular velocity measurements. By converting the inertial data within this time window into incremental changes in motion states such as rotation, translation, and velocity, and combining this with the state information from the previous moment, the estimated position, attitude, and velocity information of the robot relative to the reference coordinate system at the current moment are calculated. This process utilizes the measurement characteristics of inertial sensors within a short time frame to provide initial pose predictions for subsequent laser point cloud registration and state fusion, thereby helping to maintain the continuity and robustness of the positioning system when lidar features degrade or motion distortion occurs.

[0041] Step S108: Based on point cloud cluster data, using the predicted pose as the initial pose, the laser observation pose of the original point cloud in the local map coordinate system of the current frame is solved by iterative optimization.

[0042] This step aims to utilize the point cloud cluster data output from the preprocessing stage, combined with the predicted pose estimated by the front end, as the initial solution, to solve for the laser observation pose of the original point cloud in the local map coordinate system using an iterative optimization algorithm. Specifically, firstly, target point cloud clusters containing edge and planar features are acquired, and the predicted pose is used as the initial input for the registration algorithm. Subsequently, through an iterative optimization process, the geometric constraint relationship (such as the distance residual from point to line or point to surface) between the current frame point cloud features and the corresponding features in the local map is calculated, and the pose estimation value is continuously corrected to minimize these residuals until the convergence condition is met. This process directly achieves high-precision estimation of the robot's instantaneous pose, providing accurate laser vision observation constraints for subsequent inertial measurement unit pre-integration state updates and tightly coupled filtering optimization, thereby effectively improving the system's positioning accuracy and mapping consistency in the global coordinate system.

[0043] In one optional embodiment, based on point cloud cluster data and using the predicted pose as the initial pose, the laser observation pose of the original point cloud in the current frame in the local map coordinate system is solved by iterative optimization, including: searching for edge lines in the local map based on edge feature points in the point cloud cluster data, and calculating a first distance residual from the edge feature points to the edge lines; searching for planes in the local map based on planar feature points in the point cloud cluster data, and calculating a second distance residual from the planar feature points to the planes; with the goal of minimizing the first distance residual and the second distance residual, using the predicted pose as the initial pose, iteratively updating the initial pose through a target iterative optimization algorithm until a predetermined convergence condition is met, thereby obtaining the laser observation pose.

[0044] In this embodiment, when using the predicted pose provided by inertial navigation as the initial estimate for pose optimization, instead of relying solely on randomly distributed point clouds, the point cloud cluster data is first classified by geometric features to filter out edge feature points with significant directionality and planar feature points with obvious normal features. Subsequently, the geometric structures matching these feature points are retrieved in the local map. For edge feature points, the perpendicular distance from their position to the corresponding edge line in the local map is calculated to construct a first distance residual; for planar feature points, the perpendicular distance from their position to the corresponding plane in the local map is calculated to construct a second distance residual. This classification and calculation method can more accurately reflect the geometric constraint relationship between the point cloud and the map. Then, using... Starting with the predicted pose, the objective function is to minimize the sum of the first and second distance residuals. The pose parameters are continuously corrected through an iterative optimization algorithm (such as the Gauss-Newton method or the Levenberg-Marquardt algorithm) until the residual change is less than a predetermined threshold or the maximum number of iterations is reached, thus converging to obtain a high-precision laser observation pose. By introducing two highly stable geometric features, edge lines and planes, as constraints, this scheme effectively utilizes the abundant structured features such as walls and pipes in the power grid corridor environment. Compared with methods that only use ordinary point clouds, it significantly improves the accuracy and convergence stability of pose solving in scenarios with feature degradation or simple textures, thereby enhancing the robustness of the overall localization and mapping.

[0045] Step S110: Based on the predicted pose and the laser observation pose, an iterative extended Kalman filter is used for fusion optimization to obtain the current keyframe pose.

[0046] In this step, an iterative extended Kalman filter is first used to fuse and optimize the predicted pose and the laser observation pose to obtain the current keyframe pose. The predicted pose is calculated by the IMU pre-integrator based on the navigation state of the previous frame and the IMU data sequence within the current frame's time range, serving as a priori prediction of the current frame's state. The laser observation pose is obtained by the front-end module through iterative matching between the edge features and planar feature point cloud of the current frame and the local registration map. The iterative extended Kalman filter uses IMU pre-integration as the state propagation model and the laser pose registration result as the observation update model, tightly coupling and fusing the two through iterative optimization to output the optimal navigation state estimate, including position, velocity, attitude, and IMU bias—that is, the current keyframe pose.

[0047] In one optional embodiment, the current keyframe pose is obtained by fusing and optimizing the predicted pose and the laser observation pose using an iterative extended Kalman filter, including: initializing the current state estimate of the iterative extended Kalman filter using the predicted pose; iteratively updating the current state estimate based on the laser observation pose; during the iterative update process, detecting the consistency between the laser observation pose and the current state estimate; if the consistency is found to meet a preset condition, updating the current state estimate based on the laser observation pose; if the consistency is found to not meet the preset condition, limiting the contribution of the laser observation pose to the update of the current state estimate; and obtaining the current keyframe pose based on the current state estimate obtained after the iterative update.

[0048] In this embodiment, initializing the current state estimate of the iterative extended Kalman filter using the predicted pose means using the predicted pose obtained by integrating the inertial measurement data as the initial value of the filter's internal state variables, providing a reliable benchmark reference for subsequent state correction. Iteratively updating the current state estimate based on the laser observation pose means using the pose calculated by lidar feature matching as the observation input, continuously correcting the state estimate through an iterative process to approximate the true pose. During the iterative update process, detecting the consistency between the laser observation pose and the current state estimate means comparing the degree of difference between the current laser observation pose and the current state estimate obtained based on historical state evolution and inertial prediction in real time, thereby judging the reliability of the laser observation data. For example, in scenarios with repetitive structures or motion disturbances (such as robot gait impacts) such as power grid corridors, if mismatch occurs in laser point cloud registration, it will cause the observed pose to deviate from the true trajectory. In this case, consistency detection can identify such deviations exceeding the normal range. If the consistency is detected to meet preset conditions, then... The current state estimate is updated based on laser observation pose. When the laser observation and state estimate match well, the observation data is considered reliable, and the filter normally absorbs the observation information to optimize positioning accuracy. If the consistency does not meet the preset conditions, the contribution of the laser observation pose to the update of the current state estimate is limited. That is, when anomalies or unreliability of laser observations are identified, the correction effect of the abnormal observation on the state estimate is weakened or even blocked through mechanisms such as weighted attenuation or truncation, preventing erroneous data from polluting the state of the filter. The current keyframe pose obtained based on the current state estimate after the iterative update is the final state after consistency verification and iterative optimization, which serves as the reliable positioning result at the current moment. By introducing the above consistency detection and contribution limitation mechanism, this scheme can automatically identify and suppress unreliable laser observation data when laser registration fails due to environmental feature degradation or motion disturbance, avoiding positioning drift or accuracy reduction. This significantly improves the robustness and stability of mobile robot localization and mapping in complex power grid and pipeline inspection scenarios.

[0049] It should be noted that, under normal circumstances, laser observations are considered "reliable." The algorithm assigns a high weight (or a small covariance value) to laser observations, and the state estimation largely aligns with the results of the laser observations. After limiting contributions, if the algorithm detects that a laser observation may be unreliable (e.g., due to incomplete correction of motion distortion, feature degradation, or mismatch), it reduces the weight of the laser observation (or increases its covariance value) to achieve the limiting effect. This effectively suppresses the negative interference of abnormal laser observations caused by motion distortion, feature degradation, or mismatch on the state estimation, preventing drastic jumps or divergences in the localization results, thereby significantly improving the robustness and localization continuity of the algorithm in complex dynamic environments and scenarios with weak geometric features.

[0050] In one optional embodiment, the current keyframe pose is obtained by fusing and optimizing the predicted pose and the laser observation pose using an iterative extended Kalman filter, including: fusing and optimizing the predicted pose and the laser observation pose using an iterative extended Kalman filter to obtain the current frame pose; determining the translation increment and rotation increment of the current frame pose relative to the previous keyframe pose, wherein the previous keyframe pose is the keyframe pose most recently generated from the current sampling period; and using the current frame pose as the current keyframe pose if the translation increment is greater than a preset translation limit or the rotation increment is greater than a preset rotation limit.

[0051] In this embodiment, determining the translation and rotation increments of the current frame pose relative to the previous keyframe pose involves spatially comparing the current frame pose obtained after iterative extended Kalman filter fusion optimization with the keyframe pose most recently generated in the current sampling period, calculating the change in straight-line distance (translation increment) and the change in pose angle (rotation increment) in three-dimensional space. If the translation increment is greater than a preset translation limit or the rotation increment is greater than a preset rotation limit, the current frame pose is taken as the current keyframe pose. Specific displacement and angle thresholds are set as judgment criteria, and the determination is made only when the machine... The current frame is considered to have sufficient information representativeness and is established as a new keyframe only when the robot's movement distance between the current frame and the previous keyframe exceeds the preset translation limit or its turning angle exceeds the preset rotation limit; otherwise, the keyframe update step is skipped. This dynamic filtering mechanism based on the amplitude of pose change can effectively filter out redundant data frames generated when the robot makes fine adjustments or small movements in place, ensuring that the generated mapping keyframes can truly reflect the robot's significant pose changes. This reduces data storage redundancy while improving the efficiency of power grid corridor inspection mapping and the real-time performance and accuracy of the positioning results.

[0052] Optionally, the preset translation and rotation limits are dynamically adjusted based on the motion state of the mobile robot platform and the computational load of the edge computing device. The specific implementation process is as follows: The motion state and computational load are normalized to obtain normalized motion state and normalized computational load; a weighted summation is performed on the normalized motion state and normalized computational load to obtain a target score value; the preset translation and rotation limits are determined based on the target score value. For example, when the mobile robot is detected to be in a state of rapid turning or violent movement caused by gait impact, and the edge computing device load is at a moderate level, the preset translation and rotation limits are reduced to retain more keyframe constraints; while when the computational load is close to its peak and the motion state is stable, the above limits are increased to reduce the generation of redundant keyframes, thereby balancing positioning accuracy and computational resource consumption. Through this method, while ensuring positioning robustness under complex motion conditions, it is possible to dynamically adapt to the computational bottleneck of the low-power edge platform, avoiding positioning jumps or system lag caused by ground vibration, high-frequency vibration, or computational overload, and ensuring the real-time performance and stability of the inspection robot during long-term operation.

[0053] Optionally, perform laser-inertial navigation fusion front-end mapping for motion disturbances on the mobile robot platform, fusing the predicted pose obtained from IMU pre-integration prediction and the laser observation pose obtained from point cloud registration observation, estimating the current frame pose through an iterative extended Kalman filter (IESKF), and dynamically determining whether the current frame pose is a keyframe based on displacement, rotation, vibration intensity, and computational load, specifically including:

[0054] S11, Initialization. After the FrontEnd starts, it initializes the point cloud registration interface, IMU pre-integration, IESKF, and local registration map. The registration interface supports multiple registration modes, including point-to-surface registration based on LOAM features (K-Dimensional Tree (KD-Tree) or Incremental Voxel (iVox)), Normal Distributions Transform (NDT) registration, and Generalized Iterative ClosestPoint (GICP) registration, which can be selected through configuration parameters.

[0055] S12, IMU pre-integration state prediction. After obtaining the new frame point cloud cluster data from the preprocessing output queue, IMU pre-integration is performed based on the navigation state of the previous frame (NavStateData, containing position, velocity, attitude quaternions, acceleration deviation, and angular velocity deviation) and the IMU data sequence within the current frame time range (i.e., the current frame inertial measurement data). The pre-integration result serves as the prior prediction of the current frame state, i.e., the predicted pose, and the corresponding formula is as follows:

[0056] ;

[0057] ;

[0058] ;

[0059] in, This represents the predicted pose (rotation matrix) value for the current frame k; This represents the predicted velocity value for the current frame k. This represents the predicted position value of the current frame k; k-1 represents the previous frame of the current frame k. Indicates the pose of the previous frame; Indicates the speed of the previous frame; Indicates the position of the previous frame; g is the gravitational acceleration vector; This indicates the time interval between the current frame and the previous frame; The rotation pre-integral quantity represents the relative rotation obtained by integrating the IMU angular velocity during the time interval from the previous frame to the current frame. It describes the change in the robot's posture. The velocity integral represents the change in relative velocity obtained by integrating the IMU specific force (acceleration minus gravity projection) over the time interval from the previous frame to the current frame. The position pre-integration quantity represents the relative position change obtained by IMU comparison double integration during the time period from the previous frame to the current frame.

[0060] S13, Laser Point Cloud Registration. The registration tool is invoked to register the current frame's point cloud (edge ​​feature points and planar feature points) with the local registration map. The registration tool uses the predicted pose pre-integrated by the IMU as its initial value and iteratively optimizes to solve for the laser observation pose of the current frame in the map coordinate system. During registration, the distance residuals from edge feature points to the nearest edge line and from planar feature points to the nearest plane are calculated. These residuals are iteratively minimized using a Gauss-Newton or Levenberg-Marquardt (LM) algorithm to obtain the laser observation pose T_lidar.

[0061] S14, IESKF Fusion Optimization. The pose observations obtained from laser registration are fused and optimized with IMU pre-integration predictions within the IESKF framework. IESKF uses IMU pre-integration as the state propagation model and laser pose registration results as the observation update model, obtaining the optimal navigation state estimate for the current frame through iterative optimization. For frames where motion disturbance indices exceed a threshold, abnormal laser observation updates are restricted based on residual consistency detection results to avoid short-term misregistrations that could disrupt the state estimate due to UAV jitter or quadruped robot impacts.

[0062] S15, Keyframe Determination. Calculate the relative transformation Delta_T between the current frame pose and the previous keyframe pose, and extract the translation increment Delta_d and rotation increment Delta_theta. If Delta_d > d_threshold or Delta_theta > theta_threshold, the current frame is determined to be a keyframe. Furthermore, the preset translation upper limit d_threshold and preset rotation upper limit theta_threshold can be dynamically adjusted based on motion perturbation indicators and computational load: appropriately lowering the threshold in high-speed turning or structural degradation regions to retain more constraints, and appropriately raising the threshold when low-power load is too high to reduce redundant keyframes.

[0063] Step S112: Based on the current keyframe pose, generate the localization result of the mobile robot platform and the mapping result of the power grid corridor.

[0064] In this step, based on the current keyframe pose, the localization result of the mobile robot platform and the mapping result of the power grid tunnel are generated. In this embodiment, the focus is first on utilizing the current keyframe pose data to output the real-time position information of the mobile robot platform in the environment and the three-dimensional spatial structure of the explored area. By acquiring the keyframe pose determined during system operation, this pose data is directly applied to the localization calculation of the mobile robot platform, thereby obtaining the current localization result of the platform; simultaneously, using the keyframe pose and its associated point cloud data, map data of the corresponding power grid tunnel environment is constructed and generated, forming the mapping result. Here, the keyframe pose reflects the spatial state of the robot at a specific moment, is the direct basis for localization output, and is also the basic element for fusing local point clouds into the global coordinate system to form a complete tunnel map. By outputting the localization result, the specific location of the mobile robot platform in the power grid tunnel is clarified; by outputting the generated mapping result, the three-dimensional geometric information of the power grid tunnel environment is provided.

[0065] In one alternative embodiment, Figure 2 This is a schematic diagram of a positioning and mapping device for power grid utility tunnel inspection according to an embodiment of the present invention, as shown below. Figure 2As shown, based on the current keyframe pose, the localization results of the mobile robot platform and the mapping results of the power grid corridor are generated, including:

[0066] Step S202: Based on the current keyframe pose and the point cloud in the original point cloud of the current frame that corresponds to the current keyframe pose, obtain the current keyframe data.

[0067] Step S204: Add the current keyframe data to the keyframe sequence, wherein the keyframe sequence is used to store historical keyframe data and current keyframe data;

[0068] Step S206: Extract historical keyframe data located in the preset neighborhood of the current keyframe pose from the keyframe sequence.

[0069] Step S208: Convert the point cloud in the historical keyframe data within the preset neighborhood into a point cloud in the global map coordinate system to obtain the converted point cloud.

[0070] Step S220: The converted point cloud is fused to obtain a local map point cloud; the current keyframe pose is used as the localization result, and the mapping result is obtained based on the local map point cloud.

[0071] In this embodiment, generating current keyframe data based on the current keyframe pose and corresponding point cloud means bundling and packaging the filtered and optimized robot posture information with the real-time acquired 3D spatial point cloud information to form an independent data unit containing spatiotemporal correlation information. For example, combining the robot's coordinates at time N and the point cloud of the pipe gallery wall scanned by radar at that time into a complete keyframe object. Adding the current keyframe data to the keyframe sequence means establishing a historical data warehouse arranged in chronological order to continuously accumulate and remember the pose changes and map fragments of the mobile robot during the inspection process, preventing historical trajectory breaks due to data loss. Extracting historical keyframe data located within a preset neighborhood from the keyframe sequence means defining a spherical or cubic search range with a specific radius in 3D space with the current keyframe pose as the geometric center, and only selecting recent historical keyframes with physical distance or pose differences within this range, thereby eliminating irrelevant historical data from afar to control the computational load. For example, only selecting data from the past 10 seconds... The process involves: 1) converting the point cloud data from historical keyframes within a preset neighborhood into a point cloud in a global map coordinate system. This involves using the pose information stored in each historical keyframe as a transformation matrix to uniformly translate and rotate the point cloud coordinates, originally scattered across different local coordinate systems, to the same absolute reference system, achieving unified spatial alignment. 2) fusing the converted point cloud involves merging overlapping areas and optimizing the density of multiple frame point cloud data in the unified coordinate system, eliminating redundant points and filling gaps to generate a continuous and complete local map point cloud. 3) finally using the current keyframe pose as the localization result and obtaining the mapping result based on the local map point cloud. This means directly outputting the robot's current precise spatial position and simultaneously outputting a high-precision 3D environment model already constructed near that position. This solves the problem in existing technologies where only isolated poses are output without global view map data, enabling an intuitive and continuous display of the complete mapping result of the power grid utility tunnel in a unified global coordinate system, significantly improving the intuitiveness and completeness of the mapping.

[0072] In one optional embodiment, adding the current keyframe data to the keyframe sequence further includes: obtaining the inspection task information of the mobile robot platform, the inspection task information including at least one of the pipe gallery mileage segment, equipment section or defect point number; binding and storing the inspection task information with the corresponding current keyframe data, wherein the generation of the local map point cloud or global map is based on the current keyframe data and its bound inspection task information for block or index management.

[0073] In this embodiment, obtaining the inspection task information of the mobile robot platform specifically refers to collecting or receiving data representing the current work location and business attributes in real time while the robot is performing inspection work. The inspection task information includes at least one of the following: pipe gallery mileage segment, equipment section, or defect point number. For example, when the robot travels to pipe gallery K0+100, the system automatically associates the location with the "No. 1 power distribution room section" or a specific "defect detection task ID". Binding and storing the inspection task information with the corresponding current keyframe data refers to storing the current keyframe data, which includes LiDAR point cloud and pose information, into the keyframe data. When generating a frame sequence, a one-to-one index relationship is established between the key frame and the aforementioned inspection task information, so that each frame of map data carries a clear business semantic label. The generation of local map point clouds or global maps is based on the current key frame data and its bound inspection task information for block or index management. This means that when building a map, it no longer relies solely on geometric spatial coordinates for storage, but divides the map data into independent logical blocks or establishes an index tree based on the bound inspection task information (such as by pipe gallery mileage segment or equipment interval). For example, all key frame point clouds belonging to the "No. 1 power distribution room interval" are aggregated into the same storage block. By adopting the above steps, the geometric map data is deeply integrated with the business semantic information of power grid inspection, realizing the structured organization of map data. This enables the direct and rapid location and loading of corresponding map blocks based on inspection task information (such as specific pipe gallery sections or equipment areas) when generating local map point clouds or retrieving global maps. This avoids the problem of redundant calculation or inefficient retrieval of global maps caused by the lack of business association indexes in related technologies, thereby significantly improving the efficiency of map management and the accuracy of data retrieval for specific inspection tasks such as segmented re-inspection and defect location.

[0074] Optionally, keyframe and local map management for low-power edge computing is implemented, maintaining a keyframe list (i.e., keyframe sequence) and dynamically updating the local registration map according to the local map size, keyframe density, and real-time load. Specifically, the system main task (SystemMainTask) retrieves the current frame pose from the frame buffer queue, calls the keyframe determination function, and after confirming that the current frame pose is a keyframe, adds it to the keyframe list (keyframes), records the keyframe point cloud, optimized pose, keyframe ID, platform type, motion disturbance index, and inspection segment identifier, forming the current keyframe data. The keyframe trajectory path and keyframe ID are published. The keyframe ID is pushed to the loop closure detection input queue (loopclosure_input_deque) for processing by the loop closure detection module. The register dynamically maintains the local registration map based on the current keyframe list and the current keyframe pose. The local map is formed by fusing the point clouds of several keyframes near the current keyframe pose according to their pose transformations. The map size is jointly controlled by the configuration parameter (registration_local_map_size) and the real-time computing load. When the platform load is high, the system reduces the number of keyframes in a local map or increases the size of local map voxels; when the load returns to normal, it gradually restores the default map size. The system publishes the real-time pose and coordinate transformation (TF) of the current frame for use by the autonomous exploration, path planning, obstacle avoidance, and inspection task execution modules. It associates the current keyframe data with the power grid inspection task data. Based on the inspection task configuration, it records the mileage segment, equipment section, defect point, or task point number to which the current keyframe belongs, providing an index for subsequent defect location, map playback, and segmented re-inspection.

[0075] In an optional embodiment, before extracting historical keyframe data located within a preset neighborhood of the current keyframe pose from the keyframe sequence, the method further includes: performing loop closure detection based on the current keyframe data to identify multiple candidate loop closure pairs in the keyframe sequence, wherein each candidate loop closure pair includes the current keyframe data and one frame of historical keyframe data in the keyframe sequence; filtering the multiple candidate loop closure pairs to obtain target candidate loop closure pairs, wherein filtering is used to eliminate mismatched candidate loop closure pairs caused by the repetitive structure of the power grid corridor; obtaining the loop closure constraints of the target candidate loop closure pairs, wherein the loop closure constraints include the relative pose transformation between the two keyframe data in the target candidate loop closure pairs, and the loop closure constraints are generated based on an asynchronous task pool; and optimizing the keyframe sequence based on the loop closure constraints, wherein optimization is used to correct the keyframe poses in the keyframe sequence affected by the loop closure constraints.

[0076] In this embodiment, loop closure detection based on current keyframe data identifies multiple candidate loop closure pairs in the keyframe sequence. This refers to performing a global matching search between the keyframe features acquired at the current moment and the historical keyframe sequence during the mobile robot's inspection process, thereby finding all possible historical loop closure matches. Each candidate loop closure pair consists of the current keyframe data and a matched historical keyframe data frame, providing a candidate set for subsequent precise positioning. For scenarios with highly repetitive geometric features, such as cable supports, cable trays, or long straight pipelines commonly found in power grid utility tunnels, a specific filtering algorithm is used to eliminate matching pairs that, although similar in features, have unreasonable spatial locations or are not true loop closures, retaining only true and reliable target candidate loop closure pairs. This effectively solves the matching error problem caused by structural repetition. Furthermore, the background asynchronous task pool is used to process the fine registration calculation of the target candidate loop closure pairs in parallel, extracting the loop closures between two consecutive keyframes. The precise relative pose change is used as a loop closure constraint. Target candidate loop closure pairs are submitted to an asynchronous task pool for precise registration, generating loop closure constraints. These constraints include the relative pose transformation between two keyframes in the target candidate loop closure pair. This mechanism allows the time-consuming loop closure calculation process to run independently of the front-end mapping main thread, avoiding processing delays that could block real-time mapping. These loop closure constraints are then input as optimization factors into the back-end optimization engine to perform global or local graph optimization on the pose nodes in the keyframe sequence. This eliminates accumulated errors and corrects pose deviations affected by loop closures, ensuring the accuracy of historical keyframe data. Through these steps, in the complex environment of power grid tunnels, this solution suppresses mismatches caused by repetitive structures through a filtering mechanism and optimizes the loop closure processing flow through an asynchronous task pool. This achieves a balance between loop closure detection accuracy and real-time mapping, significantly improving the positioning accuracy and mapping quality of mobile robots during long-distance inspections.

[0077] In one optional embodiment, candidate loopback pairs are filtered based on the topological characteristics of the power grid corridor, including: obtaining the inspection section identifier, corridor mileage direction, and heading angle change of the two key frame data in any candidate loopback pair; when the two key frame data belong to the same inspection section and the heading angle change is less than a preset angle threshold, and a preset distance constraint is met, the candidate loopback pair is retained as the target candidate loopback pair; otherwise, the candidate loopback pair is eliminated.

[0078] In this embodiment, the aim is to utilize the unique topological information of the power grid tunnel to perform a secondary verification on the initially screened candidate loopback pairs, thereby eliminating mismatches caused by geometrically similar structures such as long straight tunnels or repeated supports. Specifically, the inspection section identifier is used to define the physical tunnel segment range to which the key frame belongs, ensuring that loopbacks occur within the same continuous inspection area; the tunnel mileage direction is used to constrain the relative motion logic between two key frames, such as eliminating mismatches caused by reverse travel; and the heading angle change is used to measure the difference in spatial orientation between two key frames, preventing similar structures that are parallel but in different locations from being misjudged as being in the same location. In practical operation, the topological parameters corresponding to the two keyframes in the candidate loop closure pair are first obtained. Then, a logical judgment is made: if the two keyframes belong to the same inspection section, and the change in their heading angle is less than a preset angle threshold (indicating that their orientations are basically the same and no significant turning has occurred), and the spatial distance between them meets the preset distance constraint (ensuring the possibility of physical connection), then the candidate loop closure pair is determined to conform to the pipe gallery topology logic and is retained as the target candidate loop closure pair; otherwise, if any condition is not met (such as belonging to different sections, having a large difference in orientation, or being too far apart), then the candidate loop closure pair is directly eliminated. Through this multi-dimensional topology filtering mechanism that combines section, direction, and orientation, the "false loop closure" problem caused by relying solely on geometric features or descriptor matching in strongly repetitive structure scenarios is effectively avoided, significantly improving the accuracy of loop closure detection and the reliability of subsequent pose optimization.

[0079] Optionally, asynchronous loop closure detection and backend optimization for repetitive structures in power grid utility tunnels are performed. Candidate loop closure detection, tunnel scene filtering, and fine registration are executed through independent threads and an asynchronous task pool. After generating loop closure constraints, pose graph optimization based on the General Graph Optimization (g2o) framework is used to correct accumulated drift. Specifically, this includes: Candidate loop closure detection. The loop closure detection module runs in an independent thread, waiting for new keyframe IDs (i.e., the ID corresponding to the current keyframe data). Upon receiving the current keyframe data, candidate loop closure pairs are generated according to the configuration, selecting distance-based detection, scan context descriptor-based detection, or feature-based detection. Tunnels scene candidate filtering. Nearest neighbor filtering, duplication detection, and duplication structure filtering are performed on candidate loop closure pairs. Dunnels duplication structure filtering includes: excluding nearest neighbor frames with excessively small keyframe ID differences; and filtering out suspected false loop closure candidates caused solely by repeated cable supports, cable trays, or long straight pipelines, based on the inspection section to which the keyframe belongs, tunnel mileage direction, heading angle changes, and local geometric degradation indicators. Asynchronous fine registration. Valid candidate loop closure pairs (i.e., target candidate loop closure pairs) are submitted to the asynchronous task pool (loopmatch_task_pool) for execution, avoiding blocking the front-end mapping. For each target candidate loop closure pair, the current keyframe subgraph and historical keyframe subgraph are constructed respectively. A fine registration algorithm (supporting NDT, GICP, etc.) is called to calculate the relative pose transformation T_match and registration score s between the two subgraphs. Loop closure constraint generation. If the fine registration converges and the registration score meets the threshold, and the relative pose, heading angle change, and inspection segment consistency meet the constraints, a loop closure constraint result (LoopClosureResult) is constructed, containing the loop closure pair ID, matched pose transformation, and information matrix, and written to the system's loop closure result queue. g2o backend optimization. After the system main task detects a valid loop closure result, it calls the backend optimization function. A pose graph is constructed with keyframe poses as nodes and mileage constraints and loop closure constraints between adjacent keyframes as edges. The g2o graph optimization framework is used to solve for globally consistent keyframe poses. After optimization, the poses of all keyframes are updated, triggering incremental visualization updates.

[0080] In one optional embodiment, based on the current keyframe pose, the localization result of the mobile robot platform and the mapping result of the power grid corridor are generated, which further includes: incrementally writing the current keyframe data into a local storage file in binary format and updating the mapping progress file; after receiving the loop closure constraint and performing optimization, only updating the pose matrix of the keyframe data affected by the loop closure constraint and incrementing the loop closure version number; publishing the loop closure version number and the index of the affected keyframes to the external visualization client so that the client only re-renders the point cloud of the keyframes affected by the loop closure constraint, rather than reconstructing the entire map.

[0081] In this embodiment, incrementally writing the current keyframe data to the local storage file in binary format and updating the mapping progress file means that after generating the pose of each keyframe, the system does not repeatedly write historical data, but only compresses the newly added keyframe point cloud and pose information into a binary stream and appends it to the end of the storage file. At the same time, by modifying the offset or record pointer in the progress file, the starting position of the new data is marked, thereby avoiding the repeated copying of the entire map data. After receiving the loop closure constraint and performing optimization, only updating the pose matrix of the keyframe data affected by the loop closure constraint and incrementing the loop closure version number means that when the backend optimization algorithm corrects the pose error in the closed loop determined by the loop closure detection, the system does not recalculate the entire trajectory map. Instead of optimizing the loopback result, the system precisely locates the keyframe nodes directly associated with the loopback constraint, performs local numerical corrections on their pose matrices, and increments the global loopback version number by 1 to indicate that the map data has changed. The system publishes the loopback version number and the index of the affected keyframes to external visualization clients, allowing the clients to re-render only the point cloud of the keyframes affected by the loopback constraint, rather than rebuilding the entire map. This means the backend system pushes the updated version number and the list of modified keyframe IDs to the frontend display. The frontend compares the locally cached version number with the received version number to identify the range of differences, and only calls the local point cloud data pointed to by the ID index for refresh rendering, while keeping the point cloud display state of other unaffected areas unchanged. Through these steps, low-overhead incremental storage and progress synchronization of mapping data are achieved, avoiding the huge computational overhead of full map reconstruction after loopback optimization. Simultaneously, the frontend client can accurately locate the data range that needs updating based on the version number difference, re-rendering only the affected local keyframes. This effectively solves the problems of high data transmission overhead, high frontend rendering latency, and poor real-time visualization effects in communication-constrained scenarios such as underground utility tunnels, improving system efficiency and user experience.

[0082] Optionally, map saving and incremental visualization data publishing under low bandwidth conditions are performed. Keyframe point clouds and pose matrices are incrementally written in protobuf binary format, and loop closure information, mapping progress, and inspection segment information are published in JSON format. The front-end supports incremental retrieval by keyframe index and local re-rendering based on loop closure version number. Specifically, this includes: Incremental keyframe data writing. During mapping, for each keyframe generated, the system writes the keyframe point cloud in protobuf binary format to a specified data file, and writes the current keyframe pose matrix (including a 3x3 rotation matrix, a 3x1 translation vector, loop closure version number, keyframe index, and inspection segment identifier) ​​in protobuf binary format to the specified data file. Mapping progress file update. After writing the keyframe and the corresponding current keyframe pose matrix, the mapping progress file is updated. This file contains fields such as the total number of keyframes generated, the total number of loop closures generated, whether mapping is in progress, the current inspection segment, and the low-power scheduling status. Incremental matrix update after loop closure optimization. When loop closure detection triggers backend optimization, the system only updates the matrix files of the affected keyframes and updates the closure field in the matrix to the current loop closure version number. A JSON file for this loop closure is synchronously written to a specified directory, recording the affected keyframe range, loop closure version number, and inspection segment. Incremental frontend rendering. The frontend obtains the latest keyframe count by polling the mapping status file. If the count is greater than the locally loaded count, it incrementally requests the corresponding keyframes and matrix files for rendering. When an increase in the number of loop closures is detected, the newly added loop closure file is read, and the loop closure version number in the relevant keyframe matrix is ​​checked for changes. If the version number has changed, only the matrices of the affected keyframes are re-obtained, and the point cloud positions of the corresponding keyframes are updated according to the optimized pose, achieving local re-rendering instead of full reconstruction. Low-bandwidth local caching. When the communication link of the underground utility tunnel is unstable or the bandwidth is insufficient, the system prioritizes caching keyframe point clouds, pose matrices, and inspection section indexes locally on the airborne terminal, and only uploads low-bitrate status information or sparse keyframes in real time; after the task ends or communication is restored, the complete map data is then transmitted back in batches. Atomicity write guarantee. When writing to a specified data file, a temporary file is written first, and then atomically renamed after completion to avoid the front end reading incomplete data. Map saving and tile division. When the mapping is completed, the system completes the final optimization and sets the running field in the mapping progress file to failure. At the same time, all keyframes are traversed, and the point clouds of each keyframe are transformed to the global coordinate system according to the optimized pose and merged. After voxel downsampling, it is saved as a global point cloud map in Point Cloud Data (PCD) format.Optionally, the map segmentation module (SplitMap) can be invoked to divide the global map into segmented PCD files based on the mileage segment of the utility tunnel, the equipment interval, or the grid size parameters, so that the subsequent positioning module can dynamically load them according to the location or task segment.

[0083] In an optional embodiment, the method further includes a low-power adaptive scheduling step: real-time statistics of the computational load index of the mobile robot platform and the time consumption of each processing module; when the computational load index or the time consumption of each processing module exceeds a preset threshold, a low-power priority mode is entered, automatically increasing the voxel downsampling size, increasing the keyframe generation threshold, reducing the local map size, or reducing the loop closure detection frequency; when the positioning residual increases or the motion disturbance increases, a positioning accuracy priority mode is entered, decreasing the voxel downsampling size, decreasing the keyframe generation threshold, or retaining more feature points; when the load recovers to below the threshold, the default parameters are gradually restored.

[0084] In this embodiment, during the low-power adaptive scheduling process, the computing load indicators (such as CPU / GPU utilization and memory usage) of the mobile robot platform and the time consumption of each processing module (such as point cloud preprocessing, feature extraction, pose optimization, etc.) are dynamically monitored in real time in the background to form a dynamic monitoring of the edge computing device's operating status. When the computing load or module time consumption exceeds a preset threshold, the system automatically enters a low-power priority mode. This mode directly reduces the data in subsequent algorithm processes by increasing the voxel downsampling size (reducing the number of points involved in the calculation), increasing the keyframe generation threshold (reducing the number of keyframes), reducing the local map size (reducing map management overhead), or reducing the loop closure detection frequency (reducing global optimization calculation). This reduces processing power, thereby lowering hardware resource consumption and avoiding stuttering or response delays on low-power platforms with limited computing power. Conversely, when an increase in positioning residual or enhanced motion disturbance is detected, the system switches to a positioning accuracy priority mode. This mode increases feature constraint density by reducing voxel downsampling size, lowering the keyframe generation threshold, or retaining more feature points to address the risk of positioning drift under complex conditions. When the load falls below the threshold, the system gradually restores the default parameters. This bidirectional dynamic adjustment mechanism based on real-time load and status achieves an adaptive balance between computing resource consumption and positioning accuracy under limited computing power conditions. It ensures both the real-time performance and stability of the low-power platform and the positioning accuracy in motion disturbance scenarios.

[0085] Optionally, low-power adaptive scheduling is implemented, and preprocessing time, registration time, loop closure task queue length, and onboard computing platform load are statistically analyzed in real time. The downsampling voxels, keyframe thresholds, local map size, loop closure detection frequency, and background save task priority are dynamically adjusted based on the computing budget. Specifically, this includes real-time statistics of point cloud preprocessing time, front-end registration time, IESKF update time, loop closure task queue length, map writing time, and onboard computing platform load. When the processing time for several consecutive frames exceeds a set threshold or the platform load exceeds a set threshold, the system enters a low-power priority mode, automatically increasing the voxel downsampling size, raising the keyframe generation threshold, reducing the local map size, lowering the loop closure detection frequency, or pausing unnecessary global map save tasks. When the front-end positioning residual increases, motion disturbance intensifies, or the system enters a structurally degraded pipe gallery section, the system enters a positioning accuracy priority mode, appropriately increasing the keyframe density, raising the IMU point-by-point distortion correction priority, and preserving necessary edge and planar feature points. When the system load recovers to below the threshold, the default parameters are gradually restored to ensure that the drone or quadruped robot maintains a stable positioning output frequency and acceptable map accuracy on the low-power edge computing platform.

[0086] Through steps S102 to S112, the scenario of blocked global navigation satellite systems and feature degradation is taken as the target application scenario. By receiving the original point cloud of the current frame collected by solid-state lidar and the inertial measurement data of the current frame collected by the inertial detection unit, feature extraction is performed on the original point cloud to generate point cloud cluster data, and the predicted pose is obtained by integral processing based on the inertial measurement data. Then, the predicted pose is used as the initial pose, and the laser observation pose is solved by iterative optimization. Finally, the predicted pose and the laser observation pose are fused and optimized by iterative extended Kalman filter to obtain the current key frame pose. Thus, in the case of the cumulative error easily generated by a single sensor, the tight coupling fusion optimization of inertial measurement and laser observation effectively suppresses positioning drift and improves mapping accuracy. Therefore, it can solve the problem of serious positioning drift and insufficient mapping accuracy of mobile robots caused by the cumulative error easily generated by a single sensor in the scenario of blocked global navigation satellite systems and feature degradation, thereby improving the positioning robustness and mapping accuracy of mobile robots in complex environments.

[0087] Simultaneous Localization and Mapping (SLAM) technology is the core foundation for mobile robots to achieve autonomous navigation in unknown environments. In traditional outdoor scenarios, robots can rely on Global Navigation Satellite Systems (GNSS) to obtain global position information, and use inertial measurement units (IMUs) to complete attitude estimation and trajectory calculation. However, in closed or semi-closed inspection scenarios such as high-voltage power cable tunnels, underground utility tunnels, underground mezzanines in substations, and equipment rooms, GNSS signals are severely attenuated or even completely unusable. Inspection robots cannot obtain stable global positioning information and can only rely on their onboard lidar and inertial sensors for relative positioning and environmental modeling.

[0088] With the development of intelligent power grid inspection, drones and quadruped robots are increasingly being used for autonomous inspections in scenarios such as underground utility tunnels, cable tunnels, and substation equipment areas. These platforms typically utilize low-power onboard computing units, which, limited by battery capacity, payload weight, heat dissipation, and communication links, cannot sustain long-term operation of general-purpose SLAM systems that rely on high-performance servers or high-power GPUs. Furthermore, power grid utility tunnel scenarios are characterized by numerous long, straight passages, repetitive supports and cable trays, narrow local spaces, complex lighting, and significant dust and glare interference, placing higher demands on the real-time performance, robustness, and resource efficiency of localization and mapping algorithms.

[0089] In recent years, SLAM technology based on lidar has made significant progress. LiDAR-based odometry methods, such as LOAM, extract edge and planar features from point clouds and utilize distance constraints between feature points and lines / areas for inter-frame registration, achieving high-precision real-time pose estimation. Subsequent research has built upon this foundation by introducing techniques such as IMU pre-integration and Iterative Extended Kalman Filtering (IESKF) to tightly couple inertial measurement with lidar measurement, further improving the system's robustness in scenarios involving rapid motion and feature degradation. In terms of backend optimization, pose graph-based loop closure detection and optimization methods are widely adopted. These methods detect events where the robot revisits previously explored regions, introduce loop closure constraints, and utilize graph optimization frameworks (such as g2o) to correct accumulated drift errors.

[0090] In practical engineering applications, some solid-state lidar sensors have gradually become the mainstream sensor choice for UAVs and quadruped robot platforms due to their advantages such as small size, light weight, controllable cost, and strong vibration resistance. These lidars employ a non-repeating scanning mode, and their point cloud distribution characteristics differ significantly from traditional rotating lidars, posing new adaptation requirements for point cloud preprocessing, feature extraction, and registration algorithms. For UAV platforms, it is also necessary to address point cloud distortion caused by hovering jitter, rapid turning, and airflow disturbances; for quadruped robot platforms, it is also necessary to address changes in inertial noise caused by gait periodic impacts, fuselage pitch vibrations, and low-speed stop-and-go switching.

[0091] Laser-based inertial navigation SLAM technology still has the following shortcomings when applied to underground utility tunnel inspection, UAV airborne platforms, and low-power quadruped robot platforms: 1) Insufficient motion distortion correction. Some methods in related technologies only perform linear interpolation to correct distortion based on the poses of consecutive frames, failing to fully utilize high-frequency IMU data for independent correction of each point. During rapid UAV turns, hovering jitter, or gait impacts and severe vibrations in quadruped robots, single-frame point cloud distortion is significant, leading to decreased registration accuracy. 2) Insufficient robustness of front-end positioning. Some methods in related technologies use a loosely coupled approach to fuse laser and inertial information, using the IMU only for initial pose prediction, without achieving tight coupling optimization at the state estimation level. In degraded scenarios such as long, straight, and repetitive structures like cable tunnels and underground utility tunnels, laser registration constraints are insufficient, and the loosely coupled approach struggles to effectively utilize inertial constraints to maintain positioning accuracy. 3) Insufficient adaptation to low-power airborne platforms. The algorithms in related technologies are mostly geared towards general-purpose computing platforms or high-performance industrial control computers, and do not address the computational budget control of low-power edge computing devices mounted on UAVs and quadruped robots. When the number of point clouds is large, the number of registration iterations is high, or loop closure detection is frequently triggered, it can easily lead to excessive CPU / GPU usage, increased power consumption, and decreased positioning output frequency, affecting inspection endurance and real-time control. 4) The motion disturbance characteristics of UAVs and quadruped robots are not distinguished. The methods in related technologies usually use fixed thresholds and uniform motion models, and do not adaptively adjust distortion correction, keyframe selection, local map size, and registration iteration parameters according to IMU vibration intensity, angular velocity changes, and platform motion state, resulting in insufficient stability on different inspection carriers. 5) Loop closure detection is prone to blocking the front end and is susceptible to repeated structures. In the SLAM systems of related technologies, loop closure detection and fine registration are executed synchronously in the front-end thread. When the number of loop closure candidate pairs is large or fine registration takes a long time, the front-end mapping thread is blocked; at the same time, cable supports, cable trays, pipelines, and long straight corridors have highly repetitive structures, and ordinary loop closure candidate screening is prone to mismatches. 6) Poor real-time map visualization. Related technologies typically generate a complete PCD file for front-end loading after mapping is complete, or publish the full point cloud via the Robot Operating System (ROS) topic. The former cannot meet the real-time viewing needs during inspections, while the latter experiences a sharp increase in network transmission overhead when the map size increases or underground communication links become unstable. There is a lack of incremental visualization data publishing mechanisms based on keyframe indexes, and incremental pose update mechanisms after loopback optimization. 7) Delayed map updates after loopback optimization. After loopback optimization corrects the keyframe pose, related technologies typically require a full reconstruction of the point cloud map, resulting in high computational overhead and a need for full front-end re-rendering, leading to a poor user experience. There is a lack of incremental matrix updates and local re-rendering mechanisms based on version numbers. 8) Insufficient correlation of inspection business data.The mapping methods in related technologies usually only output general point cloud maps, without fully considering the correlation between business data such as pipeline mileage sections, equipment sections, defect points, and inspection task points in power grid inspection and key frame maps, making it difficult to support subsequent defect location, task playback, and segmented re-inspection.

[0092] Based on the above embodiments and optional embodiments, the present invention proposes an optional implementation method for positioning and mapping of power grid utility tunnel inspection, which is applicable to UAVs and quadruped robot platforms equipped with solid-state LiDAR and IMU. It solves problems in related technologies such as insufficient motion distortion correction, insufficient adaptation to low-power platforms, insufficient robustness of front-end positioning, front-end blockage of loop closure detection, easy loop closure due to repeated utility tunnel structures, poor real-time map visualization, untimely map updates after loop closure optimization, and insufficient association of inspection business data. Figure 3 This is a flowchart of an optional positioning and mapping method for power grid utility tunnel inspection according to an embodiment of the present invention, such as... Figure 3 As shown, the method includes:

[0093] Step S1: Based on the low-power airborne platform control, sensor data is acquired. The system receives the current frame raw point cloud acquired by the solid-state lidar sensor on the mobile robot platform (drone or quadruped robot) and the current frame inertial measurement data acquired by the inertial measurement unit (IMU). The data is converted into an internal unified data structure and cached according to timestamps.

[0094] Step S2: Perform point cloud preprocessing for solid-state non-repeating scanning lidar, perform IMU-based point-by-point motion distortion correction, distance clipping, voxel downsampling, feature quantity budget control, and Lidar Odometry and Mapping (LOAM) feature extraction on the original point cloud of the current frame, and output the preprocessed point cloud cluster data.

[0095] Step S3: Perform laser inertial navigation fusion front-end mapping for motion disturbances of mobile robot platform, fuse the predicted pose obtained by IMU pre-integration prediction and the laser observation pose obtained by point cloud registration observation, estimate the pose of the current frame by iterative extended Kalman filter (IESKF), and dynamically determine whether the pose of the current frame is a key frame based on displacement, rotation, vibration intensity and computational load.

[0096] Step S4: Perform keyframe and local map management for low-power edge computing, maintain the keyframe list (i.e., keyframe sequence), and dynamically update the local registration map according to the local map size, keyframe density, and real-time load;

[0097] Step S5: Perform asynchronous loop closure detection and backend optimization for repetitive power grid utility tunnel structures. Candidate loop closure detection, utility tunnel scene filtering and fine registration are performed through independent threads and asynchronous task pools. After generating loop closure constraints, the cumulative drift is corrected by using a pose graph based on the g2o framework.

[0098] Step S6: Perform map saving and incremental visualization data publishing under low bandwidth conditions. Write keyframe point clouds and pose matrices incrementally in binary format of protocol buffer (protobuf). Publish loop closure information, mapping progress and inspection section information in JSON format. Support incremental retrieval by keyframe index and local re-rendering based on loop closure version number.

[0099] Step S7: Execute low-power adaptive scheduling, and perform real-time statistics on preprocessing time, registration time, loop closure task queue length and onboard computing platform load. Based on the computing budget, dynamically adjust downsampling voxels, keyframe thresholds, local map size, loop closure detection frequency and background saving task priority.

[0100] It should be noted that the specific implementation process of steps S1 to S7 is the same as that of the aforementioned embodiments, and will not be repeated here.

[0101] Step S01: Sensor Data Acquisition. The system is deployed on a drone or quadruped robot platform equipped with a solid-state lidar sensor and an IMU, running on an onboard low-power edge computing device. Application scenarios include GNSS-denied inspection environments such as high-voltage power cable tunnels, underground utility tunnels, substation underground mezzanines, equipment rooms, and enclosed corridors. The solid-state lidar sensor connects to the onboard computing platform via Ethernet, and IMU data is published via radar drivers or platform drivers. After system startup, point cloud and IMU data are subscribed to via ROS topics. IMU data is acquired at frequencies above 200Hz, including three-axis acceleration and three-axis angular velocity; the raw point cloud is acquired at a frequency of 10Hz, with each frame containing the coordinates, reflection intensity, and relative timestamps within tens of thousands of 3D points. The system loads the extrinsic parameter matrix from the lidar to the IMU through a configuration file to ensure spatial consistency of multi-sensor data.

[0102] Step S02: Point Cloud Preprocessing. The preprocessing module (PreProcessing) retrieves data from the raw point cloud queue and first converts the current frame's raw point cloud into a point cloud format (PointXYZIRT) with reflection intensity and time parameters. For solid-state LiDAR sensors, pre-registered sensor parameters (number of scan lines lidar_scan, point time scaling factor lidar_point_time_scale, etc.) are automatically adapted.

[0103] The specific process of motion distortion correction is as follows: Assume the original point cloud of the current frame contains N laser points { Each point carries a time offset relative to the frame start time. The distortion corrector queries the IMU measurement sequence within the frame time range [t_start, t_end] using the IMU data retrieval device, and integrates the attitude change step by step according to the IMU frequency starting from the frame reference time. For the i-th point, the rotation and translation of the acquisition time at that point are obtained by interpolation based on its time offset, and the point coordinates are transformed to the frame reference time coordinate system: .

[0104] The system calculates motion disturbance indices based on the IMU angular velocity variance, acceleration variance, and attitude change of the current frame. When the platform is a UAV and hovering jitter, rapid turning, or airflow disturbance is detected, the system increases the priority of point-by-point distortion correction. When the platform is a quadruped robot and gait impact or body pitch vibration is detected, the system restricts abnormal observation updates in subsequent registration and filtering. After distortion correction, points with a distance less than lidar_use_min_dist or greater than lidar_use_max_dist are filtered, and the voxel downsampling size and the maximum number of feature points per frame are adjusted according to the low-power scheduling state. Then, LOAM feature extraction is performed: the local curvature of each point in the distortion-corrected point cloud is calculated, and points with curvature greater than the edge threshold (loam_feature_corner_thres) are marked as edge features, and points with curvature less than the planar threshold (loam_feature_planar_thres) are marked as planar features. Voxel raster downsampling is performed on the edge and planar feature point clouds respectively, and PointcloudCluster data is output.

[0105] Step S03: Laser-Inertial Navigation Fusion Front-End Mapping. During FrontEnd initialization, a register instance (selecting the specific implementation based on the registration and search mode configuration, such as point-to-area registration (KD-Tree or iVox acceleration), incremental NDT registration, GICP registration, etc.), an IMU pre-integrator, and an IESKF filter are created. After each new frame is acquired from the raw point cloud queue, the following operations are performed:

[0106] The IMU pre-integrator is invoked to perform pre-integration on the IMU data between the previous frame and the current frame, obtaining the prediction increments for rotation, velocity, and position. , , Generate a priori state prediction for the current frame, i.e., predict the pose;

[0107] Using the predicted pose as the initial value, the registration tool is invoked to iteratively match the edge feature points and planar feature point cloud of the current frame with the local map to solve for the laser observation pose T_lidar. During the iteration process, the nearest edge line is searched for the edge feature points to calculate the point-to-line residual, and the nearest plane is searched for the planar feature points to calculate the point-to-plane residual. The pose is optimized by iterating through a preset maximum number of times.

[0108] The predicted pose and laser observation pose are input into the IESKF, and state prediction and iterative updates are performed. The fused optimal navigation state is output as the current frame pose (including position, velocity, attitude, and IMU bias). For frames with strong motion disturbances or abnormal registration residuals, the system reduces the impact of abnormal laser observations on state updates based on residual consistency detection results.

[0109] After fusion, the pose difference between the current frame pose and the previous keyframe pose is calculated. If the translation increment is greater than a preset translation limit or the rotation increment is greater than a preset rotation limit, the current frame pose is determined to be the current keyframe pose and cached in the system frame queue. The system dynamically adjusts the keyframe threshold based on motion disturbance indicators, structural degradation degree, and airborne platform load, balancing positioning accuracy and low-power real-time performance.

[0110] Step S04: Keyframe Management and Loop Closure Detection. After retrieving keyframes from the frame queue, the system's main task adds them to the keyframe list (keyframes), records the keyframe point cloud, pose, keyframe ID, platform type, inspection segment, and motion disturbance index, and publishes the keyframe trajectory. Simultaneously, the keyframe ID is pushed to the loop closure detection input queue.

[0111] The loop closure detection module waits for new keyframe IDs in a separate thread. Upon receiving one, it performs candidate detection: taking distance-based detection as an example, it iterates through the historical keyframe list, calculates the Euclidean distance between each historical keyframe position and the current keyframe position, and identifies historical keyframes with distances less than a preset distance threshold as candidate loop closures. After nearest neighbor filtering, duplicate detection, and pipe gallery duplicate structure filtering, valid candidate loop closure pairs are obtained and submitted to the asynchronous task pool. Pipe gallery duplicate structure filtering combines the inspection section, pipe gallery mileage direction, heading angle changes, and local geometric degradation indicators to screen out suspected false loop closures caused by duplicate cable supports, cable trays, or long straight pipelines.

[0112] The fine registration process in the asynchronous task pool involves constructing sub-graphs for the current keyframe and historical keyframes in the valid candidate loop closure pairs. (This involves taking the point clouds of historical keyframes within a predetermined neighborhood centered on the current keyframe, transforming them to a unified coordinate system according to their respective poses, and then fusing them.) A fine registration algorithm (such as multi-resolution NDT or GICP) is then called to calculate the relative pose transformation T_match and matching score between the sub-graphs. If the matching score is greater than a preset matching score threshold, and the relative pose, heading angle change, and inspection segment consistency meet the constraints, then valid loop closure constraints are generated.

[0113] After the main system task detects a valid loop closure, it constructs a pose graph and calls the g2o optimizer: using each keyframe pose as a graph node, the relative poses between adjacent keyframes as odometry constraint edges, and loop closure constraints as loop closure edges, it solves for the global pose that minimizes all constraint residuals. After optimization, it updates the poses of all keyframes.

[0114] Step S05: Incremental Visualization Data Publishing. During the map creation process, the system publishes visualization data according to the following mechanism:

[0115] When a new keyframe is generated: the keyframe point cloud is serialized into protobuf format and written to the specified data file; the keyframe pose matrix (including rotation matrix R, translation vector t, closure version number, keyframe index, and inspection segment identifier) ​​is serialized into protobuf format and written to the specified data file. The keyframes field, current inspection segment field, and low-power scheduling status field in vslam.json are updated.

[0116] When loopback optimization is complete: Update the loopback version number in the matrix file of the affected keyframes (interval [start frame, end frame]). Write a JSON file of the loopback interval (containing start frame, end frame, loopback version number, and inspected segment) to the specified loopback file. Update the closures field in vslam.json.

[0117] When communication links are restricted: the system prioritizes caching key frame point clouds and matrix files locally on the airborne terminal, and only uploads low bit rate status, sparse key frames or inspection progress information; after communication is restored or the task is completed, the complete map project directory is then sent back in batches.

[0118] At the end of mapping: After final optimization, set the `running` field to `false` (failure). Generate point cloud data in a web-based 3D point cloud visualization tool format in the map project directory for large-scale 3D browsing.

[0119] The workflow of the front-end visualization client is as follows: Poll vslam.json -> Detect the addition of keyframes -> Incrementally request keyframe and matrix files by index -> Parse protobuf point cloud and render by matrix transformation -> Detect the addition of closures -> Request the new closure file to obtain the affected interval -> Check the closure version number of the keyframe matrix within the interval -> Only re-obtain the matrix for keyframes with changed version numbers and update the rendering position -> Display the current tunnel mileage or equipment interval according to the inspection section -> Stop polling when running is false.

[0120] Step S06: Map Saving, Tiling, and Low-Power Scheduling. When the user issues a map saving command or the map is automatically saved upon completion, the system iterates through the `keyframes_` list, transforms the point cloud of each keyframe according to its optimized pose to the global coordinate system, and merges them into a global point cloud. The global point cloud is downsampled according to the pixel size (voxel_size) parameter and saved as a PCD file. If a tiled map is required, the `SplitMap` module is called to divide the global map according to the pipe gallery mileage segment, equipment interval, or map tile grid size (tile_map_grid_size) parameter. Each tile is saved as an independent PCD file, facilitating the subsequent localization module to dynamically load the local map based on the current location, inspection task point, or equipment interval.

[0121] During mapping, the system continuously monitors point cloud preprocessing time, front-end registration time, IESKF update time, loopback task queue length, map writing time, and onboard computing platform load. When the processing time for several consecutive frames exceeds a set threshold or the platform load is too high, the system enters a low-power priority mode, automatically increasing voxel downsampling size, raising the keyframe generation threshold, reducing the local map size, decreasing the loopback detection frequency, or pausing unnecessary background saving tasks. When the positioning residual increases, motion disturbance intensifies, or the system enters a structurally degraded utility tunnel section, the system enters a positioning accuracy priority mode, appropriately increasing the keyframe density while retaining necessary edge and planar feature points.

[0122] The method in this embodiment can achieve at least one of the following effects: 1) The method in this embodiment is applicable to low-power airborne platforms for UAVs and quadruped robots. This embodiment is designed for UAVs and quadruped robots equipped with solid-state LiDAR and IMU. By using point cloud quantity budgeting, keyframe threshold adaptation, local map size control, loop asynchronous scheduling, and incremental map publishing, it reduces CPU / GPU peak usage and communication overhead, making it suitable for long-term operation of inspection platforms with battery power, limited load, and limited heat dissipation. 2) High accuracy of point-by-point motion distortion correction. This embodiment uses high-frequency IMU data to independently correct motion distortion for each laser point within a frame, rather than just performing linear interpolation between frames. This can effectively eliminate single-frame point cloud distortion under conditions of rapid UAV turning, hovering jitter, and severe impact and vibration of quadruped robot gait, significantly improving the stability and accuracy of subsequent registration. 3) Strong robustness of tightly coupled laser inertial navigation positioning. This embodiment employs the IESKF framework to tightly couple and optimize IMU pre-integration prediction and laser registration observation at the state estimation level, fully utilizing inertial constraints to compensate for insufficient constraints in laser registration in long straight pipe corridors, repetitive cable supports, and weak geometric feature scenarios. Compared to loose coupling, tight coupling fusion can maintain stable pose output even when features degrade. 4) It can adapt to motion disturbances of different inspection vehicles. This embodiment identifies states such as UAV hovering jitter, rapid turning, and quadruped robot gait impact based on IMU angular velocity, acceleration changes, and attitude changes, and dynamically adjusts distortion correction, registration, keyframe, and local map parameters to improve cross-platform deployment stability. 5) Asynchronous loop closure detection does not block the front end. This embodiment submits the fine registration operation of loop closure candidates to the asynchronous task pool for execution. The front-end mapping thread can continue processing the next frame of point cloud data after submitting the loop closure task, unaffected by the loop closure matching time, ensuring the real-time performance of pose output. 6) It reduces the risk of false loop closures for repetitive structures in power grid pipe corridors. This embodiment combines the inspection section, the direction of the utility tunnel mileage, the change in heading angle, the keyframe interval, and local geometric degradation indicators to filter loopback candidates, which can reduce the probability of mismatches caused by repeated cable supports, cable trays, pipelines, and long straight corridors. 7) Incremental visualization data publishing mechanism. This embodiment adopts an incremental data publishing method based on keyframe index. Each keyframe is generated and written to the corresponding file in protobuf binary format, and the front end pulls it incrementally according to the index. Compared with full PCD publishing or ROS full topic publishing, this method significantly reduces network transmission overhead and realizes real-time 3D point cloud map display during the mapping process, which is suitable for underground utility tunnel communication-restricted environments. 8) Incremental loopback update based on version number. This embodiment introduces a loopback version number (closure) mechanism. After loopback optimization, only the pose matrix of the affected keyframes is updated and the version number is incremented. The front end can accurately identify the range of keyframes that need to be re-rendered by comparing the version number, realizing local re-rendering instead of full reconstruction, which greatly reduces the map update overhead and front-end rendering latency after loopback optimization.9) Atomicity file writing ensures data consistency. This embodiment uses a method of writing a temporary file first and then atomically renaming it after completion to update the visualization data file. This avoids the front end reading incomplete or corrupted data when the file writing is not finished, ensuring data consistency in multi-process concurrent read / write scenarios. 10) Facilitates the association of power grid inspection business data. This embodiment records the inspection section, pipeline mileage section, equipment section, defect point or task point number in keyframes and map blocks, enabling the mapping results to serve defect location, task playback, segmented re-inspection, and subsequent positioning and navigation.

[0123] This embodiment also provides a positioning and mapping device for power grid corridor inspection. This device is used to implement the above embodiments and preferred embodiments, and details already described will not be repeated. As used below, the terms "module" and "device" can refer to a combination of software and / or hardware that performs a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0124] According to an embodiment of the present invention, an apparatus embodiment for implementing the above-described positioning and mapping method for power grid utility tunnel inspection is also provided. Figure 4 This is a schematic diagram of a positioning and mapping device for power grid utility tunnel inspection according to an embodiment of the present invention, as shown below. Figure 4 As shown, the aforementioned positioning and mapping device for power grid utility tunnel inspection includes: a data receiving module 400, a point cloud cluster generation module 402, a predicted pose determination module 404, an observed pose determination module 406, a keyframe recognition module 408, and a positioning and mapping module 410, wherein:

[0125] The data receiving module 400 is used to receive the original point cloud of the current frame collected by the solid-state lidar sensor on the mobile robot platform for the power grid tunnel during the current sampling period, and the inertial measurement data of the current frame collected by the inertial detection unit on the mobile robot platform for the power grid tunnel during the current sampling period.

[0126] The point cloud cluster generation module 402 is connected to the data receiving module 400 and is used to extract features based on the original point cloud of the current frame to generate point cloud cluster data.

[0127] The predicted pose determination module 404 is connected to the point cloud cluster generation module 402 and is used to perform integral processing based on the current frame inertial measurement data to obtain the predicted pose of the current frame.

[0128] The observation pose determination module 406 is connected to the prediction pose determination module 404. It is used to solve the laser observation pose of the original point cloud in the local map coordinate system in the current frame by iterative optimization based on point cloud cluster data and the predicted pose as the initial pose.

[0129] The keyframe recognition module 408 is connected to the observation pose determination module 406. It is used to obtain the current keyframe pose by using an iterative extended Kalman filter to perform fusion optimization based on the predicted pose and the laser observation pose.

[0130] The localization and mapping module 410 is connected to the keyframe recognition module 408 and is used to generate the localization result of the mobile robot platform and the mapping result of the power grid corridor based on the current keyframe pose.

[0131] It should be noted that the above modules can be implemented by software or hardware. For example, for the latter, it can be implemented in the following ways: the above modules can be located in the same processor; or the above modules can be located in different processors in any combination.

[0132] It should be noted that the data receiving module 400, point cloud cluster generation module 402, predicted pose determination module 404, observed pose determination module 406, keyframe recognition module 408, and localization mapping module 410 mentioned above correspond to steps S102 to S112 in the embodiments. The instances and application scenarios implemented by the above modules and corresponding steps are the same, but are not limited to the content disclosed in the above embodiments. It should be noted that the above modules, as part of the device, can run in a computer terminal.

[0133] It should be noted that the optional or preferred implementation methods of this embodiment can be found in the relevant descriptions in the embodiments, and will not be repeated here.

[0134] The aforementioned positioning and mapping device for power grid tunnel inspection may also include a processor and a memory. The aforementioned data receiving module 400, point cloud cluster generation module 402, predicted pose determination module 404, observed pose determination module 406, key frame recognition module 408, positioning and mapping module 410, etc., are all stored in the memory as program modules. The processor executes the aforementioned program modules stored in the memory to realize the corresponding functions.

[0135] The processor contains a core that retrieves the corresponding program modules from memory. One or more cores may be configured. Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM. Memory includes at least one memory chip.

[0136] According to an embodiment of this application, an embodiment of a non-volatile storage medium is also provided. Optionally, in this embodiment, the non-volatile storage medium includes a stored program, wherein, when the program runs, it controls the device containing the non-volatile storage medium to execute any of the aforementioned positioning and mapping methods for power grid utility tunnel inspection.

[0137] Optionally, in this embodiment, the non-volatile storage medium may be located in any computer terminal in a group of computer terminals in a computer network, or in any mobile terminal in a group of mobile terminals, and the non-volatile storage medium includes stored programs.

[0138] Optionally, a program may be used to control the device containing the non-volatile storage medium to execute any of the above-mentioned positioning and mapping method steps for power grid pipeline inspection during program execution.

[0139] According to an embodiment of this application, an embodiment of a processor is also provided. Optionally, in this embodiment, the processor is used to run a program, wherein the program executes any of the above-mentioned positioning and mapping methods for power grid utility tunnel inspection.

[0140] According to an embodiment of this application, an embodiment of a computer program product is also provided, which, when executed on a data processing device, is adapted to execute a program that initializes the positioning and mapping method steps for power grid tunnel inspection, having any of the steps described above.

[0141] like Figure 5 As shown, an embodiment of the present invention provides an electronic device 10, which includes a processor, a memory, and a program stored in the memory and executable on the processor. When the processor executes the program, it implements the positioning and mapping method steps of any of the above-mentioned power grid corridor inspection methods.

[0142] The order of the above embodiments of the present invention is merely for description and does not represent the superiority or inferiority of the embodiments.

[0143] In the above embodiments of the present invention, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0144] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. The device embodiments described above are merely illustrative; for example, the division of modules described above can be a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces, or indirect coupling or communication connection between modules, and may be electrical or other forms.

[0145] The modules described above as separate components may or may not be physically separate. Similarly, the components shown as modules may or may not be physical modules; they may be located in one place or distributed across multiple modules. Some or all of the modules can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0146] Furthermore, the functional modules in the various embodiments of the present invention can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module. The integrated modules described above can be implemented in hardware or as software functional modules.

[0147] If the aforementioned integrated modules are implemented as software functional modules and sold or used as independent products, they can be stored in a computer-readable non-volatile storage medium. Based on this understanding, the technical solution of this invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a non-volatile storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this invention. The aforementioned non-volatile storage medium includes various media capable of storing program code, such as USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks.

[0148] The above are merely preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A positioning and mapping method for power grid utility tunnel inspection, characterized in that, include: The system receives the original point cloud of the current frame collected by the solid-state lidar sensor on the mobile robot platform for the power grid tunnel during the current sampling period, and the inertial measurement data of the current frame collected by the inertial detection unit on the mobile robot platform for the power grid tunnel during the current sampling period. Feature extraction is performed on the original point cloud of the current frame to generate point cloud cluster data; The predicted pose of the current frame is obtained by performing integration processing based on the inertial measurement data of the current frame. Based on the point cloud cluster data, and using the predicted pose as the initial pose, the laser observation pose of the original point cloud in the current frame in the local map coordinate system is solved by iterative optimization. Based on the predicted pose and the laser observation pose, an iterative extended Kalman filter is used for fusion optimization to obtain the current keyframe pose. Based on the current keyframe pose, the localization result of the mobile robot platform and the mapping result of the power grid corridor are generated.

2. The method according to claim 1, characterized in that, Feature extraction is performed based on the original point cloud of the current frame to generate point cloud cluster data, including: Based on the inertial measurement data of the current frame, distortion correction is performed on the original point cloud of the current frame to obtain the corrected point cloud; The corrected point cloud is subjected to feature processing to obtain the point cloud cluster data.

3. The method according to claim 2, characterized in that, The step of performing distortion correction on the original point cloud of the current frame based on the inertial measurement data of the current frame to obtain the corrected point cloud includes: For each point in the original point cloud of the current frame, based on the acquisition time of each point, query the inertial measurement data corresponding to each point from the inertial measurement data of the current frame; Based on the corresponding inertial measurement data, the pose transformation matrix of each point relative to the current frame reference time is determined, wherein the pose transformation matrix is ​​used to indicate the spatial position and attitude relationship between the sensor coordinate system at the corresponding point acquisition time and the sensor coordinate system at the current frame reference time. Based on the pose transformation matrix, the coordinates of each point are transformed from the sensor coordinate system at the corresponding acquisition time to the sensor coordinate system at the current frame reference time to obtain the corrected point cloud.

4. The method according to claim 2, characterized in that, The step of performing feature processing on the corrected point cloud to obtain the point cloud cluster data includes: Determine the local curvature of each point in the corrected point cloud; Based on preset edge curvature thresholds and plane curvature thresholds, the corrected point cloud is divided into an edge feature point set and a plane feature point set; Voxel downsampling is performed on the edge feature point set and the planar feature point set respectively to obtain downsampled edge feature points and downsampled planar feature points; Based on the preset upper limit of the number of features, the downsampled edge feature points and the downsampled planar feature points are filtered respectively to obtain the filtered edge feature points and the filtered planar feature points. Based on the corrected point cloud, the filtered edge feature points, and the filtered planar feature points, the point cloud cluster data is obtained.

5. The method according to claim 1, characterized in that, The step of using the predicted pose as the initial pose and iteratively optimizing the laser observation pose of the original point cloud in the local map coordinate system based on the point cloud cluster data includes: Based on the edge feature points in the point cloud cluster data, search for edge lines in the local map and calculate the first distance residual from the edge feature points to the edge lines; Based on the planar feature points in the point cloud cluster data, search for planes in the local map and calculate the second distance residual from the planar feature points to the planes; With the goal of minimizing the first distance residual and the second distance residual, and using the predicted pose as the initial pose, the initial pose is iteratively updated through a target iterative optimization algorithm until a predetermined convergence condition is met, thereby obtaining the laser observation pose.

6. The method according to claim 1, characterized in that, The process of fusing and optimizing the predicted pose and the laser observation pose using an iterative extended Kalman filter to obtain the current keyframe pose includes: The current state estimate of the iterative extended Kalman filter is initialized using the predicted pose; Based on the laser observation pose, the current state estimate is iteratively updated; During the iterative update process, the consistency between the laser observation pose and the current state estimate is detected; If the consistency is detected to meet the preset conditions, the current state estimate is updated based on the laser observation pose; If the consistency is not satisfied with the preset condition, then the contribution of the laser observation pose to the update of the current state estimate is limited. The current keyframe pose is obtained based on the current state estimate obtained after the iteration update.

7. The method according to claim 1, characterized in that, The process of fusing and optimizing the predicted pose and the laser observation pose using an iterative extended Kalman filter to obtain the current keyframe pose includes: Based on the predicted pose and the laser observation pose, the iterative extended Kalman filter is used for fusion optimization to obtain the current frame pose; Determine the translation increment and rotation increment of the current frame pose relative to the previous keyframe pose, wherein the previous keyframe pose is the keyframe pose most recently generated from the current sampling period. If the translation increment is greater than a preset translation limit or the rotation increment is greater than a preset rotation limit, the current frame pose is taken as the current keyframe pose.

8. The method according to any one of claims 1 to 7, characterized in that, The process of generating the localization result of the mobile robot platform and the mapping result of the power grid corridor based on the current keyframe pose includes: Based on the current keyframe pose and the point cloud in the original point cloud of the current frame that corresponds to the current keyframe pose, the current keyframe data is obtained. The current keyframe data is added to the keyframe sequence, wherein the keyframe sequence is used to store historical keyframe data and the current keyframe data; Extract historical keyframe data located within a preset neighborhood of the current keyframe pose from the keyframe sequence; Convert the point cloud in the historical keyframe data within the preset neighborhood into a point cloud in the global map coordinate system to obtain the converted point cloud. The transformed point cloud is fused to obtain a local map point cloud; The current keyframe pose is used as the localization result, and the mapping result is obtained based on the local map point cloud.

9. The method according to claim 8, characterized in that, Before extracting historical keyframe data located within a preset neighborhood of the current keyframe pose from the keyframe sequence, the method further includes: Based on the current keyframe data, loop closure detection is performed to identify multiple candidate loop closure pairs in the keyframe sequence, wherein each candidate loop closure pair includes the current keyframe data and one historical keyframe data frame in the keyframe sequence. The plurality of candidate loopback pairs are filtered to obtain target candidate loopback pairs, wherein the filtering is used to eliminate mismatched candidate loopback pairs caused by the repetitive structure of the power grid corridor; The loop closure constraints of the target candidate loop closure pair are obtained, wherein the loop closure constraints include the relative pose transformation between the two keyframe data in the target candidate loop closure pair, and the loop closure constraints are generated based on an asynchronous task pool; The keyframe sequence is optimized based on the loop closure constraint, wherein the optimization is used to correct the pose of keyframes in the keyframe sequence that are affected by the loop closure constraint.

10. An electronic device, characterized in that, It includes one or more processors and a memory, the memory being used to store one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to implement the positioning and mapping method for power grid corridor inspection as described in any one of claims 1 to 9.

Images