Multi-source heterogeneous information fusion positioning method for complex pipeline environment based on subgraph matching

Abstract

The present invention discloses a multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching. In order to solve the problem of point cloud degradation in similar pipeline environments, the extended Kalman filter method is used to construct the robot motion system equation and measurement equation using the preprocessed heterogeneous sensor information, iteratively update the Kalman gain and output accurate local prior pose estimation. In order to solve the problem that the cumulative error of the traditional positioning algorithm grows linearly with time, the inertial measurement unit data is used to perform planar projection on the front mechanical lidar point cloud and construct a subgraph. The subgraph matching idea is used to tightly couple the prior pose and the local constraint factors of the subgraph to estimate the robot's motion increment between adjacent data frames in the sliding window, and finally minimize the residuals of all factors to obtain the maximum a posteriori estimate of the pipeline inspection robot's state. The accuracy and effectiveness of the proposed positioning method are verified through corresponding experiments. The method can effectively reduce the positioning error of the pipeline inspection robot in a complex and narrow pipeline environment and achieve centimeter-level positioning accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of pipeline inspection robot positioning, and in particular to a multi-source heterogeneous information fusion positioning method for a complex pipeline environment based on subgraph matching. Background Art

[0002] With the continued growth of urban populations, central air conditioning duct systems have become critical equipment for ventilation and humidity control in high-rise buildings. Over time, contaminants such as fungi and bacteria accumulate within the ducts, posing a serious health threat to residents. Due to the complexity and diversity of duct structures, manual inspection of an entire building's network is impractical, leading to the emergence of pipeline inspection robots. To obtain accurate and reliable positioning information, existing pipeline inspection robots typically require heterogeneous sensor equipment such as wheel encoders, inertial measurement units (IMUs), and lidars. However, wheel encoders are subject to measurement errors caused by slippage and uneven pipe surfaces, as well as systematic errors due to inaccurate robot kinematic modeling. IMUs also suffer from cumulative errors due to zero bias and angular random walk. LiDAR's strong reflections from metal pipes can cause point cloud distortion, and point cloud degradation is common in long, straight pipes. Furthermore, due to cost and payload constraints, small pipeline inspection robots cannot be equipped with expensive high-precision positioning equipment.

[0003] In response to the above problems, there is an urgent need for a real-time positioning method that is suitable for complex metal pipeline environments and does not rely on high-precision sensors and multi-source heterogeneous information fusion positioning methods. Summary of the Invention

[0004] The purpose of the present invention is to provide a multi-source heterogeneous information fusion positioning method based on encoders, IMUs and lidars in complex pipeline environments, to solve the problems of encoder and IMU errors accumulating over time and lidar point cloud distortion in metal pipeline environments, and to achieve real-time and accurate positioning of pipeline inspection robots in complex pipeline environments by comprehensively using the extended Kalman filter (EKF) and subgraph matching ideas to fuse multi-sensor data.

[0005] The technical solution to achieve the purpose of the present invention is as follows: On the one hand, a multi-source heterogeneous information fusion positioning method for a complex pipeline environment based on subgraph matching is provided, and the method comprises the following steps:

[0006] Step 1: Establish a pipeline inspection robot system dynamics model;

[0007] Step 2: Place the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, inertial measurement units (IMUs), side solid-state laser radars, and front mechanical laser radars.

[0008] Step 3: Preprocess the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment;

[0009] Step 4: Construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance;

[0010] Step 5: Construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the extended Kalman filter (EKF) method, and iteratively update it to obtain the prior pose estimate of the pipeline inspection robot;

[0011] Step 6: Use IMU data to perform planar projection on the front mechanical lidar point cloud and construct a subgraph;

[0012] In step 7, the subgraph matching idea is used to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment between adjacent data frames in the sliding window of the pipeline inspection robot, and the residuals of all factors are minimized to solve the maximum a posteriori estimate of the pipeline inspection robot state.

[0013] Furthermore, the system dynamics model of the pipeline inspection robot is established in step 1, specifically:

[0014]

[0015] Where, v x , v y is the linear velocity of the pipeline inspection robot moving in the x and y directions in the body coordinate system, ω is the angular velocity of the pipeline inspection robot body coordinate system relative to the global coordinate system, l is the robot wheelbase, Δψ is the change in the yaw angle of the pipeline inspection robot between adjacent moments, ψ is the yaw angle of the pipeline inspection robot, and v l , v r are the linear speeds of the left and right wheels of the pipeline inspection robot respectively.

