A mobile tunnel detection robot and a tunnel detection method

Abstract

The application belongs to the technical field of tunnel detection, and specifically discloses a mobile tunnel detection robot and a tunnel detection method. The tunnel detection robot comprises a robot main body and a data acquisition module. The data acquisition module comprises a laser radar, a wheel speed meter and an inertial measurement unit (IMU). The robot main body is rigidly connected with the data acquisition module, and the coordinate transformation relationship between different sensors can be obtained through rigid transformation. The robot main body is used for driving the robot to move. Through error state Kalman filtering (ESKF), the observation data of the laser radar, the observation data of the IMU and the observation data of the wheel speed meter are fused, and a three-dimensional point cloud model of a tunnel to be detected under a world coordinate system is obtained. Through the application, the three-dimensional point cloud model of the tunnel to be detected under the world coordinate system can be efficiently and accurately obtained. The three-dimensional point cloud model can be used for tunnel disease identification and positioning, so that the detection quality and efficiency are improved, and efficient tunnel maintenance is realized.

Description

Technical Field

[0001] This application belongs to the field of tunnel inspection technology, and more specifically, relates to a mobile tunnel inspection robot and a tunnel inspection method. Background Technology

[0002] Tunnels are a crucial component of transportation systems, and the safety monitoring of tunnel structures during operation is of paramount importance. Traditional deformation monitoring technologies, such as convergence meters and total stations, acquire data that is too discrete to comprehensively reflect the overall deformation characteristics of the tunnel. Traditional monitoring methods are characterized by high workload, long cycles, significant environmental impact, and limited monitoring results, making them ill-suited to the ever-increasing demands of tunnel inspection. Three-dimensional laser scanning technology, a relatively new technology developed in recent years, can rapidly acquire large-area, high-precision point clouds of tunnel structures, overcoming the shortcomings of traditional single-point measurements. However, conventional station-based scanning monitoring involves complex technical processes such as station relocation and point cloud stitching, resulting in cumbersome operations and low efficiency, hindering its widespread adoption in the tunnel monitoring field. How to efficiently maintain tunnels is a pressing technical problem that needs to be solved in this field. Summary of the Invention

[0003] To address the shortcomings of existing technologies, the purpose of this application is to improve the quality and efficiency of tunnel inspection by using an inspection robot to generate a tunnel point cloud model, thereby achieving efficient tunnel maintenance.

[0004] To achieve the above objectives, in a first aspect, this application provides a mobile tunnel inspection robot, comprising: a robot body and a data acquisition module, wherein the data acquisition module includes a lidar, a wheel speedometer, and an inertial measurement unit (IMU);

[0005] The robot body is rigidly connected to the data acquisition module, and the coordinate transformation relationship between different sensors (LiDAR, wheel speedometer, and inertial measurement unit IMU) can be obtained through rigid transformation;

[0006] The main body of the robot is used to drive the robot's movement; through the Error State Kalman Filter (ESKF), the observation data of the lidar, the observation data of the inertial measurement unit (IMU), and the observation data of the wheel speed meter are fused to obtain a three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

[0007] In one possible implementation, the robot body includes: a mobile base, a power supply, a power control board, and an onboard computer;

[0008] The mobile base is used to drive the robot to move;

[0009] The power supply provides power to the power control board, which outputs the power supply voltage to the onboard computer, mobile base, and data acquisition module.

[0010] The onboard computer and data acquisition module are connected via communication.

[0011] The onboard computer is used to fuse the observation data from the lidar, the observation data from the inertial measurement unit (IMU), and the observation data from the wheel speedometer through the error state Kalman filter (ESKF) to obtain a three-dimensional point cloud model of the tunnel to be inspected in the world coordinate system.

[0012] In one possible implementation, the robot body also includes a motor drive board;

[0013] The mobile base is equipped with four wheels and four motors, with one motor connected to one wheel.

[0014] The onboard computer drives the motor via a motor driver board.

[0015] In one possible implementation, the robot body is equipped with a fixed base, and the robot body and the data acquisition module are rigidly connected through the fixed base.

[0016] Secondly, this application provides a tunnel detection method applied to a mobile tunnel detection robot described in the first aspect or any possible implementation thereof, the method comprising:

[0017] By using Error State Kalman Filter (ESKF), the observation data from the lidar, the inertial measurement unit (IMU), and the wheel speedometer are fused to determine the pose (position and rotation) of the data acquisition module.

[0018] Based on the pose of the lidar and the point cloud data observed by the lidar, a three-dimensional point cloud model of the tunnel to be detected in the world coordinate system is obtained. The pose of the lidar is obtained by transforming the pose of the data acquisition module based on the positional relationship between the lidar and the data acquisition module.

