閆雪嬌,謝 哲,付宏文,蔣 松

（上海宇航系統(tǒng)工程研究所,上海 201108)

0 引言

隨著城市化進(jìn)程的加快,石油、天然氣和飲用水的運輸逐漸依賴于管道,管道在人們的生活中起到了越來越重要的作用[1-3]。近年來,多起管道泄漏爆炸事故為我們敲響了警鐘,管道安全不僅關(guān)系到經(jīng)濟(jì)的發(fā)展,也與社會穩(wěn)定息息相關(guān),一旦油氣管道發(fā)生事故,就會造成嚴(yán)重的生命和財產(chǎn)損失。

對管道進(jìn)行定期檢測可以提早發(fā)現(xiàn)管道故障并進(jìn)行維修,這是避免管道事故的常見方法。管道定位作為管道檢測中的重要一環(huán),在管道安全運行中起到了重要的作用。國內(nèi)外研究人員參考鐵軌檢測小車的形式,采用管道機器人進(jìn)行管道檢測。但因管道大多深埋地下,管道機器人搭載的檢測設(shè)備無法接收到衛(wèi)星導(dǎo)航信號,通常在管道機器人上加裝慣性測量單元（Inertial Measurement Unit,IMU)和里程計等傳感器構(gòu)建導(dǎo)航系統(tǒng),實現(xiàn)地下管道的定位和檢測[4]。目前,常用的管道定位方法有純慣性導(dǎo)航定位方法、視覺同步定位與映射方法和基于IMU/里程計組合定位方法等。由于純慣性系統(tǒng)存在誤差隨時間累積的問題以及視覺同步定位方法存在長輸氣管道不適用性的缺陷,基于IMU和里程計的組合導(dǎo)航定位方法成為了主流。岳步江等[5]設(shè)計了一種IMU/里程計/地面標(biāo)記器的組合定位方法,紀(jì)文濤等[6]使用Kalman濾波將IMU和里程計數(shù)據(jù)進(jìn)行融合實現(xiàn)了管道的定位,Sahli等[7]提出了一種基于高精度三維簡化慣性系統(tǒng)和里程計組合的管道定位方法,上述方法數(shù)據(jù)融合方式復(fù)雜,對傳感器要求較高,且定位檢測方法復(fù)雜。

本文從實際工程應(yīng)用角度出發(fā),針對油氣管道定位問題,設(shè)計了一種基于IMU/輪式里程計的管道定位方法。該方法使用搭載在管道機器人上的IMU和里程計測量載體的運動信息,經(jīng)離線解算獲得管道機器人的運動軌跡。針對初始對準(zhǔn)不準(zhǔn)確造成的軌跡偏差,使用基于首尾點坐標(biāo)的軌跡校正方法補償初始條件帶來的常值角度誤差,得到準(zhǔn)確的管道定位結(jié)果,使油氣管道的定位成為可能。

1 管道定位系統(tǒng)概述

基于IMU的慣性導(dǎo)航系統(tǒng)自主性高,能獲得較全面的導(dǎo)航信息,但存在累積誤差的問題。在不發(fā)生相對摩擦的條件下,里程計可測量載體準(zhǔn)確的速度信息。使用IMU解算的姿態(tài)信息和里程計的速度信息即可實現(xiàn)管道機器人行進(jìn)軌跡解算,滿足地下管道定位的需求[8]。管道定位系統(tǒng)原理圖如圖1所示,其由IMU、里程計和存儲設(shè)備組成。在管道機器人進(jìn)行測量作業(yè)時,將傳感器的測量數(shù)據(jù)存入存儲設(shè)備,待管道機器人走完全程后再對數(shù)據(jù)進(jìn)行離線分析,完成整個管道定位過程。

圖1 管道定位系統(tǒng)原理圖Fig.1 Schematic diagram of pipeline positioning system