[0016] Furthermore, the heterogeneous sensor data collected in step 2 includes: collecting the left and right wheel encoder data at time t IMU's three-axis accelerometer data a x , a y , a z , three-axis gyroscope data g x , g y , g z , three-axis magnetometer data m x , m y , m z ; Point cloud data of side solid-state laser radar Front mechanical lidar data

[0017] Furthermore, step 3 pre-processes the collected heterogeneous sensor data to obtain the position information of the pipeline inspection robot, specifically including:

[0018] Step 3-1: Calculate the linear velocity v of the left and right wheels of the pipeline inspection robot using the encoder data. l , v r , specifically:

[0019]

[0020]

[0021] Where M is the encoder count when the pipeline inspection robot wheel rotates a complete circle, They are the left and right wheel encoder data at time t, They are the encoder data of the left and right wheels at time t+1, d is the diameter of the pipeline inspection robot wheel, Δt is the time difference between the two samples, and v can be calculated based on the left and right wheel speeds. x , v y and ω;

[0022] The following represents the position change Δx of the pipeline inspection robot within Δt calculated by the encoder data odom , Δy odom and Δψ odom , specifically:

[0023]

[0024] Then for Δx odom , Δy odom and Δψ odom By accumulating, we can get the encoded odometer pose of the robot at each moment;

[0025] Step 3-2: Filter the IMU raw data: Use the six-axis IMU complementary filtering method to fuse the data of the gyroscope, accelerometer, and magnetometer to obtain the corresponding quaternion data, specifically:

[0026]

[0027] in, Represents the change of the IMU coordinate system relative to the world coordinate system, q0, q1, q2, q3 represent the quaternion used to represent the robot's rotation angle in three-dimensional space;

[0028] The quaternion calculated from the IMU data also obtains the corresponding three-axis Euler angle: roll angle θ IMU , pitch angle φ IMUand yaw angle ψ IMU ;

[0029] Step 3-3, perform least squares fitting on the point cloud data between -20° and +20° of the side solid-state laser radar to obtain the distance between the side laser radar and the pipe wall at time t

[0030] Get the coordinate offset Δy of the robot in the y-axis direction between adjacent moments lidar , specifically:

[0031]

[0032] Among them, I bool is a symbolic constant, for the right laser radar I bool +1, for the left laser radar I bool -1

[0033] Furthermore, step 4 constructs the pipeline inspection robot motion system equation based on the encoder data and initializes the pipeline inspection robot's posture state and covariance, specifically including:

[0034] Define the state variable x = [xy ψ] T , control quantity u=[v ω] T , the system equation f is constructed based on the encoder measurement value to estimate the robot pose a priori, specifically:

[0035]

[0036] in, is the prior estimate at time k, is the best posterior estimate at k-1 time, [v k ω k ] T is the control quantity at time k, v k ,ω k are the linear velocity and angular velocity of the pipeline robot at time k, w k is the process noise of the system model at time k, which satisfies the Gaussian distribution P(w):N(0,Q), where Q is the covariance matrix of the Gaussian noise.

[0037] Furthermore, in step 5, the measurement equation is constructed and updated based on the IMU and side solid-state lidar data, and the Kalman gain is calculated and iteratively updated based on the extended Kalman filter (EKF) method to obtain the prior pose of the pipeline inspection robot, specifically including:

[0038] Step 5-1, calculate the covariance matrix of the prior error of IMU and side solid-state lidar at time k respectively and Specifically:

[0039]

[0040]

[0041] in, and Represent the error covariance matrix of IMU and solid-state lidar at time k-1, A k The Jacobian matrix representing the partial derivatives of the system equation f with respect to x, W k The Jacobian matrix representing the partial derivatives of the system equation f with respect to w is:

[0042]

[0043]

[0044] Step 5-2, calculate the Kalman gains corresponding to the IMU and the side solid-state lidar at time k respectively and Specifically:

[0045]

[0046]

[0047] Where H represents the Jacobian matrix of the partial derivatives of the sensor observation function h with respect to x, V represents the Jacobian matrix of the partial derivatives of the sensor observation function h with respect to v, H′ and V′ are the transposes of H and V respectively, σ represents the measurement noise, which satisfies the Gaussian distribution, P(σ):(0,R), and R is the measurement noise covariance matrix. The above variables for H and V are expressed as:

[0048]

[0049]

[0050] in, and They are the observation functions corresponding to the IMU and solid-state lidar sensors, and the observed values ​​are and Specifically:

[0051]

[0052]

[0053] in, Represents the y-axis offset of the robot relative to the initial state obtained by the side laser radar;

[0054] Step 5-3, use the obtained Kalman gain to calculate the posterior estimate and

