基于聯(lián)邦濾波的多傳感器管道定位方法

張 強，吳 磊，楊 迪，李 倩

(哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院，哈爾濱 150001)

0 引言

地下管道是城市基礎(chǔ)設(shè)施的重要組成部分，承擔(dān)著城市信息、能源、水暖資源流通和廢物排棄等重要功能。由于城市化的不斷發(fā)展，地下管道數(shù)量日益增多，管道分布越來越復(fù)雜，這給地下管道使用、維護(hù)以及城市地下工程開展帶來了難度。因此，有必要針對城市地下管道建立精確的三維地圖，以防止開挖和施工等工程對管道造成損壞。

目前，常用的非破壞性管道定位技術(shù)，包括探地雷達(dá)定位法、電磁定位法以及磁強計定位法都具有局限性，其定位性能受限于管道材料、內(nèi)容物、管道直徑和深度以及上覆土壤的性質(zhì)等各種因素。與上述技術(shù)不同，慣性定位技術(shù)在管道內(nèi)部運行過程中，利用慣性測量裝置采集其運行角速度與加速度信息，并通過捷聯(lián)慣性解算實現(xiàn)對管道的有效定位。由于慣性定位方法不受管道材質(zhì)以及外界環(huán)境影響，屬于一種完全自主式定位方法，因此近年來在管道定位領(lǐng)域中占據(jù)主要地位。但是，捷聯(lián)慣性解算存在累積式的原理性誤差，通常需要必要的外部量測信息對慣性解算累積誤差進(jìn)行校正，從而保證管道定位誤差保持在可接受范圍內(nèi)。在管道定位過程中，通常采用里程輪測速信息作為輔助信息對慣性解算誤差進(jìn)行校正。除此以外，管道上的已知位置點可以對管道定位儀進(jìn)行位置更新，進(jìn)一步抑制慣性解算誤差發(fā)散。非完整性約束是指管道定位儀由于受到管道的限制，在管道內(nèi)只能前后運動，垂直于縱向平面內(nèi)的速度為零，可以提供載體坐標(biāo)系下的速度輔助信息。楊陽等提出了一種改進(jìn)的容積卡爾曼濾波(Cubature Kalman Filter，CKF)算法，引入地磁和重力信息作為濾波器的量測矢量，并使用陀螺測量值自適應(yīng)地調(diào)節(jié)CKF過程噪聲陣以保持濾波器穩(wěn)定。楊理踐等提出了以里程輪速度為量測量的正向容積卡爾曼濾波算法和以基準(zhǔn)點位置為起點的反向平滑兩級濾波，實現(xiàn)了對管道缺陷地理坐標(biāo)的最優(yōu)估計。同時，由于管道起點和終點的位置已知，可以對管道定位儀采集數(shù)據(jù)分別進(jìn)行正向和逆向定位解算，然后分別將正向解算的前二分之一數(shù)據(jù)和逆向解算的前二分之一數(shù)據(jù)作為系統(tǒng)的定位結(jié)果，提高了系統(tǒng)的定位精度。

通過以上文獻(xiàn)可以發(fā)現(xiàn)，現(xiàn)有管道定位方案多以慣導(dǎo)/里程輪組合構(gòu)建定位系統(tǒng)，同時輔以終點校正或反向平滑，從而進(jìn)一步提高定位精度。但管道定位儀工作過程中，由于里程輪通過與管壁緊密接觸進(jìn)行測速，管道內(nèi)壁不平整、管道焊縫等容易使里程輪與管壁脫離接觸，出現(xiàn)打滑。當(dāng)里程輪打滑時，輸出的里程輪速度信息作為量測信息會導(dǎo)致定位誤差增加。為了解決這個問題，D. Hyun和H. S. Yang等將一種改進(jìn)的擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)用于管道定位，并使用光電測速傳感器作為管道定位儀的速度量測。光電測速傳感器屬于無接觸測速傳感器，不存在打滑現(xiàn)象，但其對傳感器與管壁間的測速距離要求嚴(yán)格，實際使用過程中測速信息不穩(wěn)定。為融合不同特點傳感器優(yōu)勢，本文利用聯(lián)邦卡爾曼濾波(Federated Kalman Filter, FKF)對微慣性測量單元(Micro Inertial Measurement Unit，MIMU)/里程輪/光電測速傳感器信息進(jìn)行融合，并基于殘差預(yù)測設(shè)計了自適應(yīng)信息分配因子，以減弱子系統(tǒng)發(fā)生故障對主濾波精度的影響，從而提高整體定位精度。

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

