Rail transit intelligent inspection robot positioning system based on laser SLAM and inertial navigation

Abstract

The invention discloses a rail transit intelligent inspection robot positioning system based on laser SLAM and inertial navigation, and relates to the field of real-time positioning, the rail transit intelligent inspection robot positioning system is characterized in that data are synchronously acquired through a laser radar, an IMU, a speedometer and an RGB-D camera, and high precision in rail positioning is realized; the point cloud filtering module extracts orbit feature points through an improved voxel grid algorithm, and the inertial navigation initialization module calculates an initial pose based on IMU data and establishes a motion constraint model; the map feature layering module divides the point cloud into a track surface, equipment and a background layer; and the data tight coupling module optimizes the pose, and the dynamic object elimination module eliminates obstacles, so that the SLAM optimization precision is ensured. And the global positioning compensation module performs repositioning through particle filtering to generate a 6DoF pose estimation semantic map. The method has the advantages that by fusing the laser SLAM and inertial navigation technologies, high-precision and stable rail transit inspection is realized, the positioning precision and the environmental adaptability are optimized, and the intelligent operation and maintenance efficiency is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of real-time positioning, in particular to a track traffic intelligent inspection robot positioning system based on laser SLAM and inertial navigation. BACKGROUND

[0002] As an important part of modern urban public transportation, the safety, stability and efficiency of track traffic directly affect the operation order of the city and the travel experience of citizens. With the increasing complexity of urban track traffic systems, traditional manual inspection methods face problems such as low efficiency, high cost, and difficulty in comprehensive coverage. Therefore, intelligent inspection robots have emerged and gradually become an important innovative technology in the field of track traffic.

[0003] Current systems on the market fail to effectively combine multi-dimensional data such as laser radar, IMU, odometer and RGB-D camera, resulting in poor positioning accuracy and stability in complex track environments. In addition, traditional systems often lack efficient dynamic object perception and rejection mechanisms, are easily disturbed by moving obstacles, and affect the SLAM optimization process, thereby reducing the reliability and accuracy of inspection. Some existing systems also have rough geometric feature recognition of track equipment, which cannot achieve fine feature extraction, limiting comprehensive monitoring of track equipment status. Furthermore, most systems fail to adaptively switch positioning modes when facing different track environments, such as using different navigation methods in tunnels and open areas, resulting in insufficient adaptability of the system in complex scenarios. The compensation mechanism for global positioning error is also relatively simple and cannot effectively correct cumulative errors through efficient repositioning algorithms, causing positioning drift problems in long-term operation. SUMMARY

[0004] To improve existing systems, a track traffic intelligent inspection robot positioning system based on laser SLAM and inertial navigation is provided. This system combines multi-source data acquisition, intelligent filtering and dynamic object recognition by integrating laser SLAM and inertial navigation technology, achieving high-precision and stable track traffic inspection, optimizing positioning accuracy and environmental adaptability, and improving intelligent operation efficiency.

[0005] To achieve the above objectives, the technical solution adopted by the present application is as follows: A track traffic intelligent inspection robot positioning system based on laser SLAM and inertial navigation, comprising: A multi-source data acquisition module: the module includes a laser radar, an IMU inertial measurement unit, an odometer and an RGB-D camera connected rigidly, to synchronously acquire track point cloud data, inertial acceleration and angular velocity data and odometer pulse signals in time stamp alignment mode; Point cloud filtering module: The module processes the original point cloud of the lidar through an improved voxel grid filtering algorithm and dynamically adjusts the filtering parameters to obtain key feature points in the orbital region; Inertial navigation initialization module: Based on the acceleration and angular velocity data provided by the IMU, the module calculates the robot's initial pose through a pre-integration algorithm and establishes a short-term motion constraint model for inertial navigation based on error state Kalman filtering; Map feature layering module: The module divides the point cloud map generated by laser SLAM into a three-layer structure, including the track surface layer, equipment layer and background layer, and encodes the geometric features of the track contact wire, sleepers and turnouts through feature descriptors; Data tightly coupled module: The module constructs a joint optimization objective function, integrates lidar feature point matching error, IMU pre-integration error, and odometry motion error, and iteratively solves for the optimal pose using the LM algorithm; Dynamic object perception and culling module: Based on the semantic segmentation results obtained by the RGB-D camera, the module identifies moving obstacles and establishes a dynamic mask in the laser point cloud to eliminate dynamic point cloud interference during the SLAM optimization stage; Global positioning error compensation module: When the module detects a track feature marker, it triggers global repositioning based on particle filtering, calculates the compensation amount for the cumulative error of inertial navigation, and updates the pose estimation. Results output visualization module: The module outputs 6DoF pose estimation results and generates a semantic map containing track equipment status data.

