Mobile tunnel detection robot and tunnel detection method

Abstract

The invention belongs to the technical field of tunnel detection, and particularly discloses a mobile tunnel detection robot and a tunnel detection method.The tunnel detection robot comprises a robot body and a data acquisition module, and 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 relation between different sensors can be obtained through rigid transformation; the robot body is used for driving the robot to move. And fusing the observation data of the laser radar, the observation data of the IMU and the observation data of the wheel speedometer through ESKF (Error State Kalman Filter), and obtaining a three-dimensional point cloud model of the tunnel to be detected in a world coordinate system. According to the tunnel detection method and device, 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 recognition and positioning, the detection quality and efficiency are improved, and efficient tunnel maintenance is achieved.

Description

Technical Field

[0001] The present application belongs to the technical field of tunnel detection, and more specifically, relates to a mobile tunnel detection robot and a tunnel detection method. Background Art

[0002] Tunnels are an important part of the transportation system, and safety monitoring of tunnel structures during operation is crucial. Traditional deformation monitoring technologies such as convergence meters and total stations obtain monitoring data that is too discrete and difficult to fully reflect the overall deformation characteristics of the tunnel. Traditional monitoring methods have the characteristics of large workload, long cycle, great impact on the environment, and single monitoring results, which are difficult to adapt to the increasing demand for tunnel detection. Three-dimensional laser scanning technology is a new technology developed in recent years. It can quickly obtain large-area, high-precision tunnel structure point clouds, overcoming the shortcomings of traditional single-point measurement. However, the ordinary station scanning monitoring process has complex technical processes such as moving stations and point cloud splicing. The operation is complex and inefficient, making it difficult to promote in the field of tunnel monitoring. How to efficiently maintain tunnels is a technical problem that needs to be solved urgently in this field. Summary of the invention

[0003] In view of the defects of the prior art, the purpose of this application is to perform tunnel inspection through an inspection robot, generate a tunnel point cloud model, improve the inspection quality and efficiency, and achieve efficient tunnel maintenance.

[0004] To achieve the above-mentioned purpose, in a first aspect, the present application provides a mobile tunnel inspection robot, comprising: 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 is rigidly connected to the data acquisition module, and the coordinate transformation relationship between different sensors (lidar, wheel speed meter and inertial measurement unit IMU) can be obtained through rigid transformation; The robot body is used to drive the robot to move; 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 the three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

[0005] In one possible implementation, the robot body includes: a mobile base, a power supply, a power control board, and an onboard computer; The mobile base is used to drive the robot to move; The power supply provides electric energy to the power control board, and the power control board outputs the supply voltage to the onboard computer, the mobile base and the data acquisition module; The onboard computer and the data acquisition module are communicated and connected; The onboard computer is used to fuse the observation data of the lidar, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through the error state Kalman filter ESKF, and obtain the three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

[0006] In a possible implementation, the robot body further includes a motor drive board; The mobile base is provided with four running wheels and four motors, and one motor is connected to one running wheel; The onboard computer drives the motor via the motor driver board.

[0007] In a possible implementation, the robot body is provided with a fixing seat, and the robot body and the data acquisition module are rigidly connected via the fixing seat.

[0008] In a second aspect, the present application provides a tunnel detection method, applied to the mobile tunnel detection robot described in the first aspect or any possible implementation manner of the first aspect, the method comprising: 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 determine the posture (position and rotation) of the data acquisition module; Based on the posture 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 the world coordinate system is obtained. The posture of the laser radar is obtained by transforming the posture of the data acquisition module based on the positional relationship between the laser radar and the data acquisition module.

[0009] In a possible implementation, the above-mentioned error state Kalman filter ESKF is used to fuse the observation data of the lidar, the observation data of the inertial measurement unit IMU, and the observation data of the wheel speed meter to determine the position and posture of the data acquisition module, including: Through the forward propagation of the error state Kalman filter ESKF, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter are fused to obtain the first fusion data, which includes the position and posture; Through the backward propagation of the error state Kalman filter ESKF, the forward propagated fusion data (the first fusion data) and the observation data of the lidar are fused to obtain the second fusion data.

[0010] Here we explain the forward propagation, which uses the data from the IMU and wheel speed meter to predict the state quantity and its covariance matrix at the current moment through integration, providing a priori estimates for the subsequent backward propagation.