[0055]

[0056]

[0057] Step 5-4, update the error covariance matrix of IMU and solid-state lidar and Specifically:

[0058]

[0059]

[0060] Where I is the identity matrix;

[0061] The obtained posterior estimate and The subgraph is constructed together with the prior pose and the front mechanical lidar data.

[0062] Furthermore, in step 6, the IMU data is used to perform a planar projection on the front mechanical lidar point cloud and construct a subgraph, specifically:

[0063] Step 6-1: Use the mapping formula to perform planar projection on the front mechanical lidar point cloud:

[0064]

[0065] Among them, φ IMU is the pitch angle of the robot at time t calculated from the IMU data, is the x-coordinate of the k′th point cloud data of the robot’s front laser radar at time t, for The mapped coordinates;

[0066] Step 6-2: Use the point cloud data scanned by the front mechanical laser radar to build a probability grid map, divide the point cloud data into different pixel areas, and set a confidence level P for the existence of the point cloud in each pixel area. dp , the confidence is continuously updated based on subsequent point cloud input.

[0067] Furthermore, in step 7, the subgraph matching idea is used to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment between adjacent data frames of the pipeline inspection robot in the sliding window, and the residuals of all factors are minimized to solve the maximum a posteriori estimate of the pipeline inspection robot state. Specifically,

[0068] Step 7-1: Use the EKF output data as the prior pose to perform sub-image matching, specifically:

[0069]

[0070] in, is the transformation matrix from the front mechanical lidar coordinate system to the world coordinate system, N is the number of point clouds during one scan, and the Com function is used to compare the point cloud data with the probability grid map data according to the pixel area division. If the point cloud falls in the map boundary area, the Com function value is 1, otherwise it is 0, to obtain the corresponding local constraint;

[0071] Step 7-2, tightly couple the prior pose and the local constraints of the subgraph to estimate the robot's motion increment between adjacent data frames in the sliding window, and minimize the residuals of all factors to solve the maximum a posteriori estimate of the robot state Specifically:

[0072]

[0073] Among them, ρ is the evaluation function representing the degree of subgraph matching, Sub represents the set of constructed subgraphs, l and i represent the subgraph numbers, and I k and I k+1 Represents two adjacent EKF data frames, and Represent two types of observations, represents the estimated state of the robot, represents the local constraints of the subgraph, Represents the prior pose constraints output by the EKF.

[0074] On the other hand, a multi-source heterogeneous information fusion positioning system for a complex pipeline environment based on subgraph matching is provided, the system comprising:

[0075] The first module is used to establish the dynamic model of the pipeline inspection robot system;

[0076] The second module places the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, inertial measurement units (IMUs), side-mounted solid-state LiDARs, and front-mounted mechanical LiDARs.

[0077] The third module is used to pre-process the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment;

[0078] The fourth module is used to construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance;

[0079] The fifth module is used to construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the extended Kalman filter (EKF) method, and iteratively update it to obtain the pipeline inspection robot's prior pose estimate;

[0080] The sixth module is used to use IMU data to perform planar projection of the front mechanical lidar point cloud and construct a subgraph;

[0081] The seventh module is used to use the subgraph matching idea to tightly couple the prior pose and the local constraints of the subgraph to estimate the motion increment between adjacent data frames in the sliding window of the pipeline inspection robot, and minimize the residuals of all factors to solve the maximum a posteriori estimation of the pipeline inspection robot's state.

[0082] Compared with the prior art, the present invention has the following significant advantages:

[0083] 1) The present invention is an autonomous real-time positioning method that does not require the deployment of auxiliary positioning equipment such as RFID tags in the pipeline in advance, nor does it require the acquisition of a priori maps of the pipeline.

[0084] 2) The present invention utilizes the subgraph matching concept to avoid the defect of conventional positioning methods that are prone to falling into local optimal solutions. By using the EKF output results as the prior pose, the computational efficiency of point cloud matching is greatly improved, and the running time and resource consumption of solving the global optimal pose are significantly reduced.

[0085] 3) The present invention can achieve good results in complex and narrow pipeline environments, which is beneficial for pipeline inspection robots to eliminate self-positioning cumulative errors in complex pipeline environments and improve the positioning accuracy of pipeline robots.

[0086] The present invention is further described in detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 This is a diagram showing the hardware composition of the pipeline inspection robot used in the present invention.

[0088] Figure 2 This is an overall block diagram of the positioning method proposed in the present invention.

[0089] Figure 3 Schematic diagram of the side solid-state laser radar point cloud image used in the present invention.