1.1 管道定位系統(tǒng)組成及工作原理

管道定位系統(tǒng)由低成本MIMU、里程輪、光電測速傳感器、微處理器以及存儲卡組成，如圖1所示。在定位過程中，微處理器將加速度計、陀螺儀信息、光電測速傳感器以及里程輪信息存儲到存儲卡中。首先，對陀螺儀與加速度信息進(jìn)行捷聯(lián)慣性解算，得到慣性解算輸出的姿態(tài)、速度和位置；然后，利用慣性解算的姿態(tài)信息將里程輪測速信息與光電測速傳感器信息投影到導(dǎo)航坐標(biāo)系()；最后，將慣性解算信息、里程輪解算速度與光電測速傳感器解算速度用聯(lián)邦濾波進(jìn)行最優(yōu)融合，得到系統(tǒng)定位結(jié)果。本文導(dǎo)航坐標(biāo)系取地理坐標(biāo)系。

圖1 管道定位系統(tǒng)組成Fig.1 Composition of pipeline positioning system

1.2 系統(tǒng)初始化

慣導(dǎo)系統(tǒng)初始對準(zhǔn)是確定參考坐標(biāo)系的一個過程。慣導(dǎo)系統(tǒng)剛上電啟動時，其載體坐標(biāo)系()相對參考坐標(biāo)系的各軸指向完全未知或不夠準(zhǔn)確，無法進(jìn)入導(dǎo)航狀態(tài)，因此必須確定載體系相對導(dǎo)航坐標(biāo)系的空間方位。MIMU初始對準(zhǔn)是在管道定位系統(tǒng)測量前靜置于管口位置，已知當(dāng)?shù)刂亓铀俣群凸芫€起點經(jīng)緯度，利用靜置期間的MIMU加速度計輸出信息完成水平姿態(tài)對準(zhǔn)，靜止?fàn)顟B(tài)下初始俯仰角和橫滾角與加速度計輸出近似有如下關(guān)系

(1)

(2)

=sin()

(3)

=tan(-)

(4)

(5)

簡化得到

(6)

所以

∈(-π,π)

(7)

其中，為載體縱軸相對于磁北的航向角，包括當(dāng)?shù)氐拇牌?。所以，實際的航向角應(yīng)減去磁偏角，即

=-

(8)

2 基于聯(lián)邦濾波的管道定位算法

2.1 聯(lián)邦濾波器結(jié)構(gòu)

本文采取聯(lián)邦濾波器對MIMU、里程輪和光電測速傳感器進(jìn)行信息融合，實現(xiàn)對慣導(dǎo)誤差狀態(tài)的最優(yōu)估計，濾波器結(jié)構(gòu)如圖2所示。

圖2 聯(lián)邦濾波器結(jié)構(gòu)Fig.2 Structure of federated filter

(9)

其中，=1,2為濾波器的下標(biāo);,(0≤,≤1)表示第個子濾波器的信息分配系數(shù)，并滿足式(10)所示的信息守恒原則

(10)

在主濾波器融合階段，主濾波器將各子濾波器輸出的狀態(tài)估計信息按式(11)進(jìn)行最優(yōu)融合。融合方程為

(11)

最終獲得全局最優(yōu)解。

2.2 聯(lián)邦子濾波器

狀態(tài)變量為

=[δδδδδδ

(12)

狀態(tài)方程為

(13)

其中，為系統(tǒng)過程噪聲輸入矩陣；為系統(tǒng)噪聲陣；為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,均可根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)誤差方程構(gòu)造。

(1)MIMU/里程輪子濾波器量測方程

在MIMU/里程輪子系統(tǒng)中，量測信息由慣性導(dǎo)航系統(tǒng)解算得到的速度信息與里程輪得到的速度信息在導(dǎo)航坐標(biāo)系上的投影做差得到，即

(14)

(15)

(16)

量測方程可以進(jìn)一步表示為

(17)

量測矩陣可以用式(18)描述

(18)

(2)MIMU/光電測速傳感器子濾波器量測方程

在該子系統(tǒng)中，量測信息由慣性導(dǎo)航系統(tǒng)解算得到的速度信息與光電測速傳感器得到的速度信息在導(dǎo)航坐標(biāo)系上的投影做差得到，即

(19)

(20)

下面簡單給出卡爾曼濾波具體流程：

式(13)、式(14)和式(19)的等效離散化模型為

(21)

-1=+()Δ

(22)

