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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

本文涉及的坐標系定義如下,坐標系之間的關系如圖3所示。

圖3 坐標系關系圖Fig.3 Relation diagram of each coordinate system

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

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

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

2.2 IMU的初始對準

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

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

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

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

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

2.3 IMU/里程計的軌跡解算

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

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

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

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

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

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

（2)里程計速度計算

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

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

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

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

（3)軌跡解算

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

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

3 基于首尾基準點的管道定位校正方法

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

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

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

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

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

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

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

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

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

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

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

4 管道定位測繪實驗

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

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

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

4.1 94m管道定位實驗

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

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

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

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

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

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

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

4.2 900m管道定位實驗

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

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

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

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

經(jīng)計算,校正后900m管道定位結果的水平誤差和高程誤差如表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

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

5 結論

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