[0019] In one possible implementation, the above-mentioned error state Kalman filter (ESKF) fuses the observation data from the lidar, the inertial measurement unit (IMU), and the wheel speedometer to determine the pose of the data acquisition module, including:

[0020] By using the forward propagation of the Error State Kalman Filter (ESKF), the observation data from the Inertial Measurement Unit (IMU) and the observation data from the wheel speedometer are fused to obtain the first fused data, which includes pose.

[0021] By using the backpropagation of the Error State Kalman Filter (ESKF), the fused data from the forward propagation (first fused data) and the observation data from the lidar are fused to obtain the second fused data.

[0022] Here we will explain forward propagation. Forward propagation uses data from the IMU and wheel speedometer to predict the state variables and their covariance matrix at the current moment through integration, providing prior estimates for subsequent backward propagation.

[0023] Here we will explain backpropagation. Backpropagation uses observation data from lidar (such as point cloud features) to correct the predictions from forward propagation by updating the error state linearly, and outputs the optimal state estimate.

[0024] In one possible implementation, the above-mentioned forward propagation of the Error State Kalman Filter (ESKF) fuses the observation data from the Inertial Measurement Unit (IMU) and the observation data from the wheel speedometer to obtain the first fused data, including:

[0025] Based on the triaxial acceleration and triaxial angular velocity data from the inertial measurement unit (IMU), the kinematic state of the data acquisition module is recursively deduced to obtain the velocity and pose of the data acquisition module.

[0026] The speed values ​​of the data acquisition module are updated using the speed information observed by the wheel speed gauge.

[0027] In one possible implementation, the aforementioned backpropagation of the Error State Kalman Filter (ESKF) fuses the forward propagation fused data (first fused data) and the lidar observation data to obtain the second fused data, including:

[0028] Based on the pose information in the first fused data, the point cloud data is distorted.

[0029] The solution that minimizes the residual between the current frame point cloud data observed by the lidar and the historical map is obtained by using the Iterative Closest Point (ICP) algorithm.

[0030] Based on the solution with the minimum residual and the first fused data, the pose information is updated by constructing the observation equation and calculating the Kalman gain;

[0031] The historical map is a set of point clouds in the world coordinate system determined based on historical frame point cloud data observed by lidar.

[0032] The above distortion correction is illustrated here. Due to the time delay in point cloud acquisition, the pose predicted by forward propagation needs to be backpropagated to each point cloud sampling time to eliminate motion distortion.

[0033] Understandably, an IMU can measure the acceleration and angular velocity of the data acquisition module, and can perform prior state deduction based on IMU data, correct distortion of the lidar point cloud and use it as the initial value for point cloud matching, thereby improving the accuracy and robustness of positioning and mapping.

[0034] In one possible implementation, after finding the solution that minimizes the residual between the current frame point cloud data observed by the lidar and the historical map using the Iterative Closest Point (ICP) algorithm, the following steps are also included:

[0035] If the minimum eigenvalue corresponding to the residual between the current frame point cloud data observed by the lidar and the historical map is less than a preset threshold, the pose of the data acquisition module calculated based on the current frame point cloud data will be decomposed, and the pose solution in the non-degenerate direction will be retained.

[0036] The non-degradation direction is perpendicular to the extension direction of the tunnel's long linear structure.

[0037] Understandably, when tunnel point clouds degrade, the components of the solution in the degraded direction are discarded, while the components in the non-degraded direction are retained. The wheel velocity meter can provide velocity observations for the data acquisition module to update the velocity state variables in the state estimation. When tunnel point clouds degrade, the wheel velocity meter provides state estimation in the degraded direction, making the positioning more robust.

[0038] One possible implementation also includes:

[0039] In the robot operating system ROS, observation data from each sensor are published as topics.

[0040] By subscribing to sensor topics, you can obtain observation data from lidar, inertial measurement unit (IMU), and wheel speedometer.

[0041] Overall, the technical solutions conceived in this application have the following beneficial effects compared with the prior art:

[0042] By driving the robot to move, it can collect data along the long linear structure of the tunnel. Through ESKF, it fuses observation data from the lidar, inertial measurement unit (IMU), and wheel speedometer to accurately obtain the pose of the data acquisition module. The robot body is rigidly connected to the data acquisition module. Based on the positional relationship between the lidar and the data acquisition module, the pose of the data acquisition module is transformed to obtain the lidar's pose. Combined with the point cloud data observed by the lidar, a three-dimensional point cloud model of the tunnel under inspection in the world coordinate system can be efficiently and accurately obtained. This three-dimensional point cloud model can be used for tunnel defect identification and localization, improving inspection quality and efficiency, and enabling efficient tunnel maintenance.