[0006] Preferably, the multi-source data acquisition module specifically includes: Data acquisition unit: The unit records the timestamp and three-dimensional coordinates of each point on the track using a lidar to acquire track point cloud data, acquires acceleration and angular velocity through an I2C / SPI interface, acquires the pulse signal of the odometer through a pulse counter, records the number of wheel and axle rotations and displacement data, and acquires RGB and depth images through an RGB-D camera; Time synchronization unit: The unit synchronizes the sampling start time of the lidar, IMU, odometer and RGB-D camera through hardware trigger signal, and adds a high-precision timestamp to the data packet of each sensor, and uses the master time source for calibration; Data processing module: The module preprocesses and fuses the collected multi-source data.

[0007] Preferably, the point cloud filtering module specifically includes: Parameter initialization unit: The unit initializes the size of the voxel grid based on the point cloud density of the track region, sets the initial threshold for point cloud filtering, predefines the spatial range of the track region, and preprocesses the point cloud. A voxel grid filtering unit: the unit divides the point cloud space into a three-dimensional voxel grid, assigns each point in the point cloud to a corresponding voxel, calculates the weighted center of the points in each voxel, and removes voxels with a point number below a density threshold; A dynamic parameter adjustment unit: the unit dynamically adjusts the filtering parameters based on the characteristics of the point cloud, and obtains high-quality feature points in different track scenarios; A feature point extraction unit: the unit segments the point cloud subset of the track area from the filtered point cloud, and identifies the key feature points of the track.

[0008] Preferably, the inertial navigation initialization module specifically comprises: A pre-integration algorithm unit: the unit calculates the velocity vector by calculating the velocity change based on the acceleration data, calculates the rotation matrix by calculating the attitude change based on the angular velocity data, and integrates the position to obtain the current position; An initial pose estimation unit: the unit obtains the initial attitude by integration based on the angular velocity data, estimates the initial velocity based on the integration result of the acceleration data, and estimates the position based on the initial velocity and the starting position; A Kalman filter unit: the unit designs a filter for the inertial navigation system through error state Kalman filtering, fuses IMU data, and estimates the position, velocity and attitude data of the robot in real time; A motion constraint model unit: the unit introduces a short-term motion constraint model based on the Kalman filter, and obtains accurate positioning by constraining the motion trajectory of the robot in a short time.

[0009] Preferably, the map feature layering module specifically comprises: A point cloud layering unit: the unit divides the point cloud map into a track surface layer, a device layer and a background layer, and classifies based on geometric and semantic information; A feature extraction unit: the unit extracts the key geometric features of the track catenary, the track tie and the turnout from the point cloud, and generates feature descriptors.

[0010] Preferably, the data tight coupling module specifically comprises: An error calculation unit: the unit constructs an error model for the lidar, IMU and odometry data respectively, and quantifies the deviation between each sensor data and the robot pose; A joint optimization objective function unit: the unit constructs a weighted least squares objective function based on each error, and optimizes the pose, velocity and IMU bias of the robot by introducing motion constraints; An LM algorithm solving unit: the unit solves the joint optimization objective function through the LM algorithm, and obtains the optimal robot pose.

[0011] Preferably, the dynamic object perception and elimination module specifically comprises: A semantic segmentation unit: the unit performs semantic segmentation on the RGB image of the RGB-D camera, generates semantic labels for each pixel, and extracts the region of the dynamic object and records its mask according to the semantic labels; A dynamic object recognition unit: the unit combines the semantic segmentation result with the depth image to generate a 3D point cloud of the dynamic object, identifies the dynamic object in the continuous frame through a tracking algorithm, and projects the point cloud of the dynamic object into the laser radar point cloud coordinate system to obtain a corresponding laser point cloud subset; A dynamic mask generation unit: the unit generates a mask for the dynamic object based on the obtained laser point cloud subset.