[0090] Figure 4 This is a diagram showing the operating effects of the positioning method proposed in this invention in several pipeline environments, where Figure 4 (a) to (c) are the operating effect diagrams under a straight pipeline environment, a 90° bend pipeline environment, and a U-bend pipeline environment, respectively. DETAILED DESCRIPTION

[0091] In order to make the purpose, technical solutions and advantages of this application more clear, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0092] It should be noted that if the embodiments of the present invention involve directional indications (such as up, down, left, right, front, back, etc.), the directional indications are only used to explain the relative position relationship, movement status, etc. between the various components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indications will also change accordingly.

[0093] In addition, if there are descriptions involving "first", "second", etc. in the embodiments of the present invention, the descriptions of "first", "second", etc. are only for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features specified as "first" and "second" may explicitly or implicitly include at least one of such features. In addition, the technical solutions between the various embodiments can be combined with each other, but this must be based on the fact that ordinary technicians in this field can implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0094] The goal of this invention is to solve the problems of severe degradation and large cumulative errors in similar scenarios of traditional positioning methods. Therefore, the extended Kalman filter and subgraph matching methods are used to fuse heterogeneous sensor data to solve the global optimal posture of the robot.

[0095] In one embodiment, combined Figure 1 , provides a multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching, the method comprising the following steps:

[0096] Step 1: Establish a pipeline inspection robot system dynamics model;

[0097] Step 2: Place the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, inertial measurement units (IMUs), side solid-state laser radars, and front mechanical laser radars.

[0098] Step 3: Preprocess the collected heterogeneous sensor information to obtain measurement information of the pipeline inspection robot and its surrounding environment;

[0099] Step 4: Construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance;

[0100] Step 5: Construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the extended Kalman filter (EKF) method and iteratively update it to obtain the prior pose estimate of the pipeline inspection robot;

[0101] Step 6: Use the IMU data to perform planar projection on the front mechanical lidar point cloud and construct a subgraph;

[0102] In step 7, the subgraph-matching idea is used to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment of the robot between adjacent data frames in the sliding window, and the residuals of all factors are minimized to solve the maximum a posteriori estimate of the robot state.

[0103] The pipeline inspection robot described is a differential drive type and should be equipped with a wheel encoder, an inertial measurement unit, a side solid-state laser radar and a front mechanical laser radar. The overall hardware structure diagram is as follows: Figure 1 shown.

[0104] Furthermore, in one embodiment, the system dynamics model of the pipeline inspection robot is established in step 1, specifically:

[0105]

[0106] Where, v x , v y is the linear velocity of the pipeline inspection robot moving in the x and y directions in the body coordinate system, ω is the angular velocity of the pipeline inspection robot body coordinate system relative to the global coordinate system, l is the robot wheelbase, Δψ is the change in the yaw angle of the pipeline inspection robot between adjacent moments, ψ is the yaw angle of the pipeline inspection robot, and v l , v r are the linear speeds of the left and right wheels of the pipeline inspection robot respectively.

[0107] Furthermore, in one embodiment, the acquisition of heterogeneous sensor data in step 2 includes: acquiring left and right wheel encoder data at time t IMU's three-axis accelerometer data a x , a y , a z , three-axis gyroscope data g x , g y , g z , three-axis magnetometer data m x , m y , m z ; Point cloud data of side solid-state laser radar Front mechanical lidar data

[0108] Furthermore, in one embodiment, the preprocessing of the collected heterogeneous sensor data in step 3 to obtain the position and posture information of the pipeline inspection robot specifically includes:

[0109] Step 3-1: Calculate the linear velocity v of the left and right wheels of the pipeline inspection robot using the encoder data. l , v r , specifically:

[0110]

[0111]

[0112] Where M is the encoder count when the pipeline inspection robot wheel rotates a complete circle, They are the left and right wheel encoder data at time t, They are the encoder data of the left and right wheels at time t+1, d is the diameter of the pipeline inspection robot wheel, Δt is the time difference between the two samples, and v can be calculated based on the left and right wheel speeds. x , v y and ω;

[0113] Furthermore, it represents the position change Δx of the pipeline inspection robot within Δt calculated by the encoder data. odom , Δy odom and Δψ odom , specifically:

[0114]

[0115] Then for Δx odom , Δy odom and Δψ odom By accumulating, we can get the encoded odometer pose of the robot at each moment;

[0116] Step 3-2: Filter the IMU raw data: Use the six-axis IMU complementary filtering method to fuse the data of the gyroscope, accelerometer, and magnetometer to obtain the corresponding quaternion data, specifically:

[0117]

