Design of in-pipe robot based on inertial positioning and visual detection

Cable’s towing resistance

This robot is mainly used for the pipeline laid by the HDD. These pipelines typically consist of three parts (shown in Figure 9): horizontal pipeline ( L 2 ), curve pipelines ( l 1 , l 2 ), and inclined straight pipelines ( L 1 , L 3 ). The curvature radius r of l 1 , l 2 is more than 400D (D: diameter of the pipeline); therefore, it can be approximately regarded as straight pipeline during the estimation of f cable .

OPEN IN VIEWER

Figure 9.

Underground pipelines.

Generally, α 1 and α 2 range from 10° to 20°, special pipelines to 30°. Assume that α 1 and α 2 are 35°. We should note that the sliding friction coefficient ( μ c ) between PVC (polyvinyl chloride) cable and pipeline changes with the material of the pipeline. When f cable is calculated, the maximum μ c (between the steel pipeline and the PVC cable) is used. The calculation parameters of f cable are designed in Table 4.

OPEN IN VIEWER

Table 4.

Calculation parameters.

L	200 m	l 1	Length: 6.84 m
α 1	35°		α3: 17°
α 2	35°	l 2	Length: 6.84 m
h	10 m		α4: 17°
L 1	13.95 m	ρ c (the cable’s linear density)	0.1 g/m
L 2	158.42 m
L 3	13.95 m	μ c	0.4

μ c is obtained from pages 36–37 of the book “Mechanical design manual.”

The cable’s towing resistance ³¹ is

F l = ρ c g ( μ c cos θ c + sin θ c ) · L

(1)

Using equation (1), f cable at the end of the underground pipelines can be calculated

f c a b l e = 76 . 19 N

(2)

To have adequate safety allowance, f cable = 100 N .

Locking condition analysis of LM

Reliable locking of LM is a precondition for the robot’s walking. As shown in Figure 8(a) and (b), to ensure reliable locking of LMA, N 1 , N 2 , and N 3 are exerted, so f 1 , f 2 , and f 3 will be created. The locking condition of LMA is written as

∑ i = 1 3 f i = ∑ i = 1 3 μ N i ≥ F L M A

(3)

where F LMA is the axial traction of LMA.

As long as the materials of the robot are determined, ∑ i = 1 3 f i only depends on N i . As shown in Figure 8(a), LMA, SWMA, and SWMB support the robot together. Corresponding to actual case, assuming that LMA, SWMA, and SWMB bear m 1 Mg , m 2 Mg , and m 3 Mg , respectively, ( m 1 + m 2 + m 3 = 1 ). Then, N 1 , N 2 , and N 3 can be presented as

