楊 洋李 賓袁 泉楊理踐谷 碩

(1.遼寧大學(xué)物理學(xué)院，遼寧 沈陽110036；2.沈陽工業(yè)大學(xué)信息科學(xué)與工程學(xué)院，遼寧 沈陽110870)

隨著我國城鎮(zhèn)化發(fā)展和管道運(yùn)輸?shù)臄U(kuò)張，地下管線的交叉重疊現(xiàn)象日益嚴(yán)重，而隨著時間的推移，有些管道的位置信息常常因?yàn)閬G失而無法獲得，因此急需一種可安全、準(zhǔn)確提供管道位置的檢測技術(shù)。將低成本微機(jī)電器件(Micro-Electro-Mechanical System，MEMS)搭載移動平臺的管道內(nèi)檢測系統(tǒng)可以實(shí)現(xiàn)對封閉管道的三維位置檢測，其采用慣性測量單元(IMU)采集檢測器運(yùn)動的慣性信息，利用航跡推算(DR)得到檢測器在管道中運(yùn)動的軌跡信息，即管道位置信息。 該方法不受外界條件干擾、不用挖開整段管道，不受管道材質(zhì)的限制，并且可以實(shí)現(xiàn)定期檢測，是一種有效的檢測方法。 采用MEMS 器件具有低成本、低功耗、輕便靈巧等優(yōu)點(diǎn)，適合小口徑地下管道的檢測要求，但其測量精度較低，噪聲干擾較大，難以保證長時間的檢測精度，需要加入有效的校正措施，論文針對這一問題展開研究工作[1-3]。

Sadovnychiy S 和Jaejong Y 等采用基于戰(zhàn)術(shù)級的光纖陀螺儀(LN200)對76.2 cm 直徑58 km 長度的管道進(jìn)行定位，該方法亦通過反演計算得到管內(nèi)行走檢測器的軌跡信息，進(jìn)而推斷管道的位置，在離線解算條件下，采用改進(jìn)的雙濾波器固定區(qū)間平滑算法，每隔800 m 行走距離對管道位置利用DGPS進(jìn)行標(biāo)定，應(yīng)用里程計對慣導(dǎo)解算(SINS)的誤差進(jìn)行校正，因?yàn)閭鞲衅骶容^高，因此需要考慮地球自轉(zhuǎn)和地表曲率引起的定位誤差，對于MEMS 型器件，因其較低的測量精度敏感不到這些信息，所以將該類誤差在模型方程中忽略，這造成誤差模型與實(shí)際不匹配，將有較大概率在雙濾波平滑的反向遞推中出現(xiàn)奇異陣求逆情況，造成濾波器失效[4-5]。 牛小驥等采用MEMS 器件測量小口徑管道，用RTS 平滑(Rauch-Tung-Striebel)代替雙濾波平滑，采用慣導(dǎo)/里程計/非完整性約束平滑濾波算法，測量2km直線距離最大誤差10 m[6-7]，該方法如果加入額外的測量信息，則將進(jìn)一步提高測量精度。 對此，Wei Zhao 等發(fā)現(xiàn)在鐵磁性管道中，某些條件下仍然可以測得地磁信號，這表明管道內(nèi)檢測可以引用地磁信號作為測量信息[8]，但由于缺少約束條件，地磁信號只能對運(yùn)動檢測器三個姿態(tài)中的偏航角進(jìn)行觀測。 賈瑞才提出重力/地磁輔助濾波估計算法提升低成本MEMS 慣導(dǎo)系統(tǒng)的姿態(tài)測量精度，將重力值近似為常量，利用靜態(tài)加速度計測量值對檢測器的傾角進(jìn)行觀測，與地磁的測得的姿態(tài)角互相補(bǔ)充，實(shí)現(xiàn)對慣導(dǎo)解算三個姿態(tài)角的測量與校正[9]，實(shí)驗(yàn)表明通過加入校正措施，姿態(tài)角估計的精度得到了提高，如能應(yīng)用于管道定位中，可為RTS 平滑提供較全面的校正參數(shù)。 但RTS 平滑作為一種數(shù)據(jù)后處理算法，雖然優(yōu)于常規(guī)的Kalman 濾波算法，卻仍然嚴(yán)重依賴初始靜態(tài)對準(zhǔn)的精度，康泰鐘等提出了基于時間雙向解算融合的位姿測量事后處理算法，利用凸組合融合最優(yōu)原則將時間正向解算結(jié)果和時間逆向解算結(jié)果進(jìn)行融合，充分利用整段測量數(shù)據(jù)進(jìn)一步提高了測量精度[10]，因?yàn)楣艿罍y量可以滿足初始和終止的靜態(tài)對準(zhǔn)條件，所以適合采用此算法，但進(jìn)行多次求逆運(yùn)算降低了方差估計的精度，需要進(jìn)行有效改進(jìn)。 論文擬在前面研究者的工作基礎(chǔ)上，根據(jù)管道定位的測量條件，對現(xiàn)有的測量系統(tǒng)進(jìn)行改進(jìn)和提高，進(jìn)一步增加定位精度，降低測量成本。