測量工作完成后,從數(shù)據(jù)存儲設(shè)備中讀取IMU數(shù)據(jù)和里程計數(shù)據(jù),解算IMU中的陀螺儀數(shù)據(jù)獲得管道機器人的姿態(tài)信息,解算里程計數(shù)據(jù)獲得速度信息。利用上述的姿態(tài)信息和速度信息計算管道機器人的相對位置,進(jìn)而繪制出管道機器人的運動軌跡。由于初始對準(zhǔn)階段設(shè)定的初始值精度不高,此時的管道機器人運動軌跡與其真實軌跡之間存在一個常值角度偏差,本文采用基于首尾點坐標(biāo)的軌跡校正方法對該角度偏差進(jìn)行補償,最終獲得準(zhǔn)確的管道定位信息。

2 基于IMU和里程計的管道定位方法

本文設(shè)計的管道定位方法是一種離線的數(shù)據(jù)處理方法,其算法流程如圖2所示。

圖2 定位算法流程圖Fig.2 Flowchart of positioning algorithm

IMU和里程計完成管道數(shù)據(jù)采集后,首先對IMU進(jìn)行初始對準(zhǔn),確定捷聯(lián)矩陣的初始值。然后解算IMU陀螺儀數(shù)據(jù),獲得管道機器人姿態(tài)信息,結(jié)合輪式里程計解算的速度信息確定管道機器人的運動軌跡。最后,采用首尾點坐標(biāo)校正方法補償常值角度誤差,實現(xiàn)油氣管道的定位。

2.1 坐標(biāo)系及姿態(tài)定義

本文涉及的坐標(biāo)系定義如下,坐標(biāo)系之間的關(guān)系如圖3所示。

圖3 坐標(biāo)系關(guān)系圖Fig.3 Relation diagram of each coordinate system

1)導(dǎo)航坐標(biāo)系（n系,OnXnYnZn):即管道機器人相對地球位置的坐標(biāo)系,是導(dǎo)航系統(tǒng)工作時選取的基準(zhǔn)[9]。本文選取東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系,以管道機器人中心為原點,OnXn指向地球正東,OnYn指向地球正北,OnZn沿垂線方向指天向。

2)管道機器人坐標(biāo)系（b系,ObXbYbZb):即固連在管道機器人上的坐標(biāo)系。本文定義右前上坐標(biāo)系為管道機器人坐標(biāo)系,其坐標(biāo)原點與導(dǎo)航坐標(biāo)系重合,為管道機器人中心,ObXb沿管道機器人橫軸指向右側(cè),ObYb沿管道機器人縱軸指向前方,ObZb由原點指向管道機器人上方。

導(dǎo)航坐標(biāo)系和管道機器人坐標(biāo)系不重合,兩者之間的夾角恰好反映了管道機器人運動的姿態(tài)角[10]。本文規(guī)定管道機器人坐標(biāo)系ObXbYbZb繞X軸旋轉(zhuǎn)的角度為俯仰角,繞Y軸旋轉(zhuǎn)的角度為橫滾角,繞Z軸旋轉(zhuǎn)的角度為航向角。將管道機器人坐標(biāo)系按照一定順序繞不同的坐標(biāo)軸進(jìn)行旋轉(zhuǎn)后,兩坐標(biāo)軸即可重合,使兩坐標(biāo)軸重合的旋轉(zhuǎn)順序和角度組成的矩陣即為方向余弦矩陣Cnb。

2.2 IMU的初始對準(zhǔn)

初始對準(zhǔn)在慣性導(dǎo)航系統(tǒng)中尤為重要,只要給定導(dǎo)航的初始條件,便可根據(jù)IMU的數(shù)據(jù)解算出各種導(dǎo)航信息[11]。初始對準(zhǔn)的任務(wù)是將載體的初始位置和初始速度引入慣性導(dǎo)航系統(tǒng),建立初始方向余弦矩陣,使管道機器人坐標(biāo)系與導(dǎo)航坐標(biāo)系相重合。

初始對準(zhǔn)在管道機器人測量初始時進(jìn)行,此時管道機器人處于靜止?fàn)顟B(tài),載體的初始速度為零,初始位置為管道起始點坐標(biāo)。由于管道機器人處于靜止?fàn)顟B(tài),所以其三個軸向的加速度向量之和等于重力加速度,可計算管道機器人的初始俯仰角和橫滾角,求解公式如下

式（1)中,θ0為管道機器人的初始俯仰角,?0為管道機器人的初始橫滾角,ax、ay和az分別為管道機器人靜止時加速度計三個軸向的測量值,g為當(dāng)?shù)氐闹亓铀俣龋ǔＶ?。

