楊 洋李 賓袁 泉楊理踐谷 碩
(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)行了分析和討論。
檢測器由可以在管道內(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 得到初始位置信息。
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)。
地磁傳感器可測得地理磁場在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)檢測的條件。
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)奇異陣情況。
系統(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。
因?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)樾U闹饕獮殡S機(jī)誤差,所以采用“凸組合原則”對雙向結(jié)果進(jìn)行融合,估計的狀態(tài)量為:
根據(jù)凸組合原理,此時P0為最小方差。 改進(jìn)的平滑算法(RTS-FC)與RTS 對反向平滑的解算方法不同,其將終止點(diǎn)校正信息加入濾波算法,所以可獲得比RTS 更高的精度。
為證明研究的誤差校正方法有效性,搭建了實(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 天向位置誤差
采用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 平滑算法。