[0012] Preferably, the global positioning error compensation module specifically comprises: A marker detection unit: the unit detects feature markers on the track through a sensor to trigger a repositioning process; A particle filtering repositioning unit: the unit performs global repositioning based on a particle filtering algorithm, corrects the cumulative error of the inertial navigation system, and updates the pose estimation; An inertial navigation error compensation unit: the unit calculates and compensates the cumulative error of the inertial navigation system based on the particle filtering repositioning result; A pose updating unit: the unit outputs the compensated global pose to other modules, monitors the running state of the modules in real time, and adjusts the parameters as needed.

[0013] Preferably, the result output visualization module specifically comprises: A 6DoF pose estimation unit: the unit is used to output the updated pose data, including the position in the three-dimensional space and the rotation angle around the three axes; A track equipment state semantic map unit: the unit integrates the 6DoF pose estimation result and the track equipment state data to generate a map with semantic labels, representing the spatial position, environmental features and state information of the equipment; A visualization unit: the unit displays the generated semantic map and equipment state data through a graphical interface.

[0014] Preferably, it further comprises a track scene adaptive mechanism: automatically switch the positioning mode according to the availability of Beidou satellite signals: open areas use Beidou satellite navigation system / INS integrated navigation, and tunnels switch to pure laser SLAM mode; when detecting a turnout area, automatically increase the laser scanning frequency to 20Hz.

[0015] Compared with the prior art, the advantages of the present application are: By fusing laser radar, IMU, odometer and RGB-D camera data through the multi-source data acquisition module, high-precision time synchronization and multi-dimensional environment perception are realized to ensure data reliability. The point cloud filtering module adopts an improved voxel grid filtering algorithm, dynamically adjusts parameters, efficiently extracts track key features, and improves positioning robustness. The inertial navigation initialization module combines pre-integration and Kalman filtering to provide accurate initial pose and short-term motion constraints, enhancing complex environment adaptability. The map feature layering module refines track device geometric properties through layered point clouds and feature descriptors, supporting fine inspection. The data tight coupling module fuses multi-source errors through joint optimization, iteratively solves the optimal pose, and improves positioning accuracy. The dynamic object perception rejection module effectively excludes moving obstacle interference using semantic segmentation to ensure SLAM optimization stability. The global positioning error compensation module corrects cumulative errors through particle filter repositioning to ensure long-term operation reliability. In addition, the track scene adaptive mechanism automatically switches positioning modes according to the environment, combines Beidou navigation and laser SLAM, adapts to open areas and tunnel scenes, and increases scanning frequency in switch areas to optimize complex scene performance. The system outputs 6DoF pose and semantic map, clearly visualizes track device status, significantly improves inspection efficiency and safety, and is suitable for intelligent operation and maintenance needs of rail transit. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 The system schematic diagram proposed by the present application; Figure 2 The multi-source data acquisition module diagram proposed by the present application; Figure 3 The point cloud filtering module diagram proposed by the present application; Figure 4 The inertial navigation initialization module diagram proposed by the present application; Figure 5 The map feature layering module diagram proposed by the present application; Figure 6 The data tight coupling module diagram proposed by the present application; Figure 7 The dynamic object perception rejection module diagram proposed by the present application: Figure 8 The global positioning error compensation module diagram proposed by the present application; Figure 9 The result output visualization module diagram proposed by the present application. DETAILED DESCRIPTION

[0017] The following description is used to disclose the present application so that those skilled in the art can implement the present application. The preferred embodiments in the following description are only as examples, and other obvious modifications can be thought of by those skilled in the art.