[0118] in, Represents the change of the IMU coordinate system relative to the world coordinate system, q0, q1, q2, q3 represent the quaternion used to represent the robot's rotation angle in three-dimensional space;

[0119] Furthermore, the quaternion calculated from the IMU data also obtains the corresponding three-axis Euler angle: roll angle θ IMU , pitch angle φ IMUand yaw angle ψ IMU ;

[0120] In step 3-3, the laser emitted by the side solid-state laser radar is a straight line, so in a pipeline environment with spatial restrictions, the laser point cloud formed is usually as follows: Figure 3 shown.

[0121] Perform least squares fitting on the point cloud data between -20° and +20° of the side solid-state laser radar to obtain the distance between the side laser radar and the pipe wall at time t

[0122] Furthermore, the coordinate offset Δy of the robot in the y-axis direction between adjacent moments is obtained lidar , specifically:

[0123]

[0124] Among them, I bool is a symbolic constant, for the right laser radar I bool +1, for the left laser radar I bool is -1.

[0125] Furthermore, in one embodiment, step 4 constructs the pipeline inspection robot motion system equation based on the encoder data and initializes the pipeline inspection robot's posture state and covariance, specifically including:

[0126] Define the state variable x = [xy ψ] T , control quantity u=[v ω] T , the system equation f is constructed based on the encoder measurement value to estimate the robot pose a priori, specifically:

[0127]

[0128] in, is the prior estimate at time k, is the best posterior estimate at k-1 time, [v k ω k ] T is the control quantity at time k, v k ,ω k are the linear velocity and angular velocity of the pipeline robot at time k, w k is the process noise of the system model at time k, which satisfies the Gaussian distribution P(w):N(0,Q), where Q is the covariance matrix of the Gaussian noise.

[0129] Furthermore, in one embodiment, step 5 constructs and updates the measurement equation based on the IMU and side solid-state lidar data, calculates the Kalman gain based on the extended Kalman filter (EKF) method and iteratively updates it to obtain a priori pose estimation of the pipeline inspection robot, specifically including:

[0130] Step 5-1, calculate the covariance matrix of the prior error of IMU and side solid-state lidar at time k respectively and Specifically:

[0131]

[0132]

[0133] in, and Represent the error covariance matrix of IMU and solid-state lidar at time k-1, A k The Jacobian matrix representing the partial derivatives of the system equation f with respect to x, W k The Jacobian matrix representing the partial derivatives of the system equation f with respect to w is:

[0134]

[0135]

[0136] Step 5-2, calculate the Kalman gains corresponding to the IMU and the side solid-state lidar at time k respectively and Specifically:

[0137]

[0138]

[0139] Where H represents the Jacobian matrix of the partial derivatives of the sensor observation function h with respect to x, V represents the Jacobian matrix of the partial derivatives of the sensor observation function h with respect to v, H′ and V′ are the transposes of H and V respectively, σ represents the measurement noise, which satisfies the Gaussian distribution, P(σ):(0,R), and R is the measurement noise covariance matrix. The above variables for H and V are expressed as:

[0140]

[0141]

[0142] in, and They are the observation functions corresponding to the IMU and solid-state lidar sensors, and the observed values ​​are and The corresponding predicted values ​​are

[0143]

[0144]

[0145] in, Represents the y-axis offset of the robot relative to the initial state obtained by the side laser radar; Step 5-3, use the obtained Kalman gain to calculate the posterior estimate and

[0146]

[0147]

[0148] Step 5-4, update the error covariance matrix of IMU and solid-state lidar and Specifically:

[0149]

[0150]

[0151] Where I is the identity matrix;

[0152] The obtained posterior estimate and The subgraph is constructed together with the prior pose and the front mechanical lidar data.

[0153] Furthermore, in one embodiment, the step 6 of using IMU data to perform planar projection on the front mechanical lidar point cloud and construct a subgraph is specifically as follows:

[0154] Step 6-1: Use the mapping formula to perform planar projection on the front mechanical lidar point cloud:

[0155]

[0156] Among them, φ IMU is the pitch angle of the robot at time t calculated from the IMU data, is the x-coordinate of the k′th point cloud data of the robot’s front laser radar at time t, for The mapped coordinates;

[0157] Step 6-2: Use the point cloud data scanned by the front mechanical laser radar to build a probability grid map, divide the point cloud data into different pixel areas, and set a confidence level P for the existence of the point cloud in each pixel area. dp , the confidence is continuously updated based on subsequent point cloud input.