-1=()Δ

(23)

其中，-1表示系統(tǒng)從-1時刻到時刻的狀態(tài)轉(zhuǎn)移矩陣；為系統(tǒng)狀態(tài)向量；-1為系統(tǒng)噪聲輸入矩陣；-1為系統(tǒng)噪聲向量；為系統(tǒng)量測向量；為系統(tǒng)量測矩陣；為系統(tǒng)量測噪聲向量。

卡爾曼濾波包括時間更新和量測更新2個階段?？柭鼮V波時間更新階段

(24)

(25)

濾波量測更新階段

(26)

(27)

(28)

其中，是濾波增益；是量測噪聲方差陣。

2.3 基于預(yù)測殘差的自適應(yīng)信息分配因子

在FKF中，信息分配因子對聯(lián)邦濾波器的估計精度有很大影響。

根據(jù)式(9)可知

(29)

(30)

因此，第個子濾波器的一步預(yù)測誤差協(xié)方差矩陣可以寫為

(31)

增益矩陣可以寫為

(32)

子濾波器在獲得光電測速傳感器和里程輪量測信息后，進(jìn)行量測更新，可以得到

(33)

(34)

將式(34)代入式(11)可得

(35)

各子濾波器狀態(tài)信息所占比例與信息分配系數(shù)成正比。各子濾波器的信息分配因子大小體現(xiàn)了子濾波器結(jié)果對主濾波器的貢獻(xiàn)，應(yīng)與其濾波精度成正比。

當(dāng)子系統(tǒng)量測信息含有故障信息時，在主濾波器信息融合時需減小該子系統(tǒng)貢獻(xiàn)。

(36)

由于預(yù)測殘差向量,可以反映系統(tǒng)運動學(xué)模型誤差和量測信息質(zhì)量，因此可以根據(jù)預(yù)測殘差構(gòu)造自適應(yīng)信息分配因子。當(dāng)系統(tǒng)模型準(zhǔn)確，量測信息可靠時，,滿足正態(tài)分布，即

(37)

根據(jù),的分布特性，構(gòu)造新統(tǒng)計量

(38)

因此，可以利用式(39)對自適應(yīng)信息分配因子進(jìn)行構(gòu)造。

(39)

其中，是常數(shù)，一般根據(jù)工程經(jīng)驗選取，這里取2.7。

由于,反映了時刻運動學(xué)模型擾動情況，,可用于確定此刻子濾波器狀態(tài)。當(dāng)|,|≤時，表明子系統(tǒng)狀態(tài)模型準(zhǔn)確且量測信息有效，子濾波器性能良好；當(dāng)|,,|>時，表明量測信息可能含有故障信息。

為了滿足信息守恒原則，對信息分配因子進(jìn)行歸一化處理

(40)

以MIMU/里程輪子系統(tǒng)為例，當(dāng)里程輪測速正常時，殘差,為零均值白噪聲;當(dāng)里程輪出現(xiàn)打滑時，MIMU/里程輪子系統(tǒng)量測信息含有野值，殘差,均值不為零，相應(yīng)的信息分配因子變小，從而減小了該子系統(tǒng)故障對主濾波器的影響。

2.4 正逆向加權(quán)校正

管道定位儀實際工作時，可以由實時動態(tài)載波相位差分(Real-Time Kinematic，RTK)技術(shù)和全站儀配合確定管道起終兩點坐標(biāo)，然后對同一管道進(jìn)行多次往返測量。在實際定位過程中，為便于對多次解算的軌跡數(shù)據(jù)進(jìn)行加權(quán)平均處理，需要多次測量得到的管道軌跡數(shù)據(jù)長度盡量一致，因此管道定位儀通過地下管道應(yīng)盡量保持勻速，且往返測量時也應(yīng)保持速度一致。

對管道的正逆向解算結(jié)果，最簡單的辦法是將兩段軌跡數(shù)據(jù)取均值，從而得到最終定位結(jié)果。但是，這種簡單取平均值的數(shù)據(jù)處理方法忽略了慣性定位誤差隨時間增長而逐漸增大的特性，即隨著離起始點距離的增大，定位誤差逐漸增大。因此，正逆向解算軌跡數(shù)據(jù)應(yīng)該根據(jù)其與起始點距離而進(jìn)行加權(quán)融合。根據(jù)以上原理，將正逆向解算數(shù)據(jù)加權(quán)平均處理方法表示為

Avg()=((-)-+)

(41)