論文第一部分介紹了管內(nèi)檢測系統(tǒng)的工作原理；第二部分介紹了DR 算法的基本原理，以及如何通過各種輔助方法得到濾波校正的測量參數(shù)；第三部分闡述了改進(jìn)的RTS 平滑濾波算法:包括建立kalman 濾波的模型方程，對方程進(jìn)行雙向平滑濾波，利用雙向解算誤差的相干性進(jìn)行數(shù)據(jù)融合；第四部分介紹了實(shí)驗(yàn)驗(yàn)證過程，實(shí)驗(yàn)結(jié)果證明了算法的有效性；第五部分對實(shí)驗(yàn)結(jié)果進(jìn)行了分析和討論。

1 管道內(nèi)檢測系統(tǒng)工作原理

1.1 檢測裝置工作原理

檢測器由可以在管道內(nèi)運(yùn)動的可移動機(jī)械平臺(可選擇管道清管器)，搭載在平臺內(nèi)的電路系統(tǒng)及電池三部分組成，電池和電路系統(tǒng)用非金屬材料密封并固定在平臺內(nèi)，以防止短路和沖擊引起的破壞。電路系統(tǒng)由嵌入式系統(tǒng)，MEMS 型的慣性測量單元(IMU)、里程測量裝置、電磁測量裝置和存儲器組成，系統(tǒng)功能框圖如圖1 所示[11-13]。

圖1 檢測器結(jié)構(gòu)及系統(tǒng)功能框圖

嵌入式系統(tǒng)將各測量裝置的檢測數(shù)據(jù)下載到存儲器中，待檢測器在管道中通過后，將檢測數(shù)據(jù)取出進(jìn)行離線分析解算。 因?yàn)楣艿纼?nèi)部只有慣性導(dǎo)航可以提供連續(xù)且全面的定位信息，所以采用慣性導(dǎo)航為主要定位算法，但是慣導(dǎo)具有誤差累積問題，需通過Kalman 濾波器對每步解算的誤差進(jìn)行估計和補(bǔ)償。 慣導(dǎo)坐標(biāo)系按如下規(guī)定設(shè)置:導(dǎo)航坐標(biāo)系的三維方向?yàn)?x-東向；y-北向；z-天向(n系)，在n系上進(jìn)行管道地理坐標(biāo)定位。 與載體固聯(lián)的三維坐標(biāo)系為載體坐標(biāo)系(b系)，方向?yàn)?y指向前進(jìn)方向，x水平指向右，z垂直xy平面指向上。 將b系三維坐標(biāo)軸方向與IMU 三個陀螺儀的旋轉(zhuǎn)軸及三個加速度計的敏感軸重合，在b系上測量檢測器運(yùn)動的慣性信息，即載體運(yùn)動的三維方向角速率和速度增量信息。

在管道入口和出口處需將檢測器靜置一段時間(2 min～3 min)，利用靜態(tài)對準(zhǔn)方法，即加速度計和地磁傳感器得到初始姿態(tài)信息，利用GPS 得到初始位置信息。

2 DR 解算原理及輔助定位方法

2.1 DR 解算原理

MEMS 型加速度計精度較低，若直接采用導(dǎo)航中的捷聯(lián)慣導(dǎo)算法(SINS)對加速度進(jìn)行一階積分，得到速度結(jié)果，進(jìn)行二階積分得到位移結(jié)果，在速度和位置更新環(huán)節(jié)的累加誤差通過時間積分被放大，精度迅速下降，對于精度較低的MEMS 型加速度計，這種問題更加嚴(yán)重。 因?yàn)楣艿纼?nèi)的檢測器只有前進(jìn)方向位移，又因里程增量可通過里程計直接測得，避免了SINS 速度更新積分通路的誤差累加現(xiàn)象，同時也可將Kalman 濾波方程的狀態(tài)噪聲誤差維數(shù)從6 個減少為4 個(見式(12))。