[0043] The robot can adapt to various complex terrains and environments, and efficiently acquire data. The inspection robot, using a laser-inertial positioning system, effectively adapts to the unavailability of GNSS (Global Navigation Satellite System) in tunnels, achieving precise robot positioning. This inspection robot is suitable for large-scale scanning and mapping work, capable of mobile operation and recording acquired mobile data. It can quickly and efficiently acquire the spatial coordinates of the scanned area in complex environments, enabling 3D reconstruction of the environmental point cloud model, showing significant advantages compared to traditional fixed systems. Attached Figure Description

[0044] Figure 1 This is a schematic diagram of the power system of the mobile tunnel inspection robot provided in the embodiments of this application;

[0045] Figure 2 This is a schematic diagram of the mobile base system for the mobile tunnel inspection robot provided in an embodiment of this application;

[0046] Figure 3 This is a physical image of the mobile tunnel inspection robot sensing system provided in the embodiments of this application;

[0047] Figure 4 This is a schematic diagram of the tunnel inspection robot software system provided in the embodiments of this application;

[0048] Figure 5 This is a schematic flowchart of the tunnel detection method provided in the embodiments of this application;

[0049] Figure 6 This is a schematic diagram of the degradation process provided in the embodiments of this application;

[0050] Figure 7 This is an internal view of the point cloud model reconstructed by the detection method provided in this application embodiment;

[0051] Figure 8 This is an overall view of the point cloud model reconstructed by the detection method provided in this application embodiment;

[0052] Figure 9 This is a schematic diagram of the point cloud model reconstructed by the detection method provided in this application embodiment in a satellite map. Detailed Implementation

[0053] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0054] In the embodiments of this application, the terms "exemplary" or "for example" are used to indicate that something is an example, illustration, or description. Any embodiment or design that is described as "exemplary" or "for example" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or design. Specifically, the use of the terms "exemplary" or "for example" is intended to present the relevant concepts in a specific manner.

[0055] In the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more, for example, multiple processing units means two or more processing units, multiple elements means two or more elements, etc.

[0056] The embodiments of this application are described below with reference to the accompanying drawings.

[0057] The tunnel inspection robot consists of a robot body and a data acquisition module. The robot body and the data acquisition module are rigidly connected by a mounting base. The data acquisition module is powered by a power module and power control board located in the robot body. The onboard computer in the robot body communicates with the data acquisition module via a network cable.

[0058] The robot's main body includes a mobile base, power supply, motor drive board, power control board, and onboard computer.

[0059] The power supply provides power to the onboard computer, mobile dock, and data acquisition module via a power control board. For example... Figure 1 As shown, the power control board provides 5V, 12V, and 24V power supply interfaces to meet the needs of different sensors. The mobile base is made of lightweight aluminum alloy and is equipped with four wheels and four motors. Figure 2 As shown, a power supply is installed at the bottom center of the mobile base. The mobile base includes an openable cover, which is locked in place by a latch. An onboard computer with various interfaces is fixed inside the robot body to integrate data from various sensors. A motor drive board is also fixed inside the robot body to drive the motors and control the robot's movement. A mounting base is fixed to the front of the upper end of the robot body, and a data acquisition module is mounted on the mounting base.

[0060] like Figure 3As shown, the data acquisition module includes a 3D LiDAR, wheel speedometer, and IMU. The LiDAR is mounted via a steel frame connector, and the coordinate relationships between the sensors can be obtained through rigid transformation. A unified coordinate system is the basis for multi-sensor information fusion. The LiDAR is used to observe point cloud data of the tunnel to be inspected; the IMU (horizontally mounted) is used to observe the pose and velocity of the data acquisition module; and the wheel speedometer is used to observe the travel speed of the data acquisition module. The data acquisition module integrates and saves the sensor data in the ROS (Robot Operating System) via an onboard computer for subsequent processing.

[0061] The 3D LiDAR features real-time data transmission and reception, 360-degree full coverage, 3D distance measurement, and calibration reflection measurement. The LiDAR has an effective measurement range of 100 meters and supports 16 channels, 300,000 points / second, a 360° horizontal field of view, and a 30° vertical field of view.

[0062] like Figure 4 As shown, by integrating and visualizing the data from the sensor system through the ROS system, real-time laser point cloud, wheel speed meter, and IMU data can be obtained and published in the ROS system as topics. The tunnel detection method of this application will obtain real-time data for algorithm analysis in the ROS system by subscribing to sensor topics.

[0063] The tunnel detection method of this application includes the following steps:

[0064] Step S101: Acquire data from the lidar, IMU, and wheel speed meter located on the data acquisition module when the tunnel to be inspected is being inspected; the tunnel to be inspected is a long linear structure with a hollow interior, and the data acquisition module performs inspection inside it.