[0018] Reference Figure 1As shown, a rail transit intelligent inspection robot positioning system based on laser SLAM and inertial navigation includes: Multi-source data acquisition module: the module includes rigidly connected laser radar, IMU inertial measurement unit, odometer and RGB-D camera, and synchronously acquires track point cloud data, inertial acceleration angular velocity data and odometer pulse signal in time stamp alignment mode; Point cloud filtering module: the module processes the original point cloud of the laser radar through an improved voxel grid filtering algorithm, dynamically adjusts the filtering parameters to obtain the key feature points of the track area; Inertial navigation initialization module: the module calculates the initial pose of the robot through the pre-integration algorithm based on the acceleration and angular velocity data provided by the IMU, and establishes an inertial navigation short-term motion constraint model based on the error state Kalman filter; Map feature layering module: the module divides the point cloud map generated by the laser SLAM into three layers of structure, including the track surface layer, the device layer and the background layer, and encodes the geometric features of the track catenary, the sleeper and the turnout through the feature descriptor; Data tight coupling module: the module fuses the laser radar feature point matching error, the IMU pre-integration error and the odometer motion error by constructing a joint optimization objective function, and iteratively solves the optimal pose through the LM algorithm; Dynamic object perception and removal module: the module identifies moving obstacles based on the semantic segmentation results obtained by the RGB-D camera, establishes a dynamic mask in the laser point cloud, and excludes dynamic point cloud interference in the SLAM optimization stage; Global positioning error compensation module: the module triggers global repositioning based on particle filtering when detecting track feature markers, calculates the compensation amount of the cumulative error of the inertial navigation and updates the pose estimation; Result output visualization module: the module outputs the 6DoF pose estimation result and generates a semantic map containing track device state data.

[0019] Referring to Figure 2 As shown, the multi-source data acquisition module specifically includes: Data acquisition unit: the unit records the time stamp and three-dimensional coordinates of each point of the track through the laser radar, acquires track point cloud data, acquires acceleration and angular velocity through the I2C / SPI interface, acquires the pulse signal of the odometer through the pulse counter, records the number of wheel shaft rotations and displacement data, and acquires RGB images and depth images through the RGB-D camera; Time synchronization unit: the unit synchronizes the sampling start time of the laser radar, IMU, odometer and RGB-D camera through a hardware trigger signal, and adds high-precision time stamps to the data packets of each sensor, and uses a main time source for calibration; The data processing module is configured to preprocess and fuse the collected multi-source data.

[0020] Referring to Figure 3 As shown in the figure, the point cloud filtering module specifically comprises: The parameter initialization unit is configured to initialize the size of the voxel grid based on the point cloud density of the track region, set the initial threshold for point cloud filtering, predefine the spatial range of the track region, and preprocess the point cloud. The voxel grid filtering unit is configured to divide the point cloud space into a three-dimensional voxel grid, assign each point in the point cloud to a corresponding voxel, calculate the weighted center of the points in each voxel, and remove voxels with a point number below a density threshold. The dynamic parameter adjustment unit is configured to dynamically adjust the filtering parameters based on the characteristics of the point cloud, and obtain high-quality feature points in different track scenarios. The feature point extraction unit is configured to segment the point cloud subset of the track region from the filtered point cloud, and identify the key feature points of the track.

[0021] Specifically, the original point cloud is preliminarily cleaned to remove obvious noise and irrelevant regions. Through k-nearest neighbor-based distance analysis, isolated points far from the main points in the point cloud are removed, such as points with a distance from the nearest neighbor point exceeding 2 times the standard deviation. The original point cloud is converted from the laser radar coordinate system to a unified world coordinate system, facilitating alignment with the track geometry model. According to the predefined track region range, the point cloud is cropped to only retain points in the track and its vicinity, excluding irrelevant background points. The integrity of the point cloud data is verified to ensure the reliability of the input data. The density distribution, curvature change, and spatial distribution characteristics of the point cloud are analyzed to determine the complexity of the track region. In high-density regions, the voxel size is reduced to retain details, and in low-density regions, the voxel size is increased to reduce noise. The density threshold is dynamically adjusted based on the point cloud density to ensure balanced filtering effect. The cropping range is dynamically adjusted based on the geometric features of the track region, such as the curvature change of the track edge, to prevent accidental deletion of key points. According to the track geometry model, the point cloud subset of the track region is segmented from the filtered point cloud. Based on the geometric features of the point cloud, such as the normal vector, curvature, or height change, the key feature points of the track are identified, including the track surface, edge, and connection point. The identified feature points are clustered and optimized to improve the stability and accuracy of the feature points, and attribute information is added to each feature point.