如圖1 所示，將里程輪轉(zhuǎn)動方向與b系的y軸正向一致，可測得b系下檢測器的速度:

vodo為里程計測得的里程增量。 里程輪用彈性元件張緊在管壁上，避免滑動造成的速度損失，同時保證檢測器在管道中良好的通過能力。

用里程增量代替捷聯(lián)慣導(dǎo)的速度更新即航跡推算(DR)算法，DR 原理如圖2 所示。

圖2 DR 算法原理圖

陀螺儀的采樣數(shù)據(jù)為載體旋轉(zhuǎn)的角速率觀測值，由于四元數(shù)計算比歐拉角形式的姿態(tài)矩陣更簡便，所以先利用四元數(shù)姿態(tài)陣對載體姿態(tài)進(jìn)行更新，再轉(zhuǎn)化為姿態(tài)矩陣形式。 將測量值轉(zhuǎn)換為四元數(shù)微分方程:

式中:ωt為陀螺儀角速率轉(zhuǎn)換而成的單位旋轉(zhuǎn)四元數(shù)。 圓圈符號代表四元數(shù)相乘，Qt為姿態(tài)更新的四元數(shù)。 在姿態(tài)更新的周期0 至T內(nèi)ωt近似恒定條件下，采用四階龍哥-庫塔法遞推求解此微分方程:

將更新后的四元數(shù)轉(zhuǎn)換為姿態(tài)矩陣，速度更新利用姿態(tài)矩陣求得n系下速度矢量:

位置更新對速度矢量進(jìn)行一階積分即得到采樣時間ts內(nèi)載體在n系的位移Rn:

將所有采樣時刻的位移進(jìn)行累加和排序，即得到載體在管道中運(yùn)動的軌跡信息，即管道的三維地理坐標(biāo)。

2.2 輔助定位方法

地磁傳感器可測得地理磁場在b系的投影值Bz，通過國家地理信息中心網(wǎng)站確定當(dāng)?shù)乜偟卮艌鲋礏，通過傾角傳感器確定仰俯角r，在初始位置確定相對磁導(dǎo)率u，根據(jù)式(10)確定偏航角:

由于地磁測量受環(huán)境干擾的影響較大，借鑒Davenport 的Q方法，對姿態(tài)解算結(jié)果進(jìn)行篩選，提高測量數(shù)據(jù)的穩(wěn)定性和可靠性[14]。

通過IMU 的加速度計測量值可以計算得到檢測器橫滾角θ和仰俯角r:

式(11)和式(12)將重力加速度g看做常量，但作為濾波校正信息，必須去除加速度計中混有的與檢測器運(yùn)動相關(guān)的分量。 文獻(xiàn)[15]利用條件判別方程(13)判定和去除這部分分量，只保留與重力加速度相關(guān)的測量分量:

式中:ξg為條件判別的閾值，通過實(shí)驗(yàn)來確定。 以上輔助方法都采用了修正算法，更新速率遠(yuǎn)小于DR 的姿態(tài)解算，但研究者考慮正常工作條件下，檢測器在管道內(nèi)的姿態(tài)角變化不是非常劇烈(彎管最小曲率半徑必須符合1.5 倍管道直徑，且檢測器管內(nèi)移動速率不超過2 m/s)，因此滿足管內(nèi)檢測的條件。

3 濾波校正原理

3.1 建立誤差的濾波模型方程

DR 解算根據(jù)前一時刻的結(jié)果進(jìn)行姿態(tài)和位置更新，誤差逐漸累積，必須加入校正環(huán)節(jié)，否則隨著時間的推移，累積誤差將使解算結(jié)果發(fā)散。 主要誤差來源中，安裝誤差，傳感器溫度漂移和尺度因數(shù)誤差屬于確定性誤差，可以通過加入誤差補(bǔ)償函數(shù)進(jìn)行校正；慣性數(shù)據(jù)的隨機(jī)游走，DR 算法的累積誤差和舍入誤差等屬于隨機(jī)性誤差，需要通過RTS 平滑去除，采用將累積誤差作為狀態(tài)量的間接校正方式，可以減小濾波解算的最大最小數(shù)值范圍，提高解算的效率與精度，工作原理如圖3 所示。