[0011] Backward propagation is explained here. Backward propagation uses the observation data of the lidar (such as point cloud features) to correct the predicted value of the forward propagation through the linear update of the error state and output the optimal state estimate.

[0012] In a possible implementation, the above-mentioned forward propagation of the error state Kalman filter ESKF is used to fuse the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter to obtain the first fusion data, including: Based on the three-axis acceleration data and three-axis angular velocity data of the inertial measurement unit IMU, the kinematic state of the data acquisition module is recursively deduced to obtain the speed and posture of the data acquisition module; The speed information observed by the wheel speed meter is used to update the speed value of the data acquisition module.

[0013] In a possible implementation, the forward propagation fusion data (first fusion data) and the laser radar observation data are fused through the backward propagation of the error state Kalman filter ESKF to obtain the second fusion data, including: Based on the pose information in the first fusion data, the point cloud data is dedistorted; The iterative closest point algorithm (ICP) is used to find the solution with the minimum residual between the current frame point cloud data observed by the lidar and the historical map; Based on the solution with the minimum residual error and the first fusion data, the pose information is updated by constructing the observation equation and calculating the Kalman gain; Among them, the historical map is a point cloud set in the world coordinate system determined based on the historical frame point cloud data observed by the lidar.

[0014] The above-mentioned de-distortion is exemplified here. Due to the time delay in point cloud acquisition, the pose predicted by the forward propagation needs to be back-propagated to each point cloud sampling moment to eliminate motion distortion.

[0015] It can be understood that the IMU can measure the acceleration and angular velocity of the data acquisition module, and can recursively infer the prior state based on the IMU data, dedistort the lidar point cloud and use it as the initial value for point cloud matching, which can improve the accuracy and robustness of positioning and mapping.

[0016] In a possible implementation, after solving the solution with the minimum residual between the current frame point cloud data observed by the laser radar and the historical map by using the iterative closest point algorithm ICP, it also includes: 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 the 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 the non-degenerate direction is retained; Among them, the non-degenerate direction is perpendicular to the extension direction of the long linear structure of the tunnel.

[0017] It can be understood that when the tunnel point cloud is degraded, the components of the solution in the degraded direction are discarded and the components in the non-degraded direction are retained. The wheel speed meter can provide speed observation for the data acquisition module to update the speed state quantity in the state estimation. When the tunnel point cloud is degraded, the wheel speed meter provides a state estimate in the degraded direction, making the positioning more robust.

[0018] In a possible implementation, the method further includes: Publish the observation data of each sensor in the form of topics in the robot operating system ROS; By subscribing to the sensor topic, you can obtain the observation data of the lidar, the observation data of the inertial measurement unit IMU, and the observation data of the wheel speed meter.

[0019] In general, the above technical solutions conceived by this application have the following beneficial effects compared with the prior art: By driving the robot to move, the robot can collect data along the long linear structure of the tunnel. 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 through ESKF, so as to accurately obtain the position and posture of the data acquisition module. The robot body is rigidly connected to the data acquisition module. Based on the position relationship of the lidar relative to the data acquisition module, the position and posture of the data acquisition module is transformed to obtain the position and posture of the lidar. Then, combined with the point cloud data observed by the lidar, the three-dimensional point cloud model of the tunnel to be inspected in 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, improve the detection quality and efficiency, and realize efficient maintenance of the tunnel.

[0020] The robot can adapt to a variety of complex working terrains and environments, and can efficiently obtain data information. The inspection robot can adapt well to the unavailability of GNSS (Global Navigation Satellite System) in tunnels through the laser inertial combined positioning system, and realize accurate positioning of the robot. The above-mentioned inspection robot is suitable for large-scale scanning and mapping work. It can move and scan and record the acquired mobile data. It can quickly and efficiently obtain the spatial coordinates of the scanning area in complex environments, and realize the three-dimensional reconstruction of the environmental point cloud model, which has significant advantages compared with traditional fixed systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 is a schematic diagram of a power supply system of a mobile tunnel inspection robot provided in an embodiment of the present application; Figure 2 It is a schematic diagram of a mobile base system of a mobile tunnel inspection robot provided in an embodiment of the present application; Figure 3 This is a physical picture of the mobile tunnel detection robot sensing system provided in the embodiment of the present application; Figure 4 is a schematic diagram of a tunnel detection robot software system provided in an embodiment of the present application; Figure 5 It is a flow chart of a tunnel detection method provided in an embodiment of the present application; Figure 6 is a schematic diagram of degradation processing provided in an embodiment of the present application; Figure 7 is an internal view of the point cloud model reconstructed by the detection method provided in the embodiment of the present application; Figure 8 It is an overall view of the point cloud model reconstructed by the detection method provided in the embodiment of the present application; Fig. 9 It is a schematic diagram of a point cloud model reconstructed in a satellite map by the detection method provided in an embodiment of the present application. DETAILED DESCRIPTION