[0158] Furthermore, in one embodiment, in combination Figure 2 In step 7, the subgraph matching idea is used to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment between adjacent data frames in the sliding window of the pipeline inspection robot, and the residuals of all factors are minimized to solve the maximum a posteriori estimate of the pipeline inspection robot state. Specifically,

[0159] Step 7-1: Use the EKF output data as the prior pose to perform sub-image matching, specifically:

[0160]

[0161] in, is the transformation matrix from the front mechanical lidar coordinate system to the world coordinate system, N is the number of point clouds during one scan, and the Com function is used to compare the point cloud data with the probability grid map data according to the pixel area division. If the point cloud falls in the map boundary area, the Com function value is 1, otherwise it is 0, to obtain the corresponding local constraint;

[0162] Step 7-2, tightly couple the prior pose and the local constraints of the subgraph to estimate the robot's motion increment between adjacent data frames in the sliding window, and minimize the residuals of all factors to solve the maximum a posteriori estimate of the robot state Specifically:

[0163]

[0164] in represents the local constraints of the subgraph, Represents the prior pose constraints output by the EKF.

[0165] Figure 4 The results of positioning experiments in several typical pipeline environments are presented. The proposed positioning system was repeated five times in straight, circular, and broken-line pipes. The positioning trajectory from each experiment is marked with a different color. The calculated results of the proposed positioning method are then compared with the robot's actual motion trajectory. The experimental results demonstrate that the proposed positioning method achieved centimeter-level positioning accuracy in these pipeline environments.

[0166] In one embodiment, a multi-source heterogeneous information fusion positioning system for a complex pipeline environment based on subgraph matching is provided, the system comprising:

[0167] The first module is used to establish the dynamic model of the pipeline inspection robot system;

[0168] The second module places the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, inertial measurement units (IMUs), side-mounted solid-state LiDARs, and front-mounted mechanical LiDARs.

[0169] The third module is used to pre-process the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment;

[0170] The fourth module is used to construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance;

[0171] The fifth module is used to construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the extended Kalman filter (EKF) method, and iteratively update it to obtain the pipeline inspection robot's prior pose estimate;

[0172] The sixth module is used to use IMU data to perform planar projection of the front mechanical lidar point cloud and construct a subgraph;

[0173] The seventh module is used to use the subgraph matching idea to tightly couple the prior pose and the local constraints of the subgraph to estimate the motion increment between adjacent data frames in the sliding window of the pipeline inspection robot, and minimize the residuals of all factors to solve the maximum a posteriori estimation of the pipeline inspection robot's state.

[0174] Regarding the specific definition of the complex pipeline environment multi-source heterogeneous information fusion positioning system based on subgraph matching, please refer to the definition of the complex pipeline environment multi-source heterogeneous information fusion positioning method based on subgraph matching above, which will not be repeated here. The various modules in the above-mentioned complex pipeline environment multi-source heterogeneous information fusion positioning system based on subgraph matching can be implemented in whole or in part through software, hardware and their combination. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0175] In one embodiment, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the following is achieved:

[0176] Step 1: Establish a pipeline inspection robot system dynamics model;

[0177] Step 2: Place the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, inertial measurement units (IMUs), side solid-state laser radars, and front mechanical laser radars.

[0178] Step 3: Preprocess the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment;

[0179] Step 4: Construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance;

[0180] Step 5: Construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the extended Kalman filter (EKF) method, and iteratively update it to obtain the prior pose estimate of the pipeline inspection robot;

[0181] Step 6: Use IMU data to perform planar projection on the front mechanical lidar point cloud and construct a subgraph;

[0182] In step 7, the subgraph matching idea is used to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment between adjacent data frames in the sliding window of the pipeline inspection robot, and the residuals of all factors are minimized to solve the maximum a posteriori estimate of the pipeline inspection robot state.

[0183] For the specific limitations of each step, please refer to the limitations of the multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching above, which will not be repeated here.

[0184] The present invention realizes the accurate, rapid and real-time positioning of the pipeline inspection robot in complex and narrow pipeline environments, effectively reduces the cumulative error and degradation problems caused by traditional positioning methods, and achieves centimeter-level positioning accuracy.

[0185] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only illustrative of the principles of the present invention. Without departing from the spirit and scope of the present invention, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the scope of protection of the present invention.

Claims