[0065] Step S102: Based on ESKF (Error State Kalman Filter), the data observed by the lidar, IMU, and wheel speed sensor are fused to determine the velocity and pose of the data acquisition module. Specifically, the data from the IMU and wheel speed sensor are fused once via forward propagation (the prediction step in ESKF), and then fused a second time via backward propagation (the update step in ESKF) with the point cloud data observed by the lidar. During backward propagation, if the minimum eigenvalue corresponding to the residual between the current frame point cloud data observed by the lidar and the historical map is less than a preset threshold, the pose of the data acquisition module calculated based on the current frame point cloud data is decomposed, and only the pose solution in the non-degenerate direction is retained for secondary fusion. The historical map is a set of point clouds in the world coordinate system determined based on the historical frame point cloud data observed by the lidar, with the non-degenerate direction perpendicular to the extension direction of the long linear structure.

[0066] Understandably, to obtain a 3D point cloud model of the tunnel under test, point cloud data from lidar observation and lidar velocity and pose information are required. In the tunnel environment with the aforementioned long linear structure, it is difficult to obtain the aforementioned velocity and pose information through traditional positioning methods (such as positioning using the Global Navigation Satellite System GNSS). Therefore, it is necessary to fuse lidar, IMU, and wheel speed measurement data to achieve this.

[0067] In this process, the aforementioned backpropagation includes:

[0068] An observation equation is constructed based on point cloud data observed by lidar and the pose obtained by forward propagation fusion.

[0069] The Jacobian matrix is ​​constructed based on the partial derivatives of the observation equation with respect to the state variables after forward propagation fusion. The Jacobian matrix is ​​then transposed and multiplied by the Jacobian matrix to obtain the observation matrix corresponding to the observation equation.

[0070] Find the minimum eigenvalue of the observation matrix. If the minimum eigenvalue is less than a preset threshold, the direction of the eigenvector corresponding to the eigenvalue is taken as the degradation direction. The pose of the data acquisition module calculated based on the point cloud data of the current frame is decomposed, and only the pose solution of the non-degradation direction is retained. The degradation direction is the extension direction of the long linear structure, and the non-degradation direction is perpendicular to the extension direction of the long linear structure.

[0071] Furthermore, for example, the observation equations are constructed, including:

[0072] ;

[0073] in, This represents the actual measurement value obtained from point cloud data observed by lidar. The observation equations representing state variables, This represents the state quantity after forward propagation prediction. Indicates Gaussian white noise;

[0074] The above Jacobian matrix for: ;

[0075] The above observation matrix is ​​as follows: .

[0076] In a more specific embodiment, based on the ESKF framework, the raw data consists of speed information obtained from the wheel speedometer, three-axis acceleration and three-axis angular velocity obtained from the IMU, and point cloud data obtained from the lidar. By fusing the data from multiple sensors, the 18-dimensional state of the data acquisition module is estimated, which consists of six three-dimensional vectors: position, rotation, velocity, accelerometer bias, gyroscope bias, and gravitational acceleration g.

[0077] like Figure 5 As shown, the first step is the forward propagation process, which integrates the IMU and wheel velocities. The forward propagation involves using the IMU's triaxial acceleration and angular velocity data to recursively deduce the kinematic state of the data acquisition module, obtaining pose information (including position and rotation). Then, the wheel velocities are used to update the velocity values ​​in the state variables. The backward propagation process involves using the pose information from the forward propagation to correct the distortion of the laser point cloud. Then, the point cloud residual is calculated using an iterative nearest-point algorithm. The solution that minimizes the point-to-line / surface residual is obtained through iterative solving. Finally, the Kalman gain is calculated to update the pose information in the state variables.

[0078] Specifically, as an example, the IMU and wheel speed sensor are fused together for forward propagation, which includes the following two main parts:

[0079] Part 1: Integrating IMU measurements allows for state recursion. Acceleration integration yields velocity, velocity integration yields position, and angular velocity integration yields rotation, thus constructing the corresponding kinematic equations. These equations are then discretized, and error states are propagated.

[0080] ;

[0081] in, The kinematic equations of ESKF are represented; the vector 𝑥 represents the predicted state. Represents the error state vector. This represents the input vector, including acceleration and angular velocity measurements. This represents a random perturbation vector, including velocity, attitude, acceleration, and angular velocity. express Regarding the error state Jacobian matrix, express Regarding the perturbation vector The Jacobian matrix.

[0082] In the error-state Kalman filter, the error covariance matrix of the prediction propagation part is obtained by linearizing the error-state kinematics. The propagation equation of the error covariance matrix is ​​as follows:

[0083] ;

[0084] in, The covariance matrix represents the state variables. and yes Regarding the error state The Jacobian matrix of the perturbation vector 𝑖 , , It is the covariance matrix of the perturbation vector; This is the assignment operator.