管道機器人的初始方位角可根據(jù)管道首尾點坐標(biāo)進(jìn)行計算,計算公式如下

式（2)中,φ0為管道機器人的初始方位角,（xs,ys)和（xe,ye)分別為管道起始點和終止點位置坐標(biāo)（x為東向坐標(biāo),y為北向坐標(biāo))。根據(jù)上文計算的初始姿態(tài),可求出IMU的初始方向余弦矩陣,完成IMU的初始對準(zhǔn)。

2.3 IMU/里程計的軌跡解算

慣性系統(tǒng)解算出的速度和位置信息具有誤差隨時間積累的缺點,為獲得準(zhǔn)確的管道機器人運動軌跡,本文結(jié)合IMU解算的姿態(tài)與里程計的速度求取管道機器人的位置信息,完成管道定位,軌跡解算原理圖如圖4所示。

圖4 軌跡解算原理Fig.4 Principle of trajectory calculation

（1)IMU姿態(tài)矩陣

IMU姿態(tài)矩陣即IMU的方向余弦矩陣,是b系到n系的坐標(biāo)轉(zhuǎn)換,常用來表示,計算方向余弦矩陣的常用方法有方向余弦法、Euler角法和四元數(shù)法。方向余弦法未知數(shù)數(shù)目較多,計算量較大;Euler角法求解存在奇點,不能對所有姿態(tài)進(jìn)行求解,故采用計算量較小的四元數(shù)法進(jìn)行姿態(tài)解算[12]。

四元數(shù)由1個實數(shù)單位和3個虛數(shù)單位組成,表示的是管道機器人坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)動,其表達(dá)形式如下

本文采用四階Runge Kutta法求解四元數(shù)微分方程,根據(jù)IMU的陀螺儀數(shù)據(jù)實時更新四元數(shù),計算出管道機器人的姿態(tài)矩陣,其計算公式如下

（2)里程計速度計算

輪式里程計是管道機器人搭載的常用傳感器,可以測量載體的行進(jìn)速度和路程[13-14]。為準(zhǔn)確測量管道機器人的行進(jìn)速度,減小在管道中里程輪發(fā)生打滑、飛轉(zhuǎn)等情況帶來的誤差,本文在管道機器人后輪輪系搭載了兩組輪式里程計測量載體速度,最終取左右兩輪的平均速度作為管道機器人的b系速度。

本文采用的里程計采樣頻率為100Hz,輪盤每轉(zhuǎn)動一周輸出3個周期方波波形,根據(jù)相鄰兩點的數(shù)據(jù)差計算里程計測量的載體速度,其公式如下

式（6)中,vd為里程計測量的管道機器人速度,k為里程計刻度因數(shù),d為里程計輪盤直徑,Δn為里程計采集的相鄰兩點數(shù)據(jù)差,Δt為采樣時間間隔。

里程計搭載在管道機器人的車輪上,測量的是管道機器人b系上的前向速度,故管道機器人的b系速度為

（3)軌跡解算

管道機器人的定位是在n系下進(jìn)行的,需將里程計測量的b系速度轉(zhuǎn)換到n系下,方可解算出管道機器人的運動軌跡。

獲得管道機器人在導(dǎo)航坐標(biāo)系下的速度后,將速度對時間進(jìn)行累加,計算管道機器人的相對位置坐標(biāo),與管道起始點坐標(biāo)相加后,得到管道機器人的絕對位置坐標(biāo),即為載體的運動軌跡。管道機器人的絕對位置計算公式如下

3 基于首尾基準(zhǔn)點的管道定位校正方法

初始對準(zhǔn)在IMU數(shù)據(jù)解算中尤為重要,設(shè)定準(zhǔn)確的初始姿態(tài)矩陣,解算出的姿態(tài)信息才不會出現(xiàn)偏差。在上述管道定位算法中,由于初始對準(zhǔn)階段設(shè)定的姿態(tài)存在誤差,導(dǎo)致解算出的每一點與管道起始點之間的連線以及該點的基準(zhǔn)點與起始點之間的連線存在常值角度誤差。針對這一問題,本文提出了基于首尾基準(zhǔn)點的管道定位校正方法。