圖3 濾波校正算法原理圖

①建立濾波狀態(tài)方程

將每一步DR 更新的解算誤差作為估計的狀態(tài)量X:

式中:F(t)為狀態(tài)轉(zhuǎn)移函數(shù)，L(t)為噪聲轉(zhuǎn)移函數(shù)，W為狀態(tài)噪聲，包括三軸陀螺儀的測量誤差和里程計測量誤差:

②建立濾波觀測方程

觀測方程為離散形式，觀測值只決定于同一時刻的狀態(tài)值，與前后時刻互不相關(guān):

式中:Z為觀測值的矢量形式，Z＝[φx，φy，φz]，H為測量轉(zhuǎn)移矩陣，R為測量噪聲矢量。

因?yàn)閄，W和R同為高斯分布，所以其統(tǒng)計分布規(guī)律只由均值和方差所決定，簡化了模型方程的求解過程。 在平滑解算的反向遞推環(huán)節(jié)必須保證協(xié)方差陣的正定性，避免求逆過程出現(xiàn)奇異陣情況。

3.2 RTS 平滑濾波解算

系統(tǒng)待檢測器通過管道后將測量數(shù)據(jù)全部取出進(jìn)行離線解算，因?yàn)榭梢愿鶕?jù)所有測量值對每一時刻的狀態(tài)量進(jìn)行估計，采用固定區(qū)間平滑將比Kalman 濾波獲得更高的精度。 RTS 平滑包括前向?yàn)V波和后向遞推兩部分，前向?yàn)V波即Kalman 濾波，需要計算和存儲每一時刻估計的狀態(tài)量和協(xié)方差陣；反向遞推利用存儲數(shù)據(jù)進(jìn)行后向遞推平滑，獲得最優(yōu)平滑結(jié)果，算法框圖如圖4 所示[16-18]。

圖4 RTS 算法框圖

①正向?yàn)V波:

(a)將狀態(tài)方程離散化，得到狀態(tài)轉(zhuǎn)移方程(k從0 至終止點(diǎn)N):

式中:A表示離散化之后的轉(zhuǎn)移矩陣，P表示狀態(tài)方差。 設(shè)定初始狀態(tài)值m0和初始噪聲方差陣Q0，進(jìn)行遞推解算的預(yù)測過程:

(b)觀測方程可以直接轉(zhuǎn)為狀態(tài)轉(zhuǎn)移方程，進(jìn)行遞推解算的更新過程:

K為正向?yàn)V波的增益。

②反向遞推

當(dāng)正向?yàn)V波結(jié)束，利用存儲的mk和Pk進(jìn)行逆時方向的遞推解算:

G為平滑增益。 遞推中，通過LU分解避免P-k＋1出現(xiàn)奇異陣情況，通過式(24)得到平滑的最優(yōu)估計結(jié)果Psk。

3.3 改進(jìn)的平滑濾波算法

因?yàn)闄z測器進(jìn)出管道時都可采用靜態(tài)的初始對準(zhǔn)方法，可利用時間逆向的雙向解算濾波平滑進(jìn)一步提高定位精度。 按式(27)進(jìn)行慣導(dǎo)解算姿態(tài)更新的時間逆向化處理:

式中:角標(biāo)“-”表示時間遞推的方向由后往前進(jìn)行，速度和位置更新與其類似。 傳統(tǒng)RTS 算法只依賴于初始運(yùn)動時刻的靜態(tài)對準(zhǔn)精度，但實(shí)際上在檢測器終止運(yùn)動時刻仍然可以進(jìn)行靜態(tài)對準(zhǔn)，利用式(27)方法進(jìn)行時間逆向化求解，可以得到逆向化解算結(jié)果，逆向化解算依賴于終止點(diǎn)對準(zhǔn)的精度。

時間正向解算過程與RTS 的正向遞推相同。因?yàn)樾Ｕ闹饕獮殡S機(jī)誤差，所以采用“凸組合原則”對雙向結(jié)果進(jìn)行融合，估計的狀態(tài)量為:

根據(jù)凸組合原理，此時P0為最小方差。 改進(jìn)的平滑算法(RTS-FC)與RTS 對反向平滑的解算方法不同，其將終止點(diǎn)校正信息加入濾波算法，所以可獲得比RTS 更高的精度。