[0022] Referring to Figure 4 As shown in the figure, the inertial navigation initialization module specifically comprises: The pre-integration algorithm unit is configured to calculate the velocity vector based on the pre-integration algorithm by calculating the velocity change from the acceleration data, calculate the rotation matrix by calculating the attitude change from the angular velocity data, and integrate the position to obtain the current position. An initial pose estimation unit: the unit estimates the initial pose by integration based on angular velocity data, estimates the initial velocity based on the integration result of acceleration data, and estimates the position based on the initial velocity and the starting position; A Kalman filter unit: the unit designs a filter of the inertial navigation system through error state Kalman filtering, fuses IMU data, and estimates the robot position, velocity and attitude data in real time; A motion constraint model unit: the unit introduces a short-term motion constraint model on the basis of the Kalman filter, acquires accurate positioning by constraining the motion trajectory of the robot in a short time.

[0023] Specifically, the state variables of the filter are defined, including the position, velocity, attitude of the robot, and the accelerometer bias and gyroscope bias of the IMU, a state transition model is constructed based on the pre-integration result and the kinematics equation, representing the state transition process from the current time to the next time, a measurement equation is designed according to the measurement model of the IMU sensor, representing the relationship between the IMU data and the state variables, wherein the velocity pre-integration formula is:

[0024] wherein, is the velocity pre-integration quantity, is the original acceleration measured by the IMU at time k, is the zero bias of the accelerometer, is the rotation matrix from the world coordinate system to the body coordinate system at time k, is the gravity vector, is the IMU sampling time interval; The position pre-integration formula is:

[0025] wherein, is the position pre-integration quantity, is the velocity pre-integration quantity from time i to k; The rotation pre-integration formula is:

[0026] wherein, is the rotation pre-integration quantity, is the original angular velocity measured by the IMU at time k, is the zero bias of the gyroscope, is the exponential mapping on the Lie group, which converts the angular velocity increment into a rotation matrix; The Kalman filter is initialized by the initial pose, velocity and IMU bias estimates, the Kalman gain is calculated according to the system process noise and measurement noise, and the state estimation is updated through filter iteration, the estimation result is optimized by updating the error state and covariance matrix in real time, and the accuracy of the pose and velocity is improved; According to the characteristics of the robot motion, a suitable motion model is set, the short-term motion of the robot is modeled, the acceleration and angular velocity limit conditions are added according to the actual physical constraints, and the estimation of the robot pose and position is continuously optimized through the constraint conditions in the Kalman filter, so that the cumulative error in the short-term motion is reduced.

[0027] Referring to Figure 5 As shown in the figure, the map feature layering module specifically comprises: A point cloud layering unit: the unit divides the point cloud map into a track surface layer, a device layer and a background layer, and classifies based on geometric and semantic information; A feature extraction unit: the unit extracts key geometric features of the track catenary, sleeper and turnout from the point cloud, and generates feature descriptors.

[0028] Specifically, the track surface layer is divided according to the characteristics that the track surface is usually located near the ground, a height threshold is set, the point cloud of the track surface layer is extracted, the track surface plane is fitted through the RANSAC algorithm, the flat track surface point cloud is separated, and the point cloud range of the track surface layer is further refined according to the track width; The device layer is divided according to the typical height range of the track device, the point cloud of the device layer is extracted, the geometric structure of the device layer is identified based on the curvature and normal vector of the point cloud, such as the linear features of the catenary or the block features of the signal device, and the point cloud of the device layer is divided into independent device objects through a clustering algorithm; The background layer is divided into the background layer by the point cloud that does not belong to the track surface layer and the device layer, additional filtering is performed on the background layer point cloud, and sparse points or irrelevant noise points are removed; The track catenary feature extraction identifies the linear structure of the catenary based on the normal vector and curvature of the point cloud, calculates the local curvature of the catenary point cloud, extracts key points, fits the geometric model of the catenary, and records the position and direction; The sleeper feature extraction detects the boundary points and center points of the sleepers based on the rectangular geometric characteristics of the sleepers in the track surface layer, calculates the spacing between adjacent sleepers, verifies whether it conforms to the standard sleeper spacing, and determines the orientation and inclination angle of the sleepers through normal vector analysis; The turnout feature extraction identifies the complex geometric shape of the turnout area, extracts key feature points of the turnout based on the curvature change and density distribution of the point cloud, and divides the point cloud of the turnout area into substructures.