解算軌跡與實際軌跡之間的角度誤差為常值,可根據(jù)解算出的始末點位置信息及其對應(yīng)的基準(zhǔn)點位置信息計算角度誤差并在解算軌跡中進(jìn)行補償,以達(dá)到管道定位信息校正的目的。

以求解方位角常值誤差為例,根據(jù)已知的被測管道基準(zhǔn)始末點位置坐標(biāo),計算被測管道終止點相對于起始點的方向角,如式（10)所示。通過解算出的始末點位置坐標(biāo),計算解算的終止點相對于起始點的方向角,如式（11)所示。

式（10)、 式（11)中, （xs,ys)和（xe,ye)分別為被測管道實際起始點和終止點的位置坐標(biāo),（xss,yss)和（xes,yes)分別為解算軌跡的起始點和終止點的位置坐標(biāo)（x為東向坐標(biāo),y為北向坐標(biāo))。

最終,計算得到的方位角常值誤差為

同理,可計算俯仰角和橫滾角的常值誤差Δθ和Δ?。值得注意的是,管道機器人沿管道壁進(jìn)行作業(yè),工作時對橫滾角影響較小,因此本文忽略了橫滾角的常值誤差。

該旋轉(zhuǎn)矩陣實現(xiàn)了解算軌跡的常值角度誤差校正。

在實際應(yīng)用中,由于里程計輪盤測量不準(zhǔn)確等原因,里程計的刻度因數(shù)k會存在誤差,影響管道機器人的定位精度。為獲得準(zhǔn)確的定位信息,需對里程計的刻度因數(shù)進(jìn)行標(biāo)定,本文采用基于首尾點坐標(biāo)的方式來標(biāo)定里程計的刻度因數(shù)。

里程計刻度因數(shù)k在數(shù)值上等于解算軌跡長度與實際軌跡長度的比值,本文根據(jù)解算的始末點位置和實際始末點位置標(biāo)定里程計的刻度因數(shù),標(biāo)定公式如下

根據(jù)上文所述的旋轉(zhuǎn)矩陣和里程計刻度因數(shù)校正解算的管道機器人行進(jìn)軌跡,校正公式如下

式（15)中,X-為校正前的管道機器人行動軌跡,X為校正后的管道機器人行動軌跡。

4 管道定位測繪實驗

為驗證本文提出的軌跡校正方法的有效性,本文進(jìn)行了管道定位測繪實驗。為直觀地顯示本文方法的有效性,解算時分別繪制了校正前管道定位信息、校正后管道定位信息和被測管道的基準(zhǔn)點位置信息,計算軌跡校正后本文方法的定位精度,驗證本文方法的有效性和工程應(yīng)用價值。

管道機器人由牽引裝置拖拽在管道內(nèi)做往復(fù)運動,管道機器人車輪直徑為50mm,利用管道機器人搭載的IMU和里程計采集數(shù)據(jù),實驗時傳感器的采樣頻率為100Hz。通過本文設(shè)計的管道定位算法解算管道機器人軌跡,得到管道的定位信息。管道機器人如圖5所示,IMU和里程計分別搭載在管道機器人機身和后側(cè)左右輪盤上。本文實驗的定位基準(zhǔn)為RTK測得的管道中心線數(shù)據(jù),分別在兩個場景中進(jìn)行實驗:一個是在果園里搭建的94m的S型管道,另一個是在農(nóng)場地面鋪設(shè)的900m管道。

圖5 實驗用輪式管道機器人Fig.5 Diagram of wheeled pipeline robot for experiment

4.1 94m管道定位實驗

94m管道的實驗場景如圖6所示,其大致形狀為 “S”形。利用管道機器人采集管道數(shù)據(jù)后,使用本文方法對采集的數(shù)據(jù)進(jìn)行處理,分別繪制校正前、校正后的管道定位結(jié)果和管道中心線基準(zhǔn)的俯視圖和剖面圖,如圖7所示。

圖6 94m管道實驗場景Fig.6 Diagram of 94m pipeline experiment scenario