[0085] Part Two: The data acquisition module travels normally in the tunnel and can obtain the speed of the detection robot along the direction of the vehicle's movement through the wheel speed gauge. When there are no bumps or sideslips, the kinematic constraints of the data acquisition module will not generate lateral or vertical speeds. At this time, the three-axis speed values ​​of the vehicle can be obtained.

[0086] In the process of state estimation, the position, velocity and rotation in the state variables are first obtained by integrating the IMU data. After obtaining the velocity measurement value, the state variables are updated according to the velocity value.

[0087] Specifically, this method discards the translational increments of the radar odometry in the longitudinal direction of the tunnel (i.e., the degradation direction) and selects to fuse wheel velocity measurements for velocity observation to achieve state correction. The wheel velocity measurement motion model used is a two-wheel differential drive motion model, and only the velocity value of the system in the forward direction (x-direction) is retained; only the wheel velocity measurement needs to compensate for the observations in the degradation direction. The velocity measurement in the robot's body frame is as follows:

[0088] ;

[0089] in, This is the robot's speed in the forward direction, given by the average of the speed measurements of the left and right wheels:

[0090] ;

[0091] Furthermore, the IMU is mounted on the robot's longitudinal geometric centerline, with the IMU's x-axis coinciding with this centerline. Therefore, the velocity measurements in the robot's body coordinate system can be considered... The robot's velocity in the IMU coordinate system equal:

[0092] ;

[0093] The above equation represents the real-time velocity measured by the system in the local coordinate system. To integrate it into the ESKF framework, it needs to be transformed to the world coordinate system. Assume the current pose is... Therefore, the robot's velocity measurement in the global coordinate system can be expressed as:

[0094] ;

[0095] In the ESKF framework, this velocity measurement can be directly viewed as an observation of the nominal state rather than the error state. The residual is defined as the difference between the measured velocity of the wheel velocimeter and the predicted velocity of the IMU in the global coordinate system. Therefore, the following observation equation is obtained:

[0096] ;

[0097] in, This is the x-component of the velocity obtained through IMU pre-integration in the prediction step of the Kalman filter. The observation equation is calculated with respect to the Jacobian matrix of the nominal state as follows:

[0098] ;

[0099] Finally, the Kalman gain is calculated using the following formula to update the state variables:

[0100] ;

[0101] ;

[0102] ;

[0103] Because the velocity was observed, and combined with the kinematic model of the IMU, the calculated pose did not diverge rapidly.

[0104] As a further example, the aforementioned backpropagation includes the following two main parts:

[0105] Part 1: The observation model can be written as follows:

[0106] ;

[0107] in, This is the observation equation, where V is Gaussian white noise.

[0108] Calculate the Kalman gain based on the observation equation, update the state, and propagate the error to obtain:

[0109] ;

[0110] ;

[0111] ;

[0112] in, It is Kalman gain. It is the predicted state quantity. Represents the optimal estimated update value of the state variables; Jacobian matrix , Represents the identity matrix.

[0113] Update the predicted state as follows:

[0114] ;

[0115] ;

[0116] ;

[0117] ;

[0118] ;

[0119] in , , , , These represent the position, velocity, rotation, accelerometer zero bias, and gyroscope zero bias after the state update, respectively. , , , , This represents the optimal estimate of each component in the error state variable. Indicates the rotation angle. It represents a generalized multiplication.

[0120] Specifically, the actual measurement values ​​are obtained by solving the point cloud data observed by lidar based on the Iterative Closest Point (ICP) algorithm. The key to the ICP algorithm is to solve for the accurate pose by iteratively finding corresponding points in the source and target point clouds and minimizing the residual between them.

[0121] Let the current iteration number of the iterative Kalman filter be k. Transform the point cloud after backpropagation distortion correction to the world coordinate system:

[0122] ;

[0123] Among them, superscript Represents the world coordinate system; This represents the expected coordinates of the feature point in the world coordinate system after the pose to be solved. This indicates the transformation from the radar laser coordinate system to the IMU coordinate system; Indicates the feature point at the th Coordinate values ​​in the radar laser coordinate system; This represents the pose to be solved; Indicates the first One feature point; Indicates the first frameIMU; This represents the total number of feature points.

[0124] Specifically, The residual is generated from the pose to be solved, the distance from the feature point to the matching feature (line or plane). For edge feature points, the residual is the distance from the point to the line; for planar feature points, the residual is the distance from the point to the plane. Definition It is the normal vector of the plane or the edge. If it is a point on a plane or edge, then the residual is... Represented as:

[0125] ;

[0126] Among them, the above residuals That is, the obtained observation equation .

[0127] The observation equations of the extended Kalman filter are derived by linearizing the observation equations. Subsequent iterations are needed to optimize the linearized observation equations and reduce the errors caused by the linearization. It can be proven that the solution method of the iterative extended Kalman filter and the optimization method based on Newton's method are mathematically equivalent and can provide an unbiased estimate of the problem.

