Dead reckoning method and system for pipeline wall-climbing robot based on multi-sensor fusion

Abstract

The present invention discloses a pipeline wall climbing robot dead reckoning method and system based on multi-sensor fusion, wherein the system is mainly composed of an inertial measurement unit, an optical navigation sensor that fits the bottom of the robot to the wall, and a host machine. The method fuses the data collected by the inertial measurement unit and the optical navigation sensor in the EKF algorithm framework, then introduces BPNN to compensate for the error caused by static drift and dynamic response difference of the inertial measurement unit, and then estimates the measurement noise information through the covariance matching technology based on ANFIS, thereby obtaining the accurate positioning position and motion state of the pipeline wall climbing robot. The present invention effectively improves the reliability, robustness and positioning accuracy of dead reckoning.

Description

Technical Field

[0001] The present invention relates to robot technology, and in particular to a pipeline wall climbing robot trajectory calculation method and system based on multi-sensor fusion. Background Art

[0002] Human-computer interaction technology is gaining more and more attention. Spatial positioning and motion trajectory tracking systems are widely used in the field of virtual reality human-computer interaction as motion detection and input devices. Using data obtained by sensors to perceive the environment is the key technology for device trajectory tracking.

[0003] With the research and development of mobile robots and various sensors, three-dimensional dead reckoning systems based on multi-sensor data fusion technology are increasingly being developed and applied to pipeline mobile robots to achieve more convenient, efficient and accurate dead reckoning inside the pipeline. However, since most industrial pipelines adopt a closed structure and have a complex internal environment, the movement of mobile robots inside the pipeline is extremely easily affected, thus deviating from the theoretical detection track, resulting in inaccurate positioning. It can be seen that how to accurately complete the three-dimensional dead reckoning of pipeline mobile robots has become an urgent problem that people need to solve. Summary of the invention

[0004] The purpose of the present invention is to overcome the deficiencies of the above prior art and provide a pipeline wall climbing robot dead reckoning method based on multi-sensor fusion. The pipeline wall climbing robot dead reckoning method based on multi-sensor fusion can accurately calculate the motion state and trajectory of the pipeline wall climbing robot.

[0005] At the same time, another object of the present invention is to provide a pipeline wall-climbing robot trajectory calculation system based on multi-sensor fusion.

[0006] The purpose of the present invention is achieved through the following technical solution: The pipeline wall climbing robot trajectory calculation method based on multi-sensor fusion comprises the following steps:

[0007] S1. An inertial measurement unit for detecting acceleration at each movement moment and an optical navigation sensor for detecting relative displacement between two movement moments are arranged on the pipeline wall-climbing robot, and the data of the inertial measurement unit and the optical navigation sensor are collected at the same frequency;

[0008] S2, fuse the data collected by the inertial measurement unit and the optical navigation sensor in the EKF algorithm framework;

[0009] S3, introduce BPNN model to compensate the error caused by static drift and dynamic response difference of inertial measurement unit;

[0010] S4, then estimate the measurement noise information through the covariance matching technology based on ANFIS, and reduce the noise interference caused by the uncertainty of the measurement process to the motion state estimation;

[0011] S5. Take the robot starting point as the coordinate origin, set up a three-dimensional absolute coordinate system, and take the counterclockwise tangent direction of the origin on the circumference of the pipeline cross section as the positive direction of the X-axis, the robot's movement direction along the pipeline as the positive direction of the Y-axis, and the vertical upward direction as the positive direction of the Z-axis; convert the estimated motion state of the pipeline climbing robot in the two-dimensional virtual plane into the optimal motion state in the three-dimensional absolute coordinate system through geometric relationships and mathematical operations, and obtain the robot's real position p(k), velocity v(k), and acceleration a(k) in the pipeline absolute coordinate system.

[0012] In step S2, the specific process of data fusion is as follows:

[0013] S2-1, unfold the inner wall of the pipeline into a two-dimensional virtual plane, set the two-dimensional virtual coordinate system with the starting point of the pipeline wall-climbing robot as the coordinate origin, and take the robot's movement direction along the pipeline as the positive direction of the Y axis and the counterclockwise tangent direction along the circumference of the pipeline as the positive direction of the X axis;