{ N 1 = N leg 1 + 2 m 1 Mg cos θ · sin φ 3 N 2 = N leg 2 + 2 m 1 Mg cos θ · sin ( 120 ∘ − φ ) 3 N 3 = N leg 3

(4)

where N legi ( i = 1 , 2 , 3 ) is that each crutch leg exerts the force because of the LM screw’s thrust force F.

Similarly, N wj ( j = 1 , 2 , … , 6 ) can be obtained

{ N w 1 = N w + 2 m 2 Mg cos θ · sin φ 3 N w 2 = N w + 2 m 2 Mg cos θ · sin ( 120 ∘ − φ ) 3 N w 3 = N w { N w 4 = N w + 2 m 3 Mg cos θ · sin φ 3 N w 5 = N w + 2 m 3 Mg cos θ · sin ( 120 ∘ − φ ) 3 N w 6 = N w

Each LM has three crutch legs fixed circumferentially 120° apart from each other. Due to circumferential symmetry of LM’s crutch legs, it is considered that screw’s thrust force exerted on each crutch leg is F / 3 (an ideal case). Corresponding to actual case, screw’s thrust force exerted on each crutch leg N legi ( i = 1 , 2 , 3 ) is n 1 F , n 2 F , and n 3 F ( n 1 + n 2 + n 3 = 1 ). The mechanical model of the crutch leg is necessary to be established as shown in Figure 10

{ y D = d + l B sin β + r B x A = l A cos α + l B cos β l A sin α + r A = l B sin β + r B

(5)

OPEN IN VIEWER

Figure 10.

Mechanical model of the LM’s crutch leg.

The differential equation of equation (5) is

{ d y D = l B cos β d x A = − ( l A sin α + l B sin β ) l A cos α = l B cos β

(6)

Based on the principle of virtual displacement, there is

N leg 1 d y D + n 1 Fd x A = 0

(7)

Choosing r A = r B and l A = l B = l , there are geometric relations

{ x A = 2 l 2 − ( y D − d − r A ) 2 tan α = tan β = 2 · y D − d − r A x A

(8)

Equations (6)–(8) lead to

N leg 1 = 2 n 1 y D − d − r A l 2 − ( y D − d − r A ) 2 F

(9)

Similarly, N leg 2 and N leg 3 can be obtained

N leg 2 = 2 n 2 y D − d − r A l 2 − ( y D − d − r A ) 2 F N leg 3 = 2 n 3 y D − d − r A l 2 − ( y D − d − r A ) 2 F

According to the above equations, N 1 , N 2 , and N 3 can be presented

{ N 1 = 2 n 1 y D − d − r A l 2 − ( y D − d − r A ) 2 F + 2 m 1 Mg cos θ · sin φ 3 N 2 = 2 n 2 y D − d − r A l 2 − ( y D − d − r A ) 2 F + 2 m 1 Mg cos θ · sin ( 120 ∘ − φ ) 3 N 3 = 2 n 3 y D − d − r A l 2 − ( y D − d − r A ) 2 F

The locking condition of LMA is written as

∑ i = 1 3 f i = ∑ i = 1 3 μ N i = μ { 2 n 1 y D − d − r A l 2 − ( y D − d − r A ) 2 F + 2 m 1 Mg cos θ · sin φ 3 + 2 n 2 y D − d − r A l 2 − ( y D − d − r A ) 2 F + 2 m 1 Mg cos θ · sin ( 120 ∘ − φ ) 3 + 2 n 3 y D − d − r A l 2 − ( y D − d − r A ) 2 F } = μ { 2 y D − d − r A l 2 − ( y D − d − r A ) 2 F + 2 m 1 Mg cos θ · sin ( φ + 30 ∘ ) }

Above results indicate that the locking condition of LMA is not correlated with the screw’s thrust force exerted on each crutch leg N legi ( i = 1 , 2 , 3 ) ( n 1 F , n 2 F , and n 3 F ). The locking condition of LMA is related to m 1 Mg (LMA bears the gravity of the relevant). When m 1 = 0 , ∑ i = 1 3 f i has the minimum value ∑ i = 1 3 f i = ∑ i = 1 3 μ N i = μ · 2 ( ( y D − d − r A ) / ( l 2 − ( y D − d − r A ) 2 ) ) F . It is necessary to increase F for the reliable lock of the LM.

Screw’s load forces of TM

TM executes the contracting action when LMA is locked and LMB is unlocked (shown in Figure 8(a)). The axial load force of the screw of the TM ( F 1 ) is presented as

F 1 A = 1 2 Mg sin θ + μ w ∑ j = 4 6 N wj + f cable = 1 2 Mg sin θ + 3 μ w N w + 2 3 μ w Mg cos θ · sin ( φ + 30 ∘ ) + f cable

(10)

When LMA is unlocked and LMB is locked, the axial load force of the screw of the TM ( F 1 ) is written as

F 1 B = 1 2 Mg sin θ + μ w ∑ j = 1 3 N wj = 1 2 Mg sin θ + 3 μ w N w + 2 3 μ w Mg cos θ · sin ( φ + 30 ∘ )

(11)

The input and output signals of the state machine

The input signal of each walking pattern is as follows:

The values of the input signals and their definitions are shown in Table 7.

OPEN IN VIEWER

Table 7.

Definition of input signals.

Input signals		Values and definitions
		0	1
P	P3 (LMB)	Not reached	Reached
	P2 (LMA)	Not reached	Reached
	P1 (TM)	Not reached	Reached
N	N2 (LMB)	Not reached	Reached
	N1 (LMA)	Not reached	Reached
S		Not reached	Received
T		Not reached	Received

The output signal of each walking pattern consists of a four-bit binary number (M = M₄M₃M₂M₁), which is, respectively, the direction of rotation and the enabling signal of three motors. The values of the output signals and their definition are shown in Table 8, for example, M = 1010 expresses that motor of the LMA reverses.

OPEN IN VIEWER

Table 8.

Definition of output signals.

Output signals		Values and definitions
		0	1
M	M 4	Positive rotating	Negative rotating
	M3 (LMB)	Not enabled	Enabled
	M2 (LMA)	Not enabled	Enabled
	M1 (TM)	Not enabled	Enabled

State machine description of basic walking patterns

The walking patterns of the telescopic in-pipe robot can be divided into CFW, CBW, SSFW, and SSBW. On the basis of the walking principle of the robot, the control of the robot’s walking patterns can be attributed to the working state conversions among the motor of the TM, the motor of the LMA, and the motor of the LMB. Thus, the control logic of autonomous walking of the in-pipe robot is that how to convert the working state among three motors. Finite-state machine theory is good at describing finite state and its transformation. So, we use the finite-state machine to describe the robot walking patterns in order to get the state transition diagram.

The mathematical model of the state machine can be described as follows

M = ( Q , Σ , δ , q 0 , F )

where Q is the finite set of the state and it is not empty, Σ is the set of all input events received by the state machine, δ is the state transition function, q 0 is the initial state of the state machine, and F is the set of the state of termination.

According to the walking principle of the robot, the robot repeatedly executes the basic telescopic action until the monitor computer sends the stop instruction, so the state machine models of CFW and CBW are as follows

Q 1 = { S 0 , S 1 , S 2 , S 3 , S 4 , S 5 , S 6 , S 7 } ; Σ 1 = { P , N , S } ; q 10 = { S 0 } ; F 1 = { S 7 }

where S 0 is the initial state, S 1 is the state that LMB is locked, S 2 is the state that LMA is unlocked, S 3 is the state that TM stretches, S 4 is the state that LMA is locked, S 5 is the state that LMB is unlocked, S 6 is the state that TM contracts, and S 7 is the state of termination.

When the robot receives the CFW instruction, the robot begins the CFW pattern. The state transition diagram of CFW is shown in Figure 16.

OPEN IN VIEWER

Figure 16.

State transition diagram of CFW.

Similar to the CFW mode, the state transition diagrams of CBW, SSFW, and SSBW are obtained respectively. Single step walking pattern of the in-pipe robot can make operators carry out non-destructive testing of the pipeline step by step.

Conversion of walking patterns

The operators can change the walking patterns of the robot through the monitor computer. And the state machine model of the walking pattern conversion is as follows

Q 2 = { A 0 , A 1 , A 2 , A 3 , A 4 , A 5 } ; Σ 2 = { C } ; q 20 = { A 0 } ; F 2 = { A 5 }

where A₀ is the initial state, A₁ is the CFW, A₂ is the CBW, A₃ is the SSFW, A₄ is the SSBW, A₅ is the state of termination, and C is the control signal.

The values of the control signals and their definitions are shown in Table 9.

OPEN IN VIEWER

Table 9.

Definition of control signals.

C	A 0	A 1	A 2	A 3	A 4	A 5
Control signals	000	001	010	011	100	111

Based on the above analysis, the state transition diagram of the conversion of walking patterns is shown in Figure 17.

OPEN IN VIEWER

Figure 17.

State transition diagram of the conversion of walking patterns.

Attitude calculation method

The geographic coordinate system (east-north-up) is chosen for the navigation coordinate system ( O X n Y n Z n ). The carrier coordinate system ( O X b Y b Z b ) is defined as forward-left-up. The process that converts O X b Y b Z b to O X n Y n Z n is as follows

x b y b z b x b γ → x ′ b y ′ b z ′ b y ′ b θ → x b ″ y b ″ z b ″ z ′ ′ b ψ → x n y n z n

where ψ , θ , and γ are yaw, pitch, and roll angles, respectively.

So, the transformation matrix is

[ x n y n z n ] = [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] [ 1 0 0 0 cos γ − sin γ 0 sin γ cos γ ] [ x b y b z b ] = C b n [ x b y b z b ]

(14)

The direction cosine matrix C n b is an orthogonal matrix, so C n b = [ C b n ] T . C n b is defined as

C b n = [ T 11 T 12 T 13 T 21 T 22 T 23 T 31 T 32 T 33 ]

(15)

According to equations (14) and (15), the principal value of the three attitude angles can be derived as

{ ψ m = arctan ( T 21 T 11 ) θ m = arcsin ( − T 31 ) γ m = arctan ( T 32 T 33 )

(16)

The true value of the three attitude angles can be determined by the principal value, T 11 and T 33 , as follows

{ ψ = { ψ m + π T 11 < 0 ψ m T 11 > 0 , ψ m > 0 ψ m + 2 π T 11 > 0 , ψ m < 0 θ = θ m γ = { γ m T 33 > 0 γ m + π T 33 < 0 , γ m < 0 γ m − π T 33 < 0 , γ m > 0

(17)

The attitude matrix elements can be used to solve the attitude angle of O X b Y b Z b relative to O X n Y n Z n

C · b n = C b n ω nb bk

(18)

[ T . 11 T . 12 T . 13 T . 21 T . 22 T . 23 T . 31 T . 32 T . 33 ] = [ T 11 T 12 T 13 T 21 T 22 T 23 T 31 T 32 T 33 ] [ 0 − ω n b z b ω n b y b ω n b z b 0 − ω n b x b − ω n b y b ω n b x b 0 ]

(19)

where ω nb bk is the antisymmetric matrix of the ω nb b .

Therefore, as long as the initial attitude angle is given, the differential equation can be determined by the attitude angle rate ω nb b , and then the attitude angle of the carrier can be determined at each time.

3D dead reckoning

The 3D dead reckoning is the way that calculates the trace only using attitude angles and the mileage information of the system. Its principle is to divide the trajectory into a number of segments, each of which is determined by the length of the system’s movement within a sampling period (shown in Figure 18).

OPEN IN VIEWER

Figure 18.

Vector’s projection.

The projection can be written as

{ Δ X = Δ l · cos ( θ ) · cos ( ψ ) Δ Y = Δ l · cos ( θ ) · sin ( ψ ) Δ Z = Δ l · sin ( θ )

(20)

where Δ X , Δ Y , and Δ Z are the variety vector of x, y, and z, respectively; θ and ψ are the vector’s pitch angle and yaw angle; and Δ l is the vector’s length.

Attitude angles’ measurement

The attitude angles’ measurement method uses gyroscopes for underground applications. Gyroscopes are immune to ferromagnetic and electromagnetic disturbances, but they have considerable DC drifts in a relatively long period of time. The accelerometer has high precision when it is in the static state, but it has poor accuracy when it is in motion. So, a reliable and precise way to measure pitch angle and roll angle is using a 3-axes accelerometer when the robot is in static state. The pitch angle and roll angle are regarded as the initial value of the gyroscope. When the robot is in motion, the output of the gyroscope serves as the attitude angles.

According to the attitude matrix C b n , the output of the accelerometer A b can be calculated

A b = [ C b n ] T A n

where A b = [ a x a y a z ] T and A n = [ 0 0 g ] T . A n is the output of the accelerometer in unaccelerated state when the navigation coordinate system ( O X n Y n Z n ) and the carrier coordinate system ( O X b Y b Z b ) coincide.

The pitch angle and roll angle can be expressed as follows

{ θ = arcsin ( − a x g ) γ = arctan ( a y a z )

The three axes of the accelerometer are mutually perpendicular. When one axis is at its minimum sensitivity of gravitational acceleration, the other two axes combined will be at the maximum sensitivity. A sensitivity of approximately 17.45 mg/degree at any tilt orientation could be maintained for pitch and roll angles. ³³ Most of the low-cost MEMS (micro-electro mechanical system) accelerometers can provide 0.1–1 mg resolution. This is accurate enough for the pitch and roll angles.

To improve the measuring precision of the attitude angles, the static calibration system of the accelerometer is established, and the error compensation of the accelerometer is calculated. The extended Kalman filter (EKF) is designed to filter out the A b . The idea of the EKF is to linearize the nonlinear system around the Kalman filter estimate, and the Kalman filter estimate is based on the linearized system.

The nonlinear system equations are given as follows

x · ( t ) = f ( x ( t ) ) + w ( t ) Z ( t ) = h ( x ( t ) ) + v ( t )

where the noise processes w ( t ) and v ( t ) are white, zero-mean, and uncorrelated.

The EKF can be summarized as the time-update equations and the measurement-update equations. The procedure of this algorithm is as follows:

x ^ k + 1 | k = x ^ k + f ( x ^ k ) · Δ t + A ( x ^ k ) · f ( x ^ k ) · Δ t 2 2 P k + 1 | k = Φ k + 1 , k P k Φ k + 1 , k T + Q k

where x ^ is the state estimation, P is the covariance of the state estimation error, A ( x ^ k ) = ∂ f / ∂ t , and Φ k + 1 , k = I + A ( x ^ k ) · Δ t .

K k + 1 = P k + 1 | k H k + 1 T ( H k + 1 P k + 1 | k H k + 1 T + R k + 1 ) − 1 x ^ k + 1 = x ^ k + 1 | k + K k + 1 [ z k + 1 − h ( x ^ k + 1 | k ) ] P k + 1 = ( I − K k + 1 H k + 1 ) P k + 1 | k

where K is the Kalman filter gain and H k + 1 = ∂ h / ∂ x .

The pitch angle and roll angle are chosen as the state of the system, and a x and a y / a z are the measurement. When a z is close to 0, a y / a z is a singular value. There are two particular cases about a y / a z : when a x is not close to g, a y / a z = tan { arcsin ( a y / ( g 2 − a x 2 ) ) } , otherwise a y / a z = 0 .

So the discrete-time equations of the system are as follows

[ θ ( k + 1 ) γ ( k + 1 ) ] = [ θ ( k ) γ ( k ) ] + w

z ( k ) = [ a x ( k ) a y ( k ) a z ( k ) ] = { − g sin θ ( k ) tan γ ( k )

Assuming that the accelerometer can provide 1-mg resolution, the result of EKF simulation is shown in Figure 19. According to Table 10, the pitch angle error is less than 2″ and the roll angle error is less than 3″. It is accurate enough for the attitude angles’ measurement of static state.

OPEN IN VIEWER

Figure 19.

Result of EKF simulation.

OPEN IN VIEWER

Table 10.

Error of the pitch and roll angles.

	Pitch angle error (″)	Roll angle error (″)
Mean	−0.0098	0.0047
Covariance	0.1967	0.3010

Finally, the robot’s movement information is calculated by the 3D dead reckoning. The robot’s movement information consists of many relative coordinates. The starting point and the ending point of the pipeline (measured by the whole station instrument or global positioning system (GPS)) are used to improve the accuracy of the robot’s inertial positioning. We develop the conversion from the relative coordinate system to the geographic coordinate system using the starting point and the ending point of the pipeline.