[0128] If the residual is less than the threshold during the iteration process, the iteration is considered to have converged, and the optimal state variable is estimated as follows:

[0129] ;

[0130] state variables Update and convert to the form of a Lie group. Each lidar point cloud in the k-th scan frame The map update is completed by projecting all data into the world coordinate system using the following formula.

[0131] ;

[0132] Through iteration, a tightly coupled state estimation of LiDAR and IMU was achieved. The pose of the tunnel detection machine and the zero bias of the IMU can be updated through LiDAR observations, resulting in more accurate state estimation and global point cloud.

[0133] Part 2: In the problem of simultaneous localization and mapping, degradation refers to the phenomenon that the algorithm may perform poorly in environments with insufficient constraints due to the instability and inaccuracy of state estimation. For example, insufficient observation information in a certain direction or dimension makes it difficult for the algorithm to accurately estimate the state from the observation data.

[0134] LiDAR requires spatial geometric information to iteratively calculate pose information. Since long linear structures lack structural features in their extension direction and belong to a degenerate environment, the state estimation along the long linear extension method is difficult to estimate due to the lack of constraints. Therefore, it is necessary to perform degenerate processing, project the solved pose onto a non-degenerate method, discard the components of the solution in the degenerate direction, and retain the components in the non-degenerate direction.

[0135] The solution process involves an observation matrix. The degradation factor D can be calculated by performing eigenvalue decomposition on the observation matrix, as shown below:

[0136] ;

[0137] Perform feature decomposition on the point cloud feature matrix. for The minimum eigenvalue is the eigenvector direction corresponding to the eigenvalue. If the minimum eigenvalue is less than the threshold, then the direction of the eigenvector corresponding to the eigenvalue corresponds to the direction of degradation.

[0138] Define the following matrix:

[0139] ;

[0140] in, Represents the degeneration direction vector; Represents a non-degenerate direction vector; This represents all the direction vectors obtained from the solution; This indicates the total number of directions of the solution.

[0141] The corresponding reprojection components are:

[0142] ;

[0143] in, It is an estimate of the actual state. It is an estimate obtained after nonlinear optimization. To optimize the solution, This represents the increment of the solution in the direction of degradation; This represents the increment of the solution in the non-degenerate direction.

[0144] like Figure 6 As shown, the solution is decomposed into degenerate and non-degenerate directions, represented by orange and blue, respectively.

[0145] Retaining the solutions in the non-degenerate directions, we obtain the final solution as follows:

[0146] ;

[0147] in, This represents the optimal estimate obtained after updating the state variables.

[0148] Further, optionally, during backward propagation, the point cloud data of the lidar in the non-degenerate direction (i.e., only the point cloud data of the lidar in this direction needs to be fused a second time to update the pose information) is fused with the data after the first fusion.

[0149] Step S103: Combine the point cloud data observed by pose and lidar to perform fusion mapping to obtain a three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

[0150] Specifically, by combining point cloud data from pose and lidar observations, a three-dimensional point cloud model of the tunnel to be inspected in the world coordinate system is obtained.

[0151] It is understandable that the pose used in step S103 is the pose of the lidar, which can be obtained by transforming the pose of the data acquisition module output in step S102 according to the positional relationship between the lidar and the data acquisition module.

[0152] It should be noted that, in order to improve the accuracy of the 3D point cloud model, this step combines all the point cloud data observed by LiDAR to construct the 3D point cloud model.

[0153] For example, Figure 7 The detection method proposed in this application reconstructs an internal view of the point cloud model. Figure 8 This is an overall view of the point cloud model reconstructed by the detection method proposed in this application. Figure 9 This is a schematic diagram of the point cloud model reconstructed by the detection method proposed in this application in a satellite map.

[0154] In this application, the data acquisition module uses LiDAR as the main sensor to achieve the robot's localization and mapping functions. However, using only LiDAR for localization and mapping has many limitations because it lacks the ability to detect the inertial state of the data acquisition module itself, resulting in significant deviations in point cloud measurement results when the data acquisition module moves at high speeds or experiences bumps. IMU can measure the acceleration and angular velocity of the data acquisition module, and can perform prior state recursion based on IMU data, correcting distortion in the LiDAR point cloud and using it as the initial value for point cloud matching, thereby improving the accuracy and robustness of localization and mapping. When the tunnel point cloud degrades, the components of the solution in the degraded direction are discarded, while the components in the non-degraded direction are retained. The wheel velocity sensor can provide velocity observations for the data acquisition module to update the velocity state variables in the state estimation. When the tunnel point cloud degrades, the wheel velocity sensor provides state estimation in the degraded direction, making the localization more robust. Therefore, this application integrates IMU, LiDAR, and wheel velocity sensor data to achieve localization and mapping functions in the tunnel environment.