[0014] S2-2, let the acceleration vector collected by the inertial measurement unit at time K be a IMU (k), the relative displacement vector collected by the optical navigation sensor is d opt (k), and based on the acceleration vector and the relative displacement vector, a mathematical model is established for the motion state of the pipeline climbing robot in the pipeline:

[0015] x(k)=Ax(k-1)+w(k-1),

[0016] z(k)=Hx(k)+v(k),

[0017] Wherein, w(k-1) is process noise, v(k) is measurement noise, A is the transition matrix from the previous state to the current state, H is the transition matrix from the current state to the measurement vector, x(k) is the state vector, and z(k) is the measurement vector;

[0018] S2-3, Order is the prior estimate of the state vector x(k) at time k, P(k|k-1) is the prior estimate error covariance matrix at time k, and we get:

[0019]

[0020] P(k|k-1)=AP(k-1)A T +Q(k-1),

[0021] Where T is the time period for the inertial measurement unit and the optical navigation sensor to send data signals, Q(k-1) is the covariance matrix of the process noise w(k-1);

[0022] S2-4. Posterior estimation using the measurement vector z(k) at time K and the measurement noise covariance matrix R(k) The estimated value of the forecast stage and P(k|k-1) are updated as the posterior estimates of the state vector x(k) And the posterior estimation error covariance matrix P(k) is used for data fusion to obtain:

[0023]

[0024]

[0025] P(k)=[IK(k)H]P(k|k-1),

[0026] Where K(k) is the Kalman gain.

[0027] The establishment process of the BPNN model in step S3 is as follows:

[0028] S3-1. Estimate the optimal state at time k-1 The acceleration vector Extract it and select the relative displacement vector d measured at time k opt (k) and and d opt (k) as input features of the BPNN model;

[0029] S3-2. Based on the input features, the tangent transfer function is selected as the transfer function between the input layer and the first hidden layer, and between the first hidden layer and the second hidden layer, and the linear transfer function is selected as the transfer function between the second hidden layer and the output layer. A double hidden layer BPNN model is established with 5 nodes in the first hidden layer and 2 nodes in the second hidden layer.

[0030] In step S3-2, the process of using the BPNN model to compensate for the error is as follows:

[0031] S3-2-1. Signals transmitted from the input layer to the first hidden layer The output signal y of the first hidden layer is j =tansig(c j );

[0032] The signal from the first hidden layer to the second hidden layer

[0033] Among them, x q is the input feature, wqj represents the weight from the input layer to the first hidden layer, b j represents the offset of the first hidden layer;

[0034] S3-2-2, the second hidden layer output signal z i =tansig(e i ), the signal transmitted from the second hidden layer to the output layer

[0035] Among them, w i represents the weight from the second hidden layer to the output layer, and b represents the offset of the output layer;

[0036] S3-2-3, the output signal h of the output layer is transferred through the linear transfer function, and finally the compensated acceleration prior estimate is obtained or where a' and b' are the coefficients of the transfer function.

[0037] The specific process of reducing noise interference in step S4 is as follows:

[0038] S4-1. Measurement residual elements at time k And use this to construct a set of innovative sequences measuring residuals To determine the actual covariance Where M = 2, i 0 =i-M+1;

[0039] S4-2. Measurement residual theoretical covariance matrix in is the prior estimate of the measurement noise covariance matrix R(k) at time k; based on the actual covariance and the theoretical covariance of the measurement residual, the matching feature DoM(k) is extracted:

[0040]

[0041] S4-2. The posterior estimate of the state variable at time k-1 The k-time measurement vector z(k), the k-time prior estimation error covariance matrix P(k|k-1), and the k-time measurement noise covariance matrix are the prior estimates. After data processing and feature extraction, the four variables are used as input signals of ANFIS;

[0042] S4-3, according to the main diagonal elements of the extracted matching feature DoM(k), the main diagonal of the measurement noise covariance matrix is ​​adapted to obtain the adjusted measurement noise covariance matrix

[0043] Among them, ΔR(k) is the adjustment matrix.

[0044] The specific steps of step S5 are as follows:

[0045] Assume that the central angle of the pipeline climbing robot moving from the starting point along the circumference of the cross section to the position at time k is The mathematical expression of the geometric relationship between the two-dimensional virtual coordinate system and the three-dimensional absolute coordinate system is:

[0046]

[0047] where p 2Dx (k) represents the two-dimensional virtual plane state variable output by the EKF at time k after a complete iteration of BPNN compensation and ANFIS covariance matching. The x-axis position coordinate; d represents the actual diameter of the inner wall of the pipe;

[0048] Then the position coordinate point p of the two-dimensional virtual plane state 2D (k)=(p 2Dx (k),p 2Dy (k),p 2Dz (k)) and the position coordinate point p(k) = (p x (k),p y (k),p z The conversion process of (k)) is as follows:

[0049]

[0050] p y (k) = p 2Dy (k)

[0051]

[0052] The velocity state point v of the two-dimensional virtual plane 2D (k)=(v 2Dx (k),v 2Dy (k),v 2Dz (k)) and the velocity state point v(k) = (v x (k),v y (k),v z The conversion process of (k)) is as follows:

[0053]

[0054] v y (k) = v 2Dy (k)

[0055]

[0056] Acceleration state point a of the two-dimensional virtual plane 2D (k)=(a 2Dx (k),a 2Dy (k),a 2Dz (k)) and the velocity state point a(k)=(a x (k),a y (k),a z The conversion process of (k)) is as follows:

[0057]

[0058] a y (k) = a 2Dy (k)

[0059]

[0060] A dead reckoning system for realizing the above-mentioned dead reckoning method of a pipeline wall-climbing robot based on multi-sensor fusion comprises:

[0061] Inertial measurement unit, used to collect the acceleration of the pipeline climbing robot at every moment of movement;

[0062] Optical navigation sensor, used to collect the relative displacement between two movement moments of the pipeline climbing robot;

[0063] The host computer is used to process data to calculate the motion trajectory and positioning points of the pipeline climbing robot.

[0064] The optical navigation sensor is arranged at the bottom of the pipeline wall-climbing robot.

[0065] Compared with the prior art, the present invention has the following advantages: the pipeline wall-climbing robot trajectory estimation method based on multi-sensor fusion fuses the data collected by the inertial measurement unit and the optical navigation sensor in the EKF algorithm framework, and then introduces BPNN to compensate for the error caused by the static drift and dynamic response difference of the inertial measurement unit, and then estimates the measurement noise information through the covariance matching technology based on ANFIS, so as to obtain the accurate positioning position and motion state of the pipeline wall-climbing robot, which effectively improves the reliability, robustness and positioning accuracy of the trajectory estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 It is a structural schematic diagram of the pipeline wall-climbing robot trajectory calculation system based on multi-sensor fusion of the present invention.

[0067] Figure 2 It is a structural schematic diagram of the pipeline wall-climbing robot trajectory calculation method based on multi-sensor fusion of the present invention.

[0068] Figure 3 A schematic diagram of the calculation results of an embodiment of the present invention.

[0069] Among them, 1 is a pipeline climbing robot, 2 is an inertial measurement unit, 3 is an optical navigation sensor, and 4 is a host computer. DETAILED DESCRIPTION

[0070] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0071] like Figure 1 A dead reckoning system of a pipeline wall climbing robot based on multi-sensor fusion dead reckoning method is shown, comprising:

[0072] Inertial measurement unit, used to collect the acceleration of the pipeline climbing robot at every moment of movement;

[0073] Optical navigation sensor, used to collect the relative displacement between two movement moments of the pipeline climbing robot;

[0074] The host computer is used to process data to calculate the motion trajectory and positioning points of the pipeline climbing robot.

[0075] The optical navigation sensor is arranged at the bottom of the pipeline wall-climbing robot.