其中，表示測量管線軌跡全長；為距離管線終點的長度；-為距離起點為-長度的正向解算軌跡數(shù)據(jù)；為距離管線終點的長度為的逆向解算軌跡數(shù)據(jù)。

通過正逆向加權(quán)校正，可以進(jìn)一步提高管道定位精度。

3 仿真實驗結(jié)果與分析

3.1 仿真管道軌跡及仿真參數(shù)

針對城市地下管道存在的直管道、彎管道和深管道軌跡進(jìn)行模擬，如圖3所示。模擬地下管道軌跡長120m，管道定位儀在管道中運行速度為0.4m/s。同時模擬里程輪和光電測速傳感器不同測速誤差特性，里程輪正常工作時，測速穩(wěn)定，但存在打滑現(xiàn)象，在管道定位儀運行100s和200s處分別出現(xiàn)持續(xù)10s的打滑；光電測速傳感器不存在打滑問題，但測速不穩(wěn)定，輸出速度信息噪聲大，如圖4所示。

圖3 管道仿真軌跡Fig.3 Pipeline simulation track

圖4 里程輪和光電測速傳感器隨時間輸出的速度Fig.4 Output speed of odometer and optical speed sensor with time

實驗中設(shè)置各傳感器技術(shù)指標(biāo)如表1所示。

表1 傳感器參數(shù)

3.2 實驗結(jié)果與分析

仿真實驗中，管道定位儀對管道進(jìn)行測量前，先靜止10s，利用這10s的數(shù)據(jù)完成系統(tǒng)初始對準(zhǔn)。然后對管道定位儀數(shù)據(jù)使用不同的定位算法進(jìn)行解算，包括MIMU/里程輪組合卡爾曼濾波、MIMU/里程輪/光電測速傳感器組合聯(lián)邦濾波以及在聯(lián)邦濾波基礎(chǔ)上的正逆向校正算法。圖5、圖6和圖7分別比較了由不同定位算法得到的管線位置在東向、北向和天向上的誤差。表2列出了不同定位算法的定位精度比較。

圖5 不同定位方法東向位置誤差Fig.5 East position error of different positioning methods

圖6 不同定位方法北向位置誤差Fig.6 North position error of different positioning methods

圖7 不同定位方法天向位置誤差Fig.7 Up position error of different positioning methods

表2 不同定位方法管道定位精度比較

根據(jù)表2可知，采用MIMU/里程輪組合定位方法，當(dāng)里程輪出現(xiàn)打滑時，東向、北向和天向最大位置誤差分別達(dá)到0.18m、0.12m和0.19m，均方根誤差分別為0.09m、0.08m和0.10m；采用MIMU/里程輪/光電測速傳感器組合定位方法，管道東向、北向、天向定位均方根誤差分別為0.03m、0.01m和0.03m；此外，在MIMU/里程輪/光電測速傳感器組合定位結(jié)果的基礎(chǔ)上采取正逆向加權(quán)校正，管道東向、北向、天向定位均方根誤差分別減小到0.01m、0.01m和0.01m。由仿真實驗結(jié)果可以看出，在管道定位過程中，里程輪打滑會導(dǎo)致MIMU/里程輪組合定位誤差過大，尤其天向位置誤差。在基于聯(lián)邦濾波的MIMU/里程輪/光電測速傳感器組合定位系統(tǒng)中，由于在里程輪打滑時控制其子濾波器貢獻(xiàn)率的信息分配因子會自適應(yīng)減小，從而可以有效抑制里程輪打滑造成的定位誤差。除此之外，正逆向加權(quán)校正算法可以進(jìn)一步減小管道定位誤差，是管道定位系統(tǒng)中常用的數(shù)據(jù)處理方法。

4 結(jié)論

本文介紹了一種基于低成本MIMU/里程輪/光電測速傳感器組合的管道定位方法。

1)針對管道定位儀定位過程中里程輪打滑、量測信息失效、慣性誤差累積導(dǎo)致定位誤差增大的問題，利用光電測速傳感器無接觸測速不存在打滑的特點，使其與里程輪實現(xiàn)測速互補，降低了里程輪打滑引起的定位誤差。

2)針對里程輪和光電測速傳感器測速特性不同的問題,利用預(yù)測殘差對聯(lián)邦濾波器中信息分配因子進(jìn)行自適應(yīng)調(diào)整，通過實時調(diào)整子濾波器對主濾波器貢獻(xiàn)大小，有效減小了子系統(tǒng)故障對主濾波器精度的影響，從而提高了定位精度。