[0155] It should be understood that expressions such as “comprising” and “may include” used in this application indicate the existence of the disclosed functions, operations, or constituent elements, and do not limit one or more additional functions, operations, and constituent elements. In this application, terms such as “comprising” and / or “having” are to be interpreted as indicating a particular characteristic, number, operation, constituent element, component, or combination thereof, but not to exclude the existence or possibility of adding one or more other characteristics, numbers, operations, constituent elements, components, or combinations thereof.

[0156] In the description of the embodiments of this application, it should be noted that, unless otherwise explicitly specified and limited, the term "connection" should be interpreted broadly. For example, "connection" can be a detachable connection or a non-detachable connection; it can be a direct connection or an indirect connection through an intermediate medium. "Fixed connection" refers to a connection where the relative positional relationship remains unchanged after connection. "Rotary connection" refers to a connection where the components can rotate relative to each other after connection. "Sliding connection" refers to a connection where the components can slide relative to each other after connection. The directional terms mentioned in the embodiments of this application, such as "top," "bottom," "inner," "outer," "left," and "right," are only for reference to the directions in the accompanying drawings. Therefore, the directional terms used are for better and clearer explanation and understanding of the embodiments of this application, and are not intended to indicate or imply that the device or component referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the embodiments of this application.

[0157] Furthermore, the mathematical concepts mentioned in the embodiments of this application, such as symmetry, equality, parallelism, and perpendicularity, are limitations specific to the current technological level, rather than absolute and strict mathematical definitions. Slight deviations are permissible; approximations of symmetry, equality, parallelism, and perpendicularity are all acceptable. For example, "A and B are parallel" means that A and B are parallel or approximately parallel, and the angle between A and B can be between 0 and 10 degrees. "A and B are perpendicular" means that A and B are perpendicular or approximately perpendicular, and the angle between A and B can be between 80 and 100 degrees.

[0158] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A mobile tunnel inspection robot, characterized in that, The method comprises the steps of: a robot body and a data acquisition module, the data acquisition module comprising a laser radar, a wheel speed meter and an inertial measurement unit (IMU); the robot body and the data acquisition module are rigidly connected, and the coordinate transformation relationship between different sensors can be obtained through rigid transformation; the robot body is used for driving the robot to move; the observation data of the laser radar, the observation data of the inertial measurement unit (IMU) and the observation data of the wheel speed meter are fused through error state Kalman filtering (ESKF), and a three-dimensional point cloud model of a tunnel to be detected in a world coordinate system is obtained; the method for fusing the observation data of the laser radar, the observation data of the inertial measurement unit (IMU) and the observation data of the wheel speed meter through error state Kalman filtering (ESKF) and obtaining the three-dimensional point cloud model of the tunnel to be detected in the world coordinate system comprises the steps of: determining the pose of the data acquisition module by fusing the observation data of the laser radar, the observation data of the inertial measurement unit (IMU) and the observation data of the wheel speed meter through error state Kalman filtering (ESKF); obtaining the three-dimensional point cloud model of the tunnel to be detected in the world coordinate system based on the pose of the laser radar and the point cloud data observed by the laser radar, wherein the pose of the laser radar is obtained by transforming the pose of the data acquisition module based on the positional relationship between the laser radar and the data acquisition module; the method for determining the pose of the data acquisition module by fusing the observation data of the laser radar, the observation data of the inertial measurement unit (IMU) and the observation data of the wheel speed meter through error state Kalman filtering (ESKF) comprises the steps of: fusing the observation data of the inertial measurement unit (IMU) and the observation data of the wheel speed meter through the forward propagation of error state Kalman filtering (ESKF) to obtain first fused data, wherein the fused data comprises a pose; fusing the first fused data and the observation data of the laser radar through the backward propagation of error state Kalman filtering (ESKF) to obtain second fused data; the method for fusing the observation data of the inertial measurement unit (IMU) and the observation data of the wheel speed meter through the forward propagation of error state Kalman filtering (ESKF) to obtain first fused data comprises the steps of: kinematically recursively propagating the state of the data acquisition module based on the three-axis acceleration data and the three-axis angular velocity data of the inertial measurement unit (IMU) to obtain the velocity and the pose of the data acquisition module; updating the velocity value of the data acquisition module by using the velocity information observed by the wheel speed meter; the method for fusing the first fused data and the observation data of the laser radar through the backward propagation of error state Kalman filtering (ESKF) to obtain second fused data comprises the steps of: de-distorting the point cloud data based on the pose information in the first fused data; solving the minimum residual solution between the current frame of point cloud data observed by the laser radar and a historical map through the iterative closest point (ICP) algorithm; updating the pose information by constructing an observation equation and calculating a Kalman gain based on the minimum residual solution and the first fused data; wherein the historical map is a point cloud set in the world coordinate system determined according to historical frames of point cloud data observed by the laser radar. After solving the minimum residual solution between the current frame point cloud data observed by the laser radar and the historical map by the iterative closest point algorithm ICP, further comprising: If the minimum eigenvalue corresponding to the residual between the current frame point cloud data observed by the laser radar and the historical map is less than a preset threshold, the pose of the data acquisition module calculated based on the current frame point cloud data is decomposed, and the pose solution of the non-degeneracy direction is retained; when the degeneration occurs, the wheel speed meter provides the state estimation in the degeneration direction; Wherein, the degeneration direction is the extension direction of the long linear structure of the tunnel, and the non-degeneration direction is perpendicular to the extension direction of the long linear structure of the tunnel.