[0076] This application adopts a three-dimensional dead reckoning system based on the organic integration of an inertial measurement unit and an optical navigation sensor, and utilizes the characteristics of the inertial measurement unit that can obtain the high-frequency angular velocity and acceleration of the movement in real time, and the optical navigation sensor that can take pictures continuously to obtain displacement data in real time, and fuses the data collected by the two to obtain the position coordinates of the robot. This technology not only combines the advantages of the two sensors, but also avoids the cumulative error of the inertial measurement unit, supplements the image loss of the optical navigation sensor, and realizes the complementary shortcomings of the two sensors, and has the advantages of rich information acquisition and strong anti-interference ability. In this embodiment, the pipeline wall-climbing robot is a four-legged wall-climbing robot for GIS pipelines, the inertial measurement unit adopts the nine-axis attitude angle sensor of Witt Intelligent Model JY901, the optical navigation sensor adopts the ADNS-9800 laser game sensor, and the three-dimensional dead reckoning module of the host computer is used to fuse the data collected by the inertial measurement unit and the optical navigation sensor to deduce the robot's motion trajectory and positioning point.

[0077] like Figure 2 As shown, the pipeline wall climbing robot trajectory estimation method based on multi-sensor fusion includes the following steps:

[0078] S1. An inertial measurement unit for detecting acceleration at each movement moment and an optical navigation sensor for detecting relative displacement between two movement moments are arranged on the pipeline wall-climbing robot, and the data of the inertial measurement unit and the optical navigation sensor are collected at the same frequency; the inertial measurement unit and the optical navigation sensor acquire data at the same frequency to determine that the data collected by both are the movement state of the pipeline wall-climbing robot at the same moment.

[0079] S2. Fusing the data collected by the inertial measurement unit and the optical navigation sensor in the EKF (Kalman filter) algorithm framework;

[0080] The specific process of data fusion is as follows:

[0081] S2-1, unfold the inner wall of the pipeline into a two-dimensional virtual plane, set the two-dimensional virtual coordinate system with the starting point of the pipeline wall-climbing robot as the coordinate origin, and take the robot's movement direction along the pipeline as the positive direction of the Y axis and the counterclockwise tangent direction along the circumference of the pipeline as the positive direction of the X axis;

[0082] S2-2, let the acceleration vector collected by the inertial measurement unit at time K be a IMU (k), the relative displacement vector collected by the optical navigation sensor is d opt (k), the acceleration vector is a IMU (k) and d opt (k) constitutes the measured variable:

[0083] z(k)=[a IMU T (k),d opt T (k)] T ;

[0084] And let the state vector be x(k)=]p 2D T (k),v 2D T (k),a 2D T (k),b 2D T (k)] T .

[0085] The motion state of the pipeline climbing robot at time k includes the position p 2D (k), speed v 2D (k), acceleration a 2D (k) and acceleration deviation b 2D (k) are all state vectors in this two-dimensional virtual coordinate system. Since the robot body is in up-and-down motion during its motion, the acceleration variable a measured by the inertial measurement unit is IMU(k) contains accelerations in the three directions of x, y, and z. After being estimated by the proposed method, the motion state on the two-dimensional virtual plane at each moment also contains the state variables in the z-axis direction. Therefore, a mathematical model of the motion state of the pipeline climbing robot in the pipeline is established:

[0086] x(k)=Ax(k-1)+w(k-1),

[0087] z(k)=Hx(k)+v(k),

[0088] Wherein, w(k-1) is the process noise, v(k) is the measurement noise, and the process noise w(k-1) and the measurement noise v(k) are assumed to be independent zero-mean Gaussian noise sequences, A is the transition matrix from the previous state to the current state, H is the transition matrix from the current state to the measurement vector, x(k) is the state vector, and z(k) is the measurement vector; the expressions of A and H are as follows:

[0089]

[0090]

[0091] Where T is the time period for the sensor to send data signals to the host computer. 3×3 is a three-row and three-column identity matrix, O 3×3 is a zero matrix with three rows and three columns;

[0092] S2-3, Order is the prior estimate of the state vector x(k) at time k, P(k|k-1) is the prior estimate error covariance matrix at time k, and the prediction stage of EKF is based on the posterior estimate of the state vector at time k-1. And the posterior estimation error covariance matrix P(k-1) is used to estimate the prior estimate at time k and P(k|k-1). We get:

[0093]

[0094] P(k|k-1)=AP(k-1)A T +Q(k-1),