[0022] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0023] In the embodiments of the present application, words such as "exemplary" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary" or "for example" is intended to present related concepts in a specific way.

[0024] In the description of the embodiments of the present application, unless otherwise specified, "multiple" means two or more than two. For example, multiple processing units refer to two or more processing units, etc.; multiple elements refer to two or more elements, etc.

[0025] The embodiments of the present application are described below in conjunction with the drawings in the embodiments of the present application.

[0026] The tunnel inspection robot includes a robot body and a data acquisition module. The robot body and the data acquisition module are rigidly connected through a fixed seat. The data acquisition module is powered by a power module and a power control board in the robot body, and the onboard computer included in the robot body and the data acquisition module communicate through a network cable.

[0027] The robot body includes a mobile base, a power supply, a motor drive board, a power control board and an onboard computer.

[0028] The power supply supplies power to the onboard computer, mobile base and data acquisition module through the power control board. 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 has four walking wheels and four motors. Figure 2 As shown, a power source is installed at the middle bottom of the mobile base, and the mobile base includes a cover part that can be opened and closed, and the cover is opened and closed by a cover lock. An onboard computer is fixed inside the robot body, and the onboard computer has various interfaces for integrating data from various sensors. A motor drive board is fixed inside the robot body to drive the motor and control the movement of the robot. A fixed seat is fixed to the front of the upper end of the robot body, and a data acquisition module is installed on the fixed seat.

[0029] like Figure 3 As shown in the figure, the data acquisition module includes a three-dimensional laser radar, a wheel speed meter and an IMU. The laser radar is installed through a steel frame connector. The coordinate relationship between sensors can be obtained through rigid transformation. The unified coordinate system is the basis for multi-sensor information fusion. The laser radar is used to observe the point cloud data of the tunnel to be inspected; the IMU (horizontally installed) is used to observe the position and speed of the data acquisition module; the wheel speed meter is used to observe the driving speed of the data acquisition module. The data acquisition module integrates and saves the sensor data in ROS (Robot Operating System) through the onboard computer for subsequent processing.

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

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

[0032] The tunnel detection method of the present application comprises the following steps: Step S101, obtaining data from a laser radar, an IMU, and a wheel speed meter located on a data acquisition module when detecting a tunnel to be detected; the tunnel to be detected is a long linear structure with a hollow interior, and the data acquisition module performs detection inside it.

[0033] Step S102, based on ESKF (Error State Kalman Filter), the data observed by the laser radar, IMU and wheel speed meter are fused to determine the speed and posture of the data acquisition module; wherein, the data of the IMU and the wheel speed meter are fused once through forward propagation (forward propagation is the prediction step in ESKF), and then the once fused data is secondary fused with the point cloud data observed by the laser radar through backward propagation (backward propagation is the update step in ESKF); during backward propagation, 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 posture of the data acquisition module calculated based on the current frame point cloud data is decomposed, and only the posture solution in the non-degenerate direction is retained for secondary fusion; the historical map is a point cloud set in the world coordinate system determined according to the historical frame point cloud data observed by the laser radar, and the non-degenerate direction is perpendicular to the extension direction of the long linear structure.

[0034] It is understandable that if you want to obtain a three-dimensional point cloud model of the tunnel to be tested, you need the point cloud data observed by the lidar and the speed and posture information of the lidar; in the above-mentioned long-line tunnel environment, it is difficult to obtain the above speed and posture information through traditional positioning methods (such as positioning using the global navigation satellite system GNSS), so it is necessary to integrate the lidar, IMU and wheel speed meter data to achieve it.