[0029] Referring to Figure 6 As shown in the figure, the data tight coupling module specifically comprises: Error calculation unit: the unit constructs error model for lidar, IMU and odometry data respectively, and quantifies the deviation between each sensor data and the pose of the robot; Joint optimization objective function unit: the unit constructs a weighted least squares objective function based on each error, and optimizes the pose, velocity and IMU bias of the robot by introducing motion constraints; LM algorithm solving unit: the unit solves the joint optimization objective function by LM algorithm, and obtains the optimal pose of the robot.

[0030] Specifically, the lidar feature point matching error is calculated based on the feature point matching algorithm, and the matching error between the current point cloud and the reference point cloud is calculated; the IMU pre-integration error is calculated based on the acceleration and angular velocity data of the IMU, and the pre-integrated pose, velocity and position change are calculated; the odometry motion error is calculated based on the pulse signal of the odometry, and the relative displacement and velocity of the robot are calculated; and the three errors are converted into a unified form; The joint optimization objective function is solved by LM algorithm, the pose, velocity and bias of the robot are initialized based on the IMU pre-integration or odometry data, the Jacobian matrix of the objective function is calculated, which represents the partial derivative of the state variable to the error, the initial value of the damping factor of the LM algorithm, the number of iterations and the convergence threshold are configured, the value of the current objective function is calculated, the state variable is updated based on the Jacobian matrix, the damping factor is dynamically adjusted, the convergence speed of gradient descent and Newton method is balanced, and the iteration is repeated until the objective function converges or the maximum number of iterations is reached.

[0031] Referring to Figure 7 As shown, the dynamic object perception and elimination module specifically includes: Semantic segmentation unit: the unit performs semantic segmentation on the RGB image of the RGB-D camera, generates semantic labels for each pixel, and extracts the region of the dynamic object and records its mask according to the semantic labels; Dynamic object recognition unit: the unit combines the semantic segmentation result with the depth image to generate the 3D point cloud of the dynamic object, identifies the dynamic object in the continuous frame through a tracking algorithm, and projects the point cloud of the dynamic object into the laser radar point cloud coordinate system to obtain the corresponding laser point cloud subset; Dynamic mask generation unit: the unit generates a mask for the dynamic object based on the obtained laser point cloud subset.

[0032] Specifically, for each point in the laser point cloud, initialize the mask label, the initial state is static point, according to the dynamic object recognition result, mark the corresponding dynamic object point in the laser point cloud as dynamic point, expand the boundary point of the dynamic object, ensure to cover all possible related dynamic points, smooth the boundary of the dynamic mask through the clustering algorithm, reduce the labeling error of fragmentation or isolated points, check the coverage and accuracy of the dynamic mask through visualization or statistical analysis, ensure that the dynamic points are correctly labeled; According to the dynamic mask, remove the points marked as dynamic from the laser point cloud, retain the static point cloud, check the integrity of the point cloud after removing the dynamic points, such as whether the track area is retained intact, adjust the mask range if necessary, input the static point cloud after removing the dynamic points into the SLAM optimization module for pose estimation and map updating, feed back the tracking information of the dynamic object to the SLAM system to assist long-term dynamic object management, output the optimized static point cloud and dynamic mask information, and store them in a standard format.

[0033] Referring to Figure 8 As shown in the figure, the global positioning error compensation module specifically includes: Marker detection unit: the unit detects the feature markers on the track through the sensor, triggers the repositioning process; Particle filter repositioning unit: the unit performs global repositioning based on the particle filter algorithm, corrects the cumulative error of the inertial navigation, and updates the pose estimation; Inertial navigation error compensation unit: the unit calculates and compensates the cumulative error of the inertial navigation system based on the particle filter repositioning result; Pose updating unit: the unit outputs the compensated global pose to other modules, monitors the running state of the module in real time, and adjusts the parameters as needed.