2. The mobile tunnel inspection robot of claim 1, wherein, The robot body comprises a mobile base, a power supply, a power supply control board and an on-board computer; The mobile base is used to drive the robot to move; The power supply provides power for the power supply control board, and the power supply control board outputs a power supply voltage to the on-board computer, the mobile base and the data acquisition module; The on-board computer and the data acquisition module are in communication connection; The on-board computer is used to fuse the observation data of the laser radar, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through error state Kalman filtering ESKF, and obtain a three-dimensional point cloud model of the tunnel to be detected in a world coordinate system.

3. The mobile tunnel inspection robot of claim 2, wherein, The robot body comprises a motor drive board; The mobile base is provided with four walking wheels and four motors, and one motor is connected with one walking wheel; The on-board computer drives the motor through the motor drive board.

4. The mobile tunnel inspection robot according to any one of claims 1-3, wherein, The robot body is provided with a fixing seat, and the robot body and the data acquisition module are rigidly connected through the fixing seat.

5. A tunnel detection method characterized by, Applied to the mobile tunnel detection robot as claimed in any one of claims 1-4, comprising: Fusing the observation data of the laser radar, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through error state Kalman filtering ESKF to determine the pose of the data acquisition module; Based on the pose of the laser radar and the point cloud data observed by the laser radar, a three-dimensional point cloud model of the tunnel to be detected in a world coordinate system is obtained, and the pose of the laser radar is obtained by transforming the pose of the data acquisition module based on the relative position relationship between the laser radar and the data acquisition module; The method for fusing the observation data of the laser radar, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through error state Kalman filtering ESKF to determine the pose of the data acquisition module, comprising: Fusing the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through forward propagation of error state Kalman filtering ESKF to obtain first fused data, and the fused data includes the pose; Fusing the first fused data obtained through the forward propagation and the observation data of the laser radar through backward propagation of error state Kalman filtering ESKF to obtain second fused data; The method for fusing the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through forward propagation of error state Kalman filtering ESKF to obtain first fused data, comprising: Based on the three-axis acceleration data and the three-axis angular velocity data of the inertial measurement unit IMU, the kinematic state of the data acquisition module state is recursively propagated to obtain the velocity and the pose of the data acquisition module; The speed value of the data acquisition module is updated by using the speed information observed by the wheel speed meter; The second fusion data is obtained by fusing the forward propagation fusion data and the observation data of the lidar through back propagation of the error state Kalman filter (ESKF), and the second fusion data comprises: De-distorting the point cloud data based on the pose information in the first fusion data; Solving the minimum residual solution between the current frame point cloud data observed by the lidar and the historical map through the iterative closest point (ICP) algorithm; Updating the pose information by constructing an observation equation and calculating a Kalman gain based on the minimum residual solution and the first fusion data; The historical map is a point cloud set in a world coordinate system determined according to historical frame point cloud data observed by the lidar; After solving the minimum residual solution between the current frame point cloud data observed by the lidar and the historical map through the iterative closest point (ICP) algorithm, the method further comprises: If the minimum eigenvalue corresponding to the residual between the current frame point cloud data observed by the lidar and the historical map is less than a preset threshold, the pose of the data acquisition module calculated based on the current frame point cloud data is decomposed, and the pose solution in a non-degeneracy direction is retained; when a degeneration occurs, the wheel speed meter provides state estimation in a degeneration direction; The degeneration direction is the extension direction of the long linear structure of the tunnel, and the non-degeneration direction is perpendicular to the extension direction of the long linear structure of the tunnel.

6. The tunnel detection method of claim 5, wherein, The method further comprises: Publishing the observation data of each sensor in the form of a topic in the robot operating system (ROS); Obtaining the observation data of the lidar, the observation data of the inertial measurement unit (IMU), and the observation data of the wheel speed meter by subscribing to the sensor topic.

Images