[0035] In this process, the above-mentioned back propagation includes: Based on the point cloud data observed by the lidar, the observation equation is constructed in combination with the pose after forward propagation fusion; Construct a Jacobian matrix based on the partial derivative of the observation equation relative to the state quantity after forward propagation fusion, and transpose the Jacobian matrix and multiply it by the Jacobian matrix to obtain the observation matrix corresponding to the observation equation; Solve the minimum eigenvalue of the observation matrix. If the minimum eigenvalue is less than the preset threshold, the eigenvector direction corresponding to the eigenvalue is taken as the degenerate direction, and 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; the degenerate direction is the extension direction of the long linear structure, and the non-degenerate direction is perpendicular to the extension direction of the long linear structure.

[0036] Further, for example, constructing an observation equation includes: ; in, represents the actual measurement value obtained based on the point cloud data observed by the lidar, The observation equation representing the state quantity, represents the state quantity after forward propagation prediction, represents Gaussian white noise; The Jacobian matrix above for: ; The above observation matrix is: .

[0037] In a more specific embodiment, based on the ESKF framework, the original data is the speed information obtained by the wheel speed meter, the three-axis acceleration and three-axis angular velocity obtained by the IMU, and the point cloud data obtained by the lidar. The 18-dimensional state of the data acquisition module is estimated by fusing the data of multiple sensors, which are six three-dimensional vectors: position, rotation, speed, accelerometer bias, gyroscope bias, and gravity acceleration g.

[0038] like Figure 5 As shown in the figure, the first is the process of forward propagation by fusing the IMU and the wheel speed meter. The forward propagation link is to use the three-axis acceleration and three-axis angular velocity data of the IMU to perform kinematic state recursion on the state of the data acquisition module to obtain the posture information (including position and rotation), and then use the wheel speed meter speed information to update the speed value in the state quantity. The backward propagation process is to use the posture information in the forward propagation to dedistort the laser point cloud, and then calculate the point cloud residual through the iterative closest point algorithm. The solution that minimizes the point-to-line surface residual is obtained through iterative solution, and the Kalman gain is calculated to update the posture information in the state quantity.

[0039] Specifically, as an example, the IMU and wheel speed meter are fused for forward propagation, which includes the following two main parts: Part 1: Integrating the IMU measurement values ​​can be used for state recursion. Acceleration integration can get velocity, velocity integration can get position, and angular velocity integration can get rotation, and then the corresponding kinematic equations are constructed. After that, the kinematic equations are discretized and the error states are propagated: ; in, represents the kinematic equation of the ESKF; the vector 𝑥 represents the predicted state, represents the error state vector, represents the input vector, including acceleration and angular velocity measurements, Represents random perturbation vectors, including velocity, attitude, acceleration, and angular velocity; express About Error Status The Jacobian matrix of express About the perturbation vector The Jacobian matrix of .

[0040] The error covariance matrix of the prediction propagation part in the error state Kalman filter is obtained by linearizing the error state kinematics. The error covariance matrix propagation equation is as follows: ; in, represents the covariance matrix of the state quantity, and yes About Error Status and the Jacobian matrix of the perturbation vector 𝑖, , , is the covariance matrix of the perturbation vector; Is the assignment symbol.

[0041] Part 2: When the data acquisition module is driving normally in the tunnel, the speed of the detection robot along the forward direction of the vehicle can be obtained through the measurement value of the wheel speed meter. When there is no bumping and side sliding, the kinematic constraints of the data acquisition module will not produce lateral or vertical speeds, so the three-axis speed values ​​of the vehicle can be obtained.

[0042] In the process of state estimation, the position, velocity and rotation of the state quantity are first obtained by integrating the IMU data. After the velocity measurement value is obtained, the state quantity is updated according to the velocity value.