[0034] Specifically, the particle filter repositioning unit specifically includes the following steps: According to the current known initial pose and uncertainty, initialize the particle group, each particle represents a possible state, fuse the detection data from INS and feature markers to update the weight of the particle, use the state transition model to predict each particle, update the position and attitude of the particle according to the sensor input, calculate the matching degree of each particle with the true state, and update the weight according to the likelihood, resample according to the weight of the particle, generate a new particle group, retain those high-weight particles, and accurately estimate the current pose, calculate the weighted average value from the resampled particle group to get the final global pose estimation; The specific steps of the inertial navigation error compensation unit include: The results of the particle filter are compared with the current estimate of the inertial navigation system, the errors of the position and the attitude are calculated, according to the calculated errors, the compensation amount is applied to the state estimate of the INS to eliminate the errors, and the estimated value of the INS is updated to reflect the compensation of the global positioning error.

[0035] Referring to Figure 9 As shown in the result output visualization module specifically includes: 6DoF pose estimation unit: the unit is used to output the updated acquired pose data, including the position in the three-dimensional space and the rotation angle around the three axes; Track equipment state semantic map unit: the unit integrates the 6DoF pose estimation result and the track equipment state data to generate a map with semantic labels, representing the spatial position, environmental characteristics and state information of the equipment; Visualization unit: the unit displays the generated semantic map and equipment state data through a graphical interface.

[0036] It should be noted that: the above-mentioned sequence of the embodiments of the present application is only for description, not representing the advantages and disadvantages of the embodiments. And the above description is for a specific embodiment of the present application. In addition, the processes depicted in the drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multi-task processing and parallel processing are possible or may be advantageous.

[0037] Each embodiment in the specification is described in a progressive manner, and the same or similar parts between each embodiment can be referred to each other. Each embodiment focuses on the difference from other embodiments.

[0038] The above is only the preferred embodiment of the present application, and does not limit the present application. Any modification, equivalent replacement, improvement, etc. within the principles of the present application shall be included in the protection scope of the present application.

Claims

1. A positioning system for an intelligent inspection robot for rail transit based on laser SLAM and inertial navigation, characterized in that, include: Multi-source data acquisition module; point cloud filtering module; Inertial navigation initialization module; Map feature layering module; Data tightly coupled module; Dynamic object awareness and culling module; Global positioning error compensation module; result output visualization module.

2. The positioning system for an intelligent inspection robot for rail transit based on laser SLAM and inertial navigation according to claim 1, characterized in that, The multi-source data acquisition module specifically includes: Data acquisition unit: The unit records the timestamp and three-dimensional coordinates of each point on the track using a lidar to acquire track point cloud data, acquires acceleration and angular velocity through an I2C / SPI interface, acquires the pulse signal of the odometer through a pulse counter, records the number of wheel and axle rotations and displacement data, and acquires RGB and depth images through an RGB-D camera; Time synchronization unit: The unit synchronizes the sampling start time of the lidar, IMU, odometer and RGB-D camera through hardware trigger signal, and adds a high-precision timestamp to the data packet of each sensor, and uses the master time source for calibration; Data processing module: The module preprocesses and fuses the collected multi-source data.

3. The positioning system for an intelligent inspection robot for rail transit based on laser SLAM and inertial navigation according to claim 1, characterized in that, The point cloud filtering module specifically includes: Parameter initialization unit: The unit initializes the size of the voxel grid based on the point cloud density of the track region, sets the initial threshold for point cloud filtering, predefines the spatial range of the track region, and preprocesses the point cloud. Voxel grid filtering unit: The unit divides the point cloud space into a three-dimensional voxel grid, assigns each point in the point cloud to the corresponding voxel, calculates the weighted center for the points in each voxel, and removes voxels with a number of points lower than the density threshold. Dynamic parameter adjustment unit: The unit dynamically adjusts the filtering parameters based on the point cloud characteristics to obtain high-quality feature points under different orbital scenarios; Feature point extraction unit: The unit segments a subset of the point cloud of the track region from the filtered point cloud and identifies key feature points of the track.

Images