MEMS 慣導(dǎo)管內(nèi)坐標(biāo)測(cè)量的改進(jìn)RTS 平滑濾波算法?

楊 洋李 賓袁 泉楊理踐谷 碩

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

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

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

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

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

1.1 檢測(cè)裝置工作原理

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

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

嵌入式系統(tǒng)將各測(cè)量裝置的檢測(cè)數(shù)據(jù)下載到存儲(chǔ)器中，待檢測(cè)器在管道中通過(guò)后，將檢測(cè)數(shù)據(jù)取出進(jìn)行離線分析解算。 因?yàn)楣艿纼?nèi)部只有慣性導(dǎo)航可以提供連續(xù)且全面的定位信息，所以采用慣性導(dǎo)航為主要定位算法，但是慣導(dǎo)具有誤差累積問題，需通過(guò)Kalman 濾波器對(duì)每步解算的誤差進(jìn)行估計(jì)和補(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 三個(gè)陀螺儀的旋轉(zhuǎn)軸及三個(gè)加速度計(jì)的敏感軸重合，在b系上測(cè)量檢測(cè)器運(yùn)動(dòng)的慣性信息，即載體運(yùn)動(dòng)的三維方向角速率和速度增量信息。

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

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

2.1 DR 解算原理

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

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

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

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

圖2 DR 算法原理圖

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

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

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

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

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

2.2 輔助定位方法

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

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

通過(guò)IMU 的加速度計(jì)測(cè)量值可以計(jì)算得到檢測(cè)器橫滾角θ和仰俯角r:

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

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

3 濾波校正原理

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

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

圖3 濾波校正算法原理圖

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

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

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

②建立濾波觀測(cè)方程

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

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

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

3.2 RTS 平滑濾波解算

系統(tǒng)待檢測(cè)器通過(guò)管道后將測(cè)量數(shù)據(jù)全部取出進(jìn)行離線解算，因?yàn)榭梢愿鶕?jù)所有測(cè)量值對(duì)每一時(shí)刻的狀態(tài)量進(jìn)行估計(jì)，采用固定區(qū)間平滑將比Kalman 濾波獲得更高的精度。 RTS 平滑包括前向?yàn)V波和后向遞推兩部分，前向?yàn)V波即Kalman 濾波，需要計(jì)算和存儲(chǔ)每一時(shí)刻估計(jì)的狀態(tài)量和協(xié)方差陣；反向遞推利用存儲(chǔ)數(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ù)測(cè)過(guò)程:

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

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

②反向遞推

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖7 東向位置誤差

圖8 北向位置誤差

圖9 天向位置誤差

5 結(jié)論

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