1. A multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching, characterized by: The method comprises the following steps: Step 1: Establish a pipeline inspection robot system dynamics model; Step 2: Place the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, IMUs, side solid-state laser radars, and front mechanical laser radars. Step 3: Preprocess the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment; Step 4: Construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance; Step 5: Construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the EKF and iteratively update it to obtain the prior pose estimate of the pipeline inspection robot; Step 6: Use the IMU data to perform planar projection on the front mechanical lidar point cloud and construct a subgraph; specifically: Step 6-1: Use the mapping formula to perform planar projection on the front mechanical lidar point cloud: Among them, φ IMU is the pitch angle of the robot at time t calculated from the IMU data, is the x-coordinate of the k'th point cloud data of the robot's front laser radar at time t, for The mapped coordinates; Step 6-2: Use the point cloud data scanned by the front mechanical laser radar to build a probability grid map, divide the point cloud data into different pixel areas, and set a confidence level P for the existence of the point cloud in each pixel area. dp ,The confidence is continuously updated based on subsequent point cloud input; In step 7, the subgraph matching is used to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment between adjacent data frames of the pipeline inspection robot in the sliding window, and the residuals of all factors are minimized to solve the maximum a posteriori estimate of the pipeline inspection robot state.

2. The multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching according to claim 1 is characterized in that: The system dynamics model of the pipeline inspection robot is established as described in step 1, specifically: Where, v x , v y is the linear velocity of the pipeline inspection robot moving in the x and y directions in the body coordinate system, ω is the angular velocity of the pipeline inspection robot body coordinate system relative to the global coordinate system, l is the robot wheelbase, Δψ is the change in the yaw angle of the pipeline inspection robot between adjacent moments, ψ is the yaw angle of the pipeline inspection robot, and v l , v r are the linear speeds of the left and right wheels of the pipeline inspection robot respectively.

3. The multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching according to claim 1 or 2 is characterized in that: In step 2, heterogeneous sensor data is collected, including: collecting left and right wheel encoder data at time t IMU's three-axis accelerometer data a x , a y , a z , three-axis gyroscope data g x , g y , g z , three-axis magnetometer data m x , m y , m z ; Point cloud data of side solid-state laser radar Front mechanical lidar data 4. The multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching according to claim 3 is characterized in that: Step 3 pre-processes the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment, specifically including: Step 3-1: Calculate the linear velocity v of the left and right wheels of the pipeline inspection robot using the encoder data. l , v r , specifically: Where M is the encoder count when the pipeline inspection robot wheel rotates a complete circle, They are the left and right wheel encoder data at time t, are the encoder data of the left and right wheels at time t+1, d is the diameter of the pipeline inspection robot wheel, and Δt is the time difference between the two samples; Furthermore, it represents the position change Δx of the pipeline inspection robot within Δt calculated by the encoder data. odom , Δy odom and Δψ odom , specifically: Then for Δx odom , Δy odom and Δψ odom By accumulating, we can get the encoded odometer pose of the robot at each moment; Step 3-2: Filter the IMU raw data: Use the six-axis IMU complementary filtering method to fuse the gyroscope and accelerometer data to obtain the corresponding quaternion data, specifically: <h2 style=";text-align:left;direction:ltr"> W <h2 style=";text-align:left;direction:ltr"> I <h2 style=";text-align:left;direction:ltr"> q=[q0 q1 q2 q3]<h2 style=";text-align:left;direction:ltr"> T in, W I q represents the change of the IMU coordinate system relative to the world coordinate system, q0, q1, q2, q3 represent the quaternion used to represent the robot's rotation angle in three-dimensional space; Furthermore, the quaternion calculated from the IMU data also obtains the corresponding three-axis Euler angle: roll angle θ IMU , pitch angle φ IMU and yaw angle ψ IMU ; Step 3-3, perform least squares fitting on the point cloud data between -20° and +20° of the side solid-state laser radar to obtain the distance between the side laser radar and the pipe wall at time t Furthermore, the coordinate offset Δy of the robot in the y-axis direction between adjacent moments is obtained lidar , specifically: Among them, I bool is a symbolic constant, for the right laser radar I bool +1, for the left laser radar I bool is -1.

5. The multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching according to claim 4 is characterized in that: Step 4 constructs the pipeline inspection robot motion system equation based on the encoder data and initializes the pipeline inspection robot's posture state and covariance, specifically including: Define the state variable x = [xy ψ] T , control quantity u=[v ω] T , the system equation f is constructed based on the encoder measurement value to estimate the robot pose a priori, specifically: in, is the prior estimate at time k, is the best posterior estimate at k-1 time, [v k-1 ω k-1 ] T is the control quantity at time k-1, v k-1 ,ω k-1 are the linear velocity and angular velocity of the pipeline robot at time k-1, w k-1 is the process noise of the system model at time k-1, which satisfies the Gaussian distribution P(w)~N(0,Q), where Q is the covariance matrix of the Gaussian noise.