圖7 校正前后的94m管道定位結(jié)果Fig.7 Positioning results of 94m pipeline before and after correction

由圖7可知,校正前與校正后的管道機器人軌跡之間的角度誤差為常值,校正前的管道定位結(jié)果與管道基準(zhǔn)相差較大,校正后的管道定位結(jié)果與管道基準(zhǔn)基本重合,說明了本文提出的管道定位校正方法的有效性。

為直觀說明本文方法的有效性,現(xiàn)在管道中選取15個采樣點,分別計算采樣點上校正后的軌跡坐標(biāo)與管道基準(zhǔn)坐標(biāo)之間的誤差,統(tǒng)計水平定位誤差和高程定位誤差的最大值、均值和方差,計算結(jié)果如表1所示。

由表1可知,本文提出的管道定位校正方法能實現(xiàn)較為準(zhǔn)確的定位。校正后的管道水平誤差和高程誤差的均值和方差均小于0.11%D,說明本文方法在管道定位數(shù)據(jù)解算中具有穩(wěn)定性。同時,管道定位水平誤差和高程誤差分別為0.19m和0.09m,由定位精度等于定位誤差與管道機器人航程之比可知,94m管道的水平定位精度為0.20%D,高程定位精度為0.10%D,該定位精度滿足油氣管 道測繪要求。

表1 校正后94m管道定位誤差分析結(jié)果Table 1 Analysis results of 94m pipeline positioning error after correction

4.2 900m管道定位實驗

900m管道的實驗場景如圖8所示,其形狀為帶有小幅度拐點的直線形。管道機器人采集管道數(shù)據(jù)后,使用本文方法處理數(shù)據(jù),結(jié)果如圖9所示。

圖8 900m管道實驗場景Fig.8 Diagram of 900m pipeline experiment scenario

圖9 校正前后的900m管道定位結(jié)果Fig.9 Positioning results of 900m pipeline before and after correction

與94m管道定位結(jié)果相同,校正前后900m管道定位結(jié)果之間的角度誤差為常值,校正后的管道定位結(jié)果與管道基準(zhǔn)基本重合,表明了本文提出的管道定位校正方法具有普遍適用性。

經(jīng)計算,校正后900m管道定位結(jié)果的水平誤差和高程誤差如表2所示。由表2可知,校正后900m管道直線部分的水平定位誤差為0.16%D,高程定位誤差為0.10%D,彎道部分的最大水平定位誤差為0.18%D,高程定位誤差為0.10%D,且最大定位誤差點出現(xiàn)在轉(zhuǎn)彎或臨近轉(zhuǎn)彎處。

表2 校正后900m管道定位誤差Table 2 Positioning error of 900m pipeline after correction

綜上所述,本文設(shè)計的管道定位校正方法能有效補償管道定位誤差,該方法具有普遍適用性。該管道定位方法在油氣管道定位中精度較高,尤其在直線形管道的定位中具有明顯優(yōu)勢。當(dāng)管道存在拐點時,拐點的幅度越小,其定位精度越高。但無論何種形狀的管道,其水平定位精度均小于航程的0.20%,說明了本文方法在管道定位上的有效性。且常規(guī)管道多為直線型,相較于彎道數(shù)據(jù)解算,直線形管道數(shù)據(jù)的解算結(jié)果更有意義。

5 結(jié)論

本文設(shè)計了一種基于IMU和里程計的管道定位方法,通過解算IMU和里程計數(shù)據(jù),實現(xiàn)了油氣管道的定位。針對初始對準(zhǔn)引入的常值角度誤差,提出了基于首尾點坐標(biāo)的軌跡校正方法,提高了管道的定位精度。采用管道測繪實驗對本文方法進(jìn)行驗證,實驗結(jié)果表明:本文提出的管道定位校正方法可有效補償初始對準(zhǔn)引入的角度常值誤差,校正后的管道定位精度較高,且具有較好的定位穩(wěn)定性。本文方法在管道定位中表現(xiàn)良好,水平定位誤差和高程定位誤差均小于航程的0.20%,可用于實際管道的定位測繪。在后續(xù)的管道定位中,將重點研究慣性導(dǎo)航轉(zhuǎn)角、管道打滑等條件下的可靠定位問題。