[0043] Specifically, this method discards the translation increment of the radar odometer in the longitudinal direction of the tunnel (i.e., the degenerate direction), and selects the fusion wheel speed meter for speed observation to achieve state correction. The specific wheel speed meter motion model used is a two-wheel differential drive motion model, and only the speed value of the system in the forward direction (x direction) is retained. Only the wheel speed meter needs to be used to compensate for the observation in the degenerate direction. The speed measurement in the robot body frame is as follows: ; in, is the speed of the robot in the forward direction, given by the average of the left and right wheel speed measurements: ; In addition, the IMU is installed on the longitudinal geometric centerline of the robot, and the x-axis of the IMU coincides with this centerline. Therefore, the velocity measurement value in the robot's body coordinate system can be considered The speed of the robot in the IMU coordinate system equal: ; The above formula is the real-time measured velocity of the system in the local coordinate system. In order to integrate it into the ESKF framework, it needs to be converted to the world coordinate system. Assume that the current posture is , then the speed measurement of the robot in the global coordinate system can be expressed as: ; In the ESKF framework, the speed measurement can be directly regarded as an observation of the nominal state rather than the error state, and the residual is defined as the difference between the speed measured by the wheel speed meter and the speed predicted by the IMU in the global coordinate system. Therefore, the following observation equation is obtained: ; in, is the component of velocity in the x direction obtained by IMU pre-integration in the prediction step of the Kalman filter. The Jacobian matrix of this observation equation relative to the nominal state is calculated as follows: ; Finally, the Kalman gain is calculated using the following formula to update the state: ; ; ; Since the speed is observed and combined with the IMU's kinematic model, the inferred posture will not diverge quickly.

[0044] As an example, the above backward propagation includes the following two main parts: Part I: The observation model can be written as follows: ; in, is the observation equation, and V is Gaussian white noise.

[0045] Calculate the Kalman gain according to the observation equation, update the state, and propagate the error to obtain: ; ; ; in, is the Kalman gain, is the predicted state quantity, Represents the optimal estimated update value of the state quantity; Jacobian matrix , Represents the identity matrix.

[0046] Update the forecast status as follows: ; ; ; ; ; in , , , , They respectively represent the position, velocity, rotation, accelerometer bias, and gyroscope bias after the status is updated. , , , , represents the optimal estimate of each component in the error state quantity, represents the rotation angle, Represents generalized multiplication.

[0047] Specifically, the actual measurement value is obtained by solving the point cloud data observed by the lidar based on the iterative closest point algorithm (ICP). The key to the ICP algorithm is to solve the precise pose by continuously iterating to find corresponding points in the source point cloud and the target point cloud and minimizing the residual between them.

[0048] Assume that the current number of iterations of the iterative Kalman filter is k, and transform the point cloud dedistorted by backpropagation into the world coordinate system: ; Among them, the superscript Represents the world coordinate system; It indicates the expected coordinate value of the feature point after the pose to be solved is transformed into the world coordinate system; Represents the transformation from the radar laser coordinate system to the IMU coordinate system; Indicates that the feature point is Coordinate values ​​in the frame radar laser coordinate system; represents the pose to be solved; Indicates feature points; Indicates frameIMU; Indicates the total number of feature points.

[0049] Specifically, Is the pose to be solved, the distance from the feature point to the matching feature (line or plane) to generate the residual. For edge feature points, the residual is the distance from the point to the line; for plane feature points, the residual is the distance from the point to the plane. Definition is the normal vector of the plane or edge, is a point on a plane or an edge, then the residual It is expressed as: ; Among them, the above residual The observation equation is obtained .

[0050] By solving the observation equation linearly, the observation equation of the extended Kalman filter is derived. Subsequently, multiple iterative optimizations are required to reduce the error caused by the linearized observation equation. It can be proved that the solution method of the iterative extended Kalman filter and the optimization method based on Newton's method are mathematically equivalent, and can solve the problem with an unbiased estimate. During the iteration process, if the residual is less than the threshold, the iteration is considered to have converged, and the optimal state quantity is estimated as: ; The state quantity Update and convert to Lie Group form , each lidar point cloud in the kth scanning frame The map is updated by projecting everything into the world coordinate system using the following formula.

[0051] ; Through iteration, the tightly coupled state estimation of LiDAR and IMU is achieved. The posture of the tunnel detection machine and the zero bias of the IMU can be updated through the observation of LiDAR, achieving more accurate state estimation and global point cloud.

[0052] Part II: In the simultaneous localization and mapping problem, degradation refers to the phenomenon that the algorithm may perform poorly in an environment with insufficient constraints due to the instability and inaccuracy of state estimation, such as insufficient observation information in a certain direction or dimension, making it difficult for the algorithm to accurately estimate the state from the observation data.