6. The multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching according to claim 5 is characterized in that: In step 5, the measurement equation is constructed and updated based on the IMU and side solid-state lidar data. The Kalman gain is calculated and iteratively updated based on the extended Kalman filter (EKF) method to obtain the prior pose estimate of the pipeline inspection robot. Specifically, the following steps are performed: Step 5-1, calculate the covariance matrix of the prior error of IMU and side solid-state lidar at time k respectively and Specifically: in, and Represent the error covariance matrix of IMU and solid-state lidar at time k-1, A k The Jacobian matrix representing the partial derivatives of the system equation f with respect to x, W k The Jacobian matrix representing the partial derivatives of the system equation f with respect to w is: Step 5-2, calculate the Kalman gains corresponding to the IMU and the side solid-state lidar at time k respectively and Specifically: Where H represents the Jacobian matrix of the partial derivatives of the sensor observation function h with respect to x, V represents the Jacobian matrix of the partial derivatives of the sensor observation function h with respect to v, H′ and V′ are the transposes of H and V, respectively, σ represents the measurement noise, which satisfies the Gaussian distribution, P(σ) ~ (0, R), and R is the measurement noise covariance matrix. The above variables for H and V are expressed as: in, and They are the observation functions corresponding to the IMU and solid-state lidar sensors, and the observed values ​​are and The corresponding predicted values ​​are in, Represents the y-axis offset of the robot relative to the initial state obtained by the side laser radar; Step 5-3, use the obtained Kalman gain to calculate the posterior estimate and Step 5-4, update the error covariance matrix of IMU and solid-state lidar and Specifically: Where I is the identity matrix; The obtained posterior estimate and The subgraph is constructed together with the prior pose and the front mechanical lidar data.

7. The multi-source heterogeneous information fusion positioning method for complex pipeline environments based on subgraph matching according to claim 6 is characterized in that: Step 7 uses subgraph matching to tightly couple the prior pose with the local constraints of the subgraph to estimate the motion increment between adjacent data frames of the pipeline inspection robot in the sliding window, and minimizes all factor residuals to solve the maximum a posteriori estimate of the pipeline inspection robot state. Specifically, Step 7-1: Use the EKF output data as the prior pose to perform sub-image matching, specifically: in, is the transformation matrix from the front mechanical lidar coordinate system to the world coordinate system, N is the number of point clouds during one scan, and the Com function is used to compare the point cloud data with the probability grid map data according to the pixel area division. If the point cloud falls in the map boundary area, the Com function value is 1, otherwise it is 0, to obtain the corresponding local constraint; Step 7-2, tightly couple the prior pose and the local constraints of the subgraph to estimate the robot's motion increment between adjacent data frames in the sliding window, and minimize the residuals of all factors to solve the maximum a posteriori estimate of the robot state Specifically: Among them, ρ is the evaluation function representing the degree of subgraph matching, Sub represents the set of constructed subgraphs, l and i represent the subgraph numbers, and I k and I k+1 Represents two adjacent EKF data frames, and Represent two types of observations, represents the estimated state of the robot, represents the local constraints of the subgraph, Represents the prior pose constraints output by the EKF.

8. A multi-source heterogeneous information fusion positioning system for complex pipeline environments based on subgraph matching according to the method according to any one of claims 1 to 7, characterized in that: The system comprises: The first module is used to establish the dynamic model of the pipeline inspection robot system; The second module places the pipeline inspection robot in a complex pipeline environment to collect heterogeneous sensor data. The sensors include encoders, inertial measurement units (IMUs), side-mounted solid-state LiDARs, and front-mounted mechanical LiDARs. The third module is used to pre-process the collected heterogeneous sensor data to obtain measurement information of the pipeline inspection robot and its surrounding environment; The fourth module is used to construct the pipeline inspection robot motion system equation based on the encoder data and initialize the pipeline inspection robot's posture state and covariance; The fifth module is used to construct and update the measurement equation based on the IMU and side solid-state lidar data, calculate the Kalman gain based on the extended Kalman filter (EKF) method, and iteratively update it to obtain the pipeline inspection robot's prior pose estimate; The sixth module is used to use IMU data to perform planar projection of the front mechanical lidar point cloud and construct a subgraph; The seventh module is used to use the subgraph matching idea to tightly couple the prior pose and the local constraints of the subgraph to estimate the motion increment between adjacent data frames in the sliding window of the pipeline inspection robot, and minimize the residuals of all factors to solve the maximum a posteriori estimation of the pipeline inspection robot's state.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the method according to any one of claims 1 to 7 is implemented.

Images