4 實(shí)驗(yàn)驗(yàn)證和結(jié)果分析

為證明研究的誤差校正方法有效性，搭建了實(shí)驗(yàn)檢測裝置進(jìn)行模擬實(shí)驗(yàn)，將除地磁傳感器外的測量系統(tǒng)封閉在檢測器內(nèi)，各傳感器性能指標(biāo)如表1所示，檢測器及IMU 外觀如圖5 所示。

表1 各傳感器性能指標(biāo)

圖5 檢測器、數(shù)據(jù)記錄板卡及IMU

通過登錄國家地理信息系統(tǒng)，查找到管道起點(diǎn)的總地磁場強(qiáng)度B，水平磁場強(qiáng)度Bz，與大地水準(zhǔn)面仰俯角r，偏航角ψ0，如表2 所示。

表2 實(shí)驗(yàn)地點(diǎn)的地磁場信息

通過式(31)求得管內(nèi)相對磁導(dǎo)率:

為了在實(shí)驗(yàn)條件下模擬實(shí)際檢測過程，采用管外拖曳的方式實(shí)現(xiàn)檢測器在管道內(nèi)的運(yùn)動，在拖曳繩上安裝編碼器作為里程測量裝置，記錄檢測器的里程增量。 實(shí)驗(yàn)管道長度180 m，管道內(nèi)徑0.1 m。檢測器以大約1.5 m/s 在管內(nèi)勻速前進(jìn)，采用TI 的OMAP-L138 對傳感器進(jìn)行統(tǒng)一采樣，避免由于量測時間不同步造成的計算誤差。 在測量前還需對IMU 與檢測器的安裝誤差進(jìn)行校正[19]。 根據(jù)原始測量信號的統(tǒng)計分布特性及實(shí)驗(yàn)觀察，設(shè)定初始狀態(tài)值為:

初始狀態(tài)噪聲值為:

將檢測器在管道內(nèi)通過的同時，記錄下所有的測量數(shù)據(jù)，再進(jìn)行離線分析解算。 實(shí)驗(yàn)結(jié)果如圖6所示，其中RTS-FC 結(jié)果用菱形加實(shí)線表示，RTS 用叉號加實(shí)線表示，KF 用圓形加實(shí)線表示。

為方便分析，分別計算了KF，RTS 平滑和RTSFC 在東向，北向和天向的位置誤差，如圖7～圖9 所示，其中RTS-FC 誤差為實(shí)線表示，RTS 誤差用點(diǎn)劃線表示，KF 誤差用虛線表示。 通過示意圖可以看出，在相同條件下，RTS-FC 在東向和天向的誤差最小，北向的最大誤差為7.9 m，實(shí)驗(yàn)表明，RTS-FC 相較KF 算法和RTS 平滑算法，對誤差具有較好的校正效果。

圖6 實(shí)驗(yàn)管道的地理坐標(biāo)結(jié)果

圖7 東向位置誤差

圖8 北向位置誤差

圖9 天向位置誤差

5 結(jié)論

采用MEMS 慣性器件對小口徑埋地管道進(jìn)行管內(nèi)地理坐標(biāo)定位具有方便，成本小，污染小，可實(shí)現(xiàn)定期檢測等優(yōu)點(diǎn)，但是受器件精度的影響，難以滿足實(shí)用的需要。 論文將原來的捷聯(lián)慣導(dǎo)解算改為航跡推算形式，降低了濾波誤差的維數(shù)，利用里程計的速度測量值代替加速度測量值，降低了測量誤差對定位的影響；新引入管內(nèi)可測的地磁信息和重力信息作為測量數(shù)據(jù)，利用其對DR 解散的姿態(tài)角誤差進(jìn)行估計和補(bǔ)償；針對系統(tǒng)離線解算的特點(diǎn)，建立RTS 平滑濾波算法對DR 解算的累積誤差進(jìn)行估計和校正；利用起點(diǎn)和終點(diǎn)位置信息可測的特點(diǎn)，采用RTS-FC 進(jìn)一步提高定位精度。 模擬實(shí)際檢測環(huán)境進(jìn)行了實(shí)驗(yàn)，結(jié)果表明，應(yīng)用RTS-FC 檢測180 m 管道，最大誤差為8 m，優(yōu)于KF 和RTS 平滑算法。