[0053] LiDAR requires spatial geometric structure information to iteratively solve the pose information. Since the long linear structure lacks structural features in its extension direction and belongs to a degenerate environment, the state estimation is difficult to estimate along the long linear extension method due to the lack of constraints. It must be degenerated and the solved pose is projected to the non-degenerate method. The components of the solution in the degenerate direction are discarded, and the components in the non-degenerate direction are retained.

[0054] There is an observation matrix in the solution process. The degradation factor D can be calculated by characteristic decomposition of the observation matrix, as shown below: ; Perform feature decomposition on the point cloud feature matrix. for If the minimum eigenvalue is less than the threshold, the direction of the eigenvector corresponding to the eigenvalue corresponds to the direction of degradation.

[0055] Define the following matrix: ; in, represents the degradation direction vector; represents the non-degenerate direction vector; Represents all direction vectors obtained by solution; Represents the total number of solution directions.

[0056] The corresponding reprojection components are: ; in, is an estimate of the true state, is the estimated value obtained after nonlinear optimization, To optimize the solution, represents the increment of the solution in the degenerate direction; represents the increment of the solution in the non-degenerate direction.

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

[0058] Keeping the solution in the non-degenerate direction, the final solution is: ; in, Represents the optimal estimated value of the state quantity after updating.

[0059] Further optionally, during the backward propagation, the point cloud data of the laser radar in the non-degenerate direction (that is, only the point cloud data of the laser radar in this direction is needed for secondary fusion to update the posture information) is secondary fused with the data after the primary fusion.

[0060] Step S103, combining the position and posture with the point cloud data observed by the laser radar to perform fusion mapping to obtain a three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

[0061] Specifically, the three-dimensional point cloud model of the tunnel to be inspected in the world coordinate system is obtained by combining the position and point cloud data observed by the lidar.

[0062] It can be understood that the posture used in step S103 is the posture of the laser radar, which can be obtained by transforming the posture of the data acquisition module output in step S102 according to the positional relationship between the laser radar and the data acquisition module.

[0063] It should be noted that in order to make the three-dimensional point cloud model more accurate, this step combines the point cloud data observed by all lidars to construct the three-dimensional point cloud model.

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

[0065] In this application, the data acquisition module chooses to use laser radar as the main sensor to realize the positioning and mapping functions of the robot. However, there are many limitations in using only laser radar for positioning and mapping, because the lack of the ability to detect the inertial state of the data acquisition module itself leads to large deviations in the point cloud measurement results when the data acquisition module moves faster or there are bumps. IMU can measure the acceleration and angular velocity of the data acquisition module, and can recurse the prior state based on the IMU data, dedistort the laser radar point cloud and use it as the initial value of the point cloud matching, which can improve the accuracy and robustness of positioning and mapping. When the tunnel point cloud is degraded, the components of the solution in the degraded direction are discarded, and the components in the non-degraded direction are retained, and the wheel speed meter can provide speed observation for the data acquisition module to update the speed state quantity in the state estimation. When the tunnel point cloud is degraded, the wheel speed meter provides a state estimate in the degraded direction, making the positioning more robust. Therefore, this application integrates IMU, laser radar and wheel speed meter data to realize the positioning and mapping functions of the tunnel environment.

[0066] It should be understood that expressions such as "including" and "may include" that may be used in the present 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 the present application, terms such as "including" and / or "having" may be interpreted as indicating specific characteristics, numbers, operations, constituent elements, components, or combinations thereof, but may not be interpreted as excluding the existence or possibility of adding one or more other characteristics, numbers, operations, constituent elements, components, or combinations thereof.

[0067] In the description of the embodiments of the present application, it should be noted that, unless otherwise clearly specified and limited, the term "connection" should be understood in a broad sense. 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. Among them, "fixed connection" means that the relative position relationship after connection remains unchanged. "Rotational connection" means that the two are connected to each other and can rotate relative to each other after connection. "Sliding connection" means that the two are connected to each other and can slide relative to each other after connection. The directional terms mentioned in the embodiments of the present application, such as "top", "bottom", "inside", "outside", "left", "right", etc., are only reference directions of the accompanying drawings. Therefore, the directional terms used are for better and clearer explanation and understanding of the embodiments of the present application, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the embodiments of the present application.