[0095] Where T is the time period for the inertial measurement unit and the optical navigation sensor to send data signals, Q(k-1) is the covariance matrix of the process noise w(k-1);

[0096] S2-4. Posterior estimation using the measurement vector z(k) at time K and the measurement noise covariance matrix R(k) The estimated value of the forecast stage and P(k|k-1) are updated as the posterior estimates of the state vector x(k) and the posterior estimation error covariance matrix P(k), where the state vector posterior estimation It is the optimal state estimate of each iteration and the optimal state point used to calculate the track. The Kalman gain K(k) represents the update weight between the measurement value of the new iteration and the prediction of the system dynamic model, so as to perform data fusion:

[0097]

[0098]

[0099] P(k)=[IK(k)H]P(k|k-1),

[0100] S3, introduce BPNN (neural network) model to compensate the error caused by static drift and dynamic response difference of inertial measurement unit;

[0101] The process of establishing the BPNN model is as follows:

[0102] S3-1. Estimate the optimal state at time k-1 The acceleration vector Extract it and select the relative displacement vector d measured at time k opt (k) and and d opt (k) as input features of the BPNN model;

[0103] S3-2. Based on the input features, the tangent transfer function is selected as the transfer function between the input layer and the first hidden layer, and between the first hidden layer and the second hidden layer, and the linear transfer function is selected as the transfer function between the second hidden layer and the output layer, and a double hidden layer BPNN model is established with 5 nodes in the first hidden layer and 2 nodes in the second hidden layer. Specifically, the establishment of BPNN is divided into three layers: input layer, hidden layer and output layer, where the number of nodes in the input layer is 2 and the number of nodes in the output layer is 1, both of which are fixed. The hidden layer includes the first hidden layer and the second hidden layer.

[0104] The process of compensating errors using the BPNN model is as follows:

[0105] S3-2-1. Signals transmitted from the input layer to the first hidden layer The output signal y of the first hidden layer is j =tansig(c j );

[0106] The signal from the first hidden layer to the second hidden layer

[0107] Among them, q = 1, 2 represents the input node, j = 1, 2, 3, 4, 5 represents the first hidden layer node, x q is the input feature, w qj represents the weight from the input layer to the first hidden layer, b j Represents the offset of the first hidden layer; when the pipeline climbing robot moves left and right is the optimal state estimate at time k-1 The acceleration in the x direction, x 2 =d optx (k) is the relative displacement in the x direction at time k. is the optimal state estimate at time k-1 Acceleration in the y direction, x 2 =d opty (k) is the relative displacement in the y direction at time k.

[0108] S3-2-2, the second hidden layer output signal z i =tan sig(e i ), the signal transmitted from the second hidden layer to the output layer

[0109] Among them, w i represents the weight from the second hidden layer to the output layer, and b represents the offset of the output layer;

[0110] S3-2-3, the output signal h of the output layer is transferred through the linear transfer function, and finally the compensated acceleration prior estimate is obtained or where a' and b' are the coefficients of the transfer function.

[0111] In this step, BPNN is used to adjust the prediction phase of EKF. That is, when the k-th iteration occurs, the optimal state at the k-1th moment is estimated The acceleration vector is replaced by the vector compensated by BPNN In the update phase, the optimal state estimate is obtained by coordinating with other prior estimated vectors to compensate for the errors caused by static drift and dynamic response differences.

[0112] S4, then estimate the measurement noise information through the covariance matching technology based on ANFIS (neuro-fuzzy inference system), and reduce the noise interference caused by the uncertainty of the measurement process to the motion state estimation;

[0113] The specific process of reducing noise interference is as follows:

[0114] S4-1. Measurement residual elements at time k And use this to construct a set of innovative sequences measuring residuals To determine the actual covariance Where M = 2, i 0 =i-M+1; Specifically, the data stream of the residual innovation sequence is measured The actual covariance is approximated by using a moving estimation window of size M on the According to experience, the size of the estimated window M is 2, i 0 =i-M+1 is the first sample in the estimation window M.

[0115] S4-2. Measurement residual theoretical covariance matrix in is the prior estimate of the measurement noise covariance matrix R(k) at time k; based on the actual covariance and the theoretical covariance of the measurement residual, the matching feature DoM(k) is extracted:

[0116]

[0117] S4-2. The posterior estimate of the state variable at time k-1 The k-time measurement vector z(k), the k-time prior estimation error covariance matrix P(k|k-1), and the k-time measurement noise covariance matrix are the prior estimates. After data processing and feature extraction, the four variables are used as input signals of ANFIS;

[0118] S4-3, according to the main diagonal elements of the extracted matching feature DoM(k), the main diagonal of the measurement noise covariance matrix is ​​adapted to obtain the adjusted measurement noise covariance matrix

[0119] Among them, ΔR(k) is the adjustment matrix.

[0120] Specifically, ANFIS has a fuzzification layer, a weighting layer, a normalization layer, a defuzzification layer and a summation layer. Among them:

[0121] The fuzzification layer is the adaptive input node after feature extraction, and the output is O 1 For the fuzzy membership of the input matching degree, the Gaussian function is selected as the fuzzy membership function, that is, where μ j (x) represents the membership function (MF); a j Indicates the width of the membership function, b j represents the center of the membership function, and the two are called antecedent parameters.

[0122] The weighted layer is originally a simple multiplier. Since the system has a single input, the output is O 2 It can be expressed as, 2 =ωj =μ j (x), j=1,2,3.

[0123] The normalization layer normalizes the emission intensity of the previous layer.

[0124] The defuzzification layer is also an adaptive node. By multiplying the output of the previous layer with the corresponding fuzzy rule, the output can be obtained.

[0125] The original output of the sum layer should be the sum of all outputs from the fourth layer (defuzzification layer), but since this method is a single input, the final output is Where ΔR(k)(i,i) represents the main diagonal elements of the adjustment matrix ΔR(k), which is the adjustment factor of the main diagonal elements of the measurement noise covariance matrix R(k).

[0126] Finally, according to the main diagonal elements of the extracted matching feature DoM(k), the main diagonal of the measurement noise covariance matrix is ​​adapted to obtain the adjusted measurement noise covariance matrix

[0127] S5. Take the robot starting point as the coordinate origin, set up a three-dimensional absolute coordinate system, and take the counterclockwise tangent direction of the origin on the circumference of the pipeline cross section as the positive direction of the X-axis, the robot's movement direction along the pipeline as the positive direction of the Y-axis, and the vertical upward direction as the positive direction of the Z-axis; convert the estimated motion state of the pipeline climbing robot in the two-dimensional virtual plane into the optimal motion state in the three-dimensional absolute coordinate system through geometric relationships and mathematical operations, and obtain the robot's real position p(k), velocity v(k), and acceleration a(k) in the pipeline absolute coordinate system.

[0128] The specific steps are as follows:

[0129] Assume that the central angle of the pipeline climbing robot moving from the starting point along the circumference of the cross section to the position at time k is The mathematical expression of the geometric relationship between the two-dimensional virtual coordinate system and the three-dimensional absolute coordinate system is:

[0130]

[0131] where p 2Dx (k) represents the two-dimensional virtual plane state variable output by the EKF at time k after a complete iteration of BPNN compensation and ANFIS covariance matching. The x-axis position coordinate; d represents the actual diameter of the inner wall of the pipe;

[0132] Then the position coordinate point p of the two-dimensional virtual plane state 2D (k)=(p2Dx (k),p 2Dy (k),p 2Dz (k)) and the position coordinate point p(k) = (p x (k),p y (k),p z The conversion process of (k)) is as follows:

[0133]

[0134] p y (k) = p 2Dy (k)

[0135]

[0136] The velocity state point v of the two-dimensional virtual plane 2D (k)=(v 2Dx (k),v 2Dy (k),v 2Dz (k)) and the velocity state point v(k) = (v x (k),v y (k),v z The conversion process of (k)) is as follows:

[0137]

[0138] v y (k) = v 2Dy (k)

[0139]

[0140] Acceleration state point a of the two-dimensional virtual plane 2D (k)=(a 2Dx (k),a 2Dy (k),a 2Dz (k)) and the velocity state point a(k)=(a x (k),a y (k),a z The conversion process of (k)) is as follows:

[0141]

[0142] a y (k) = a 2Dy (k)

[0143]

[0144] Specifically, Figure 2As shown, after the pipeline climbing robot is started, steps S3 and S4 are repeated. After the pipeline climbing robot stops, step S5 is performed to calculate the three-dimensional track of the pipeline climbing robot and the motion state of each moment on the track. Figure 3 shown.

[0145] The above specific implementation modes are preferred embodiments of the present invention and cannot be used to limit the present invention. Any other changes or other equivalent replacement methods that do not deviate from the technical solution of the present invention are included in the protection scope of the present invention.

Claims

1. Dead reckoning method of pipeline climbing robot based on multi-sensor fusion, Features: The following steps are involved: S1. An inertial measurement unit for detecting acceleration at each movement moment and an optical navigation sensor for detecting relative displacement between two movement moments are arranged on the pipeline wall-climbing robot, and the data of the inertial measurement unit and the optical navigation sensor are collected at the same frequency; S2. In the EKF algorithm framework, the data collected by the inertial measurement unit and the optical navigation sensor are fused. In step S2, the specific process of data fusion is as follows: S2-1, unfold the inner wall of the pipeline into a two-dimensional virtual plane, set the two-dimensional virtual coordinate system with the starting point of the pipeline wall-climbing robot as the coordinate origin, and take the robot's movement direction along the pipeline as the positive direction of the Y axis and the counterclockwise tangent direction along the circumference of the pipeline as the positive direction of the X axis; S2-2, let the acceleration vector collected by the inertial measurement unit at time K be a IMU (k), the relative displacement vector collected by the optical navigation sensor is d opt (k), and based on the acceleration vector and the relative displacement vector, a mathematical model is established for the motion state of the pipeline climbing robot in the pipeline: x(k)=Ax(k-1)+w(k-1), z(k)=Hx(k)+v(k), Wherein, w(k-1) is process noise, v(k) is measurement noise, A is the transition matrix from the previous state to the current state, H is the transition matrix from the current state to the measurement vector, x(k) is the state vector, and z(k) is the measurement vector; S2-3, Order is the prior estimate of the state vector x(k) at time k, P(k|k-1) is the prior estimate error covariance matrix at time k, and we get: P(k|k - 1)=AP(k - 1)A T +Q(k - 1), Where T is the time period for the inertial measurement unit and the optical navigation sensor to send data signals, Q(k-1) is the covariance matrix of the process noise w(k-1); S2-4. Posterior estimation using the measurement vector z(k) at time K and the measurement noise covariance matrix R(k) The estimated value of the forecast stage and P(k|k-1) are updated as the posterior estimates of the state vector x(k) And the posterior estimation error covariance matrix P(k) is used for data fusion to obtain: P(k)=[IK(k)H]P(k|k-1) Where, K(k) is the Kalman gain; S3, introduce BPNN model to compensate the error caused by static drift and dynamic response difference of inertial measurement unit; S4, then estimate the measurement noise information through the covariance matching technology based on ANFIS, and reduce the noise interference caused by the uncertainty of the measurement process to the motion state estimation; the specific process of reducing the noise interference in step S4 is as follows: S4-1. Measurement residual elements at time k And construct a set of innovation sequences of measurement residuals based on this To determine the actual covariance S4-2. Measurement residual theoretical covariance matrix in is the prior estimate of the measurement noise covariance matrix R(k) at time k; based on the actual covariance and the theoretical covariance of the measurement residual, the matching feature DoM(k) is extracted: S4-2. The posterior estimate of the state variable at time k-1 The k-time measurement vector z(k), the k-time prior estimation error covariance matrix P(k|k-1), and the k-time measurement noise covariance matrix are the prior estimates. After data processing and feature extraction, the four variables are used as input signals of ANFIS; S4-3, according to the main diagonal elements of the extracted matching feature DoM(k), the main diagonal of the measurement noise covariance matrix is ​​adapted to obtain the adjusted measurement noise covariance matrix Among them, ΔR(k) is the adjustment matrix; S5. Take the robot starting point as the coordinate origin, set a three-dimensional absolute coordinate system, and take the counterclockwise tangent direction of the origin on the circumference of the pipeline cross section as the positive direction of the X-axis, the robot's movement direction along the pipeline as the positive direction of the Y-axis, and the vertical upward direction as the positive direction of the Z-axis; convert the estimated motion state of the pipeline climbing robot in the two-dimensional virtual plane into the optimal motion state in the three-dimensional absolute coordinate system through geometric relationships and mathematical operations, and obtain the robot's real position p(k), velocity v(k), and acceleration a(k) in the pipeline absolute coordinate system; the specific steps of step S5 are as follows: Assume that the central angle of the pipeline climbing robot moving from the starting point along the circumference of the cross section to the position at time k is The mathematical expression of the geometric relationship between the two-dimensional virtual coordinate system and the three-dimensional absolute coordinate system is: where p 2Dx (k) represents the two-dimensional virtual plane state variable output by the EKF at time k after a complete iteration of BPNN compensation and ANFIS covariance matching. The x-axis position coordinate; d represents the actual diameter of the inner wall of the pipe; Then the position coordinate point p of the two-dimensional virtual plane state 2D (k) = (p 2Dx (k), p 2Dy (k), p 2Dz (k)) and the position coordinate point p(k) = (p x (k), p y (k), p z (k)) of the final output three-dimensional pipeline state are converted as follows: p y (k)=p 2Dy (k), The velocity state point v of the two-dimensional virtual plane 2D (k)=(v 2Dx (k),v 2Dy (k),v 2Dz (k)) and the velocity state point v(k) = (v x (k),v y (k),v z The conversion process of (k)) is as follows: v y (k)=v 2Dy (k), Acceleration state point a of the two-dimensional virtual plane 2D (k)=(a 2Dx (k),a 2Dy (k),a 2Dz (k)) and the velocity state point a(k)=(a x (k0,a y (k),a z The conversion process of (k)) is as follows: a y (k)=a 2Dy (k), 2. According to claim 1, the dead reckoning method of the pipeline wall climbing robot based on multi-sensor fusion, Features: The establishment process of the BPNN model in step S3 is as follows: S3-1. Estimate the optimal state at time k-1 The acceleration vector Extract it and select the relative displacement vector d measured at time k opt (k) and and d opt (k) as input features of the BPNN model; S3-2. Based on the input features, the tangent transfer function is selected as the transfer function between the input layer and the first hidden layer, and between the first hidden layer and the second hidden layer, and the linear transfer function is selected as the transfer function between the second hidden layer and the output layer. A double hidden layer BPNN model is established with 5 nodes in the first hidden layer and 2 nodes in the second hidden layer.

