一種基于慣性測(cè)量單元和里程計(jì)的油氣管道定位方法

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

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

0 引言

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

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

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

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

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

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

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

2 基于IMU和里程計(jì)的管道定位方法

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

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

IMU和里程計(jì)完成管道數(shù)據(jù)采集后,首先對(duì)IMU進(jìn)行初始對(duì)準(zhǔn),確定捷聯(lián)矩陣的初始值。然后解算IMU陀螺儀數(shù)據(jù),獲得管道機(jī)器人姿態(tài)信息,結(jié)合輪式里程計(jì)解算的速度信息確定管道機(jī)器人的運(yùn)動(dòng)軌跡。最后,采用首尾點(diǎn)坐標(biāo)校正方法補(bǔ)償常值角度誤差,實(shí)現(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):即管道機(jī)器人相對(duì)地球位置的坐標(biāo)系,是導(dǎo)航系統(tǒng)工作時(shí)選取的基準(zhǔn)[9]。本文選取東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系,以管道機(jī)器人中心為原點(diǎn),OnXn指向地球正東,OnYn指向地球正北,OnZn沿垂線方向指天向。

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

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

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

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

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

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

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

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

2.3 IMU/里程計(jì)的軌跡解算

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

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

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

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

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

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

（2)里程計(jì)速度計(jì)算

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

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

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

里程計(jì)搭載在管道機(jī)器人的車輪上,測(cè)量的是管道機(jī)器人b系上的前向速度,故管道機(jī)器人的b系速度為

（3)軌跡解算

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 管道定位測(cè)繪實(shí)驗(yàn)

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

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

圖5 實(shí)驗(yàn)用輪式管道機(jī)器人Fig.5 Diagram of wheeled pipeline robot for experiment

4.1 94m管道定位實(shí)驗(yàn)

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

圖6 94m管道實(shí)驗(yàn)場(chǎng)景Fig.6 Diagram of 94m pipeline experiment scenario

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

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

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

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

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

4.2 900m管道定位實(shí)驗(yàn)

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

圖8 900m管道實(shí)驗(yàn)場(chǎng)景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)計(jì)算,校正后900m管道定位結(jié)果的水平誤差和高程誤差如表2所示。由表2可知,校正后900m管道直線部分的水平定位誤差為0.16%D,高程定位誤差為0.10%D,彎道部分的最大水平定位誤差為0.18%D,高程定位誤差為0.10%D,且最大定位誤差點(diǎn)出現(xiàn)在轉(zhuǎn)彎或臨近轉(zhuǎn)彎處。

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

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

5 結(jié)論

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