[0068] In addition, in the embodiments of the present application, the mathematical concepts mentioned are symmetry, equality, parallelism, verticality, etc. These limitations are all for the current state of the art, rather than being absolutely strict definitions in a mathematical sense, and allow a small amount of deviation, approximation to symmetry, approximation to equality, approximation to parallelism, approximation to verticality, etc. are all possible. For example, A and B are parallel, which 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, which means that A and B are perpendicular or approximately perpendicular, and the angle between A and B can be between 80 and 100 degrees.

[0069] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A mobile tunnel inspection robot, characterized in that: include: The robot body and data acquisition module include laser radar, wheel speed meter and inertial measurement unit IMU; The robot body is rigidly connected to the data acquisition module, and the coordinate transformation relationship between different sensors can be obtained through rigid transformation; The robot body is used to drive the robot to move; 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 the three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

2. The mobile tunnel inspection robot according to claim 1, characterized in that: The robot body includes: a mobile base, a power supply, a power control board and an onboard computer; The mobile base is used to drive the robot to move; The power supply provides electric energy to the power control board, and the power control board outputs the supply voltage to the onboard computer, the mobile base and the data acquisition module; The onboard computer and the data acquisition module are communicated and connected; The onboard computer is used to fuse the observation data of the lidar, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter through the error state Kalman filter ESKF, and obtain the three-dimensional point cloud model of the tunnel to be detected in the world coordinate system.

3. The mobile tunnel inspection robot according to claim 2, characterized in that: The robot body includes a motor drive board; The mobile base is provided with four running wheels and four motors, and one motor is connected to one running wheel; The onboard computer drives the motor via the motor driver board.

4. The mobile tunnel inspection robot according to any one of claims 1 to 3, characterized in that: 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 in that: The mobile tunnel inspection robot is applied to any one of claims 1 to 4, comprising: 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 determine the position and posture of the data acquisition module; Based on the posture 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 the world coordinate system is obtained. The posture of the laser radar is obtained by transforming the posture of the data acquisition module based on the positional relationship between the laser radar and the data acquisition module.

6. The tunnel detection method according to claim 5, characterized in that: The error state Kalman filter ESKF 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 to determine the position and posture of the data acquisition module, including: Through the forward propagation of the error state Kalman filter ESKF, the observation data of the inertial measurement unit IMU and the observation data of the wheel speed meter are fused to obtain the first fusion data, which includes the position and posture; Through the backward propagation of the error state Kalman filter ESKF, the forward propagated fusion data and the observation data of the lidar are fused to obtain the second fusion data.

7. The tunnel detection method according to claim 6, characterized in that: The method 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 the error state Kalman filter ESKF to obtain the first fusion data includes: Based on the three-axis acceleration data and three-axis angular velocity data of the inertial measurement unit IMU, the kinematic state of the data acquisition module is recursively deduced to obtain the speed and posture of the data acquisition module; The speed information observed by the wheel speed meter is used to update the speed value of the data acquisition module.

8. The tunnel detection method according to claim 6, characterized in that: The method of fusing the forward propagated fusion data with the laser radar observation data through the backward propagation of the error state Kalman filter ESKF to obtain the second fusion data includes: Based on the pose information in the first fusion data, the point cloud data is dedistorted; The iterative closest point algorithm (ICP) is used to find the solution with the minimum residual between the current frame point cloud data observed by the lidar and the historical map; Based on the solution with the minimum residual error and the first fusion data, the pose information is updated by constructing the observation equation and calculating the Kalman gain; Among them, the historical map is a point cloud set in the world coordinate system determined based on the historical frame point cloud data observed by the lidar.

9. The tunnel detection method according to claim 8, characterized in that: After solving the solution with the minimum residual between the current frame point cloud data observed by the lidar and the historical map through the iterative closest point algorithm ICP, it also includes: 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 the 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 the non-degenerate direction is retained; Among them, the non-degenerate direction is perpendicular to the extension direction of the long linear structure of the tunnel.

10. The tunnel detection method according to any one of claims 5 to 9, characterized in that: Also includes: Publish the observation data of each sensor in the form of topics in the robot operating system ROS; By subscribing to the sensor topic, you can obtain the observation data of the lidar, the observation data of the inertial measurement unit IMU, and the observation data of the wheel speed meter.

Images