3. According to claim 2, the dead reckoning method of the pipeline wall-climbing robot based on multi-sensor fusion, Features: In step S3-2, the process of using the BPNN model to compensate for the error is as follows: S3-2-1. Signals transmitted from the input layer to the first hidden layer The output signal y of the first hidden layer is j =tansig(c j ); The signal from the first hidden layer to the second hidden layer Among them, x q is the input feature, w qj represents the weight from the input layer to the first hidden layer, b j represents the offset of the first hidden layer; S3-2-2, the second hidden layer output signal z i =tansig(e i ), the signal transmitted from the second hidden layer to the output layer Among them, w i represents the weight from the second hidden layer to the output layer, and b represents the offset of the output layer; S3-2-3, the output signal h of the output layer is transferred through the linear transfer function, and finally the compensated acceleration prior estimate is obtained or where a' and b' are the coefficients of the transfer function.

4. A dead reckoning system for realizing the dead reckoning method of a pipeline wall-climbing robot based on multi-sensor fusion as described in any one of claims 1 to 3, It is characterized in that include: Inertial measurement unit, used to collect the acceleration of the pipeline climbing robot at every moment of movement; Optical navigation sensor, used to collect the relative displacement between two movement moments of the pipeline climbing robot; The host computer is used to process data to calculate the motion trajectory and positioning points of the pipeline climbing robot.

5. The estimation system according to claim 4, Features: The optical navigation sensor is arranged at the bottom of the pipeline wall-climbing robot.

Images