基于聯邦濾波的多傳感器管道定位方法

張 強，吳 磊，楊 迪，李 倩

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

0 引言

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

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

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

1 管道定位系統

1.1 管道定位系統組成及工作原理

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

圖1 管道定位系統組成Fig.1 Composition of pipeline positioning system

1.2 系統初始化

慣導系統初始對準是確定參考坐標系的一個過程。慣導系統剛上電啟動時，其載體坐標系()相對參考坐標系的各軸指向完全未知或不夠準確，無法進入導航狀態，因此必須確定載體系相對導航坐標系的空間方位。MIMU初始對準是在管道定位系統測量前靜置于管口位置，已知當地重力加速度和管線起點經緯度，利用靜置期間的MIMU加速度計輸出信息完成水平姿態對準，靜止狀態下初始俯仰角和橫滾角與加速度計輸出近似有如下關系

(1)

(2)

=sin()

(3)

=tan(-)

(4)

(5)

簡化得到

(6)

所以

∈(-π,π)

(7)

其中，為載體縱軸相對于磁北的航向角，包括當地的磁偏角。所以，實際的航向角應減去磁偏角，即

=-

(8)

2 基于聯邦濾波的管道定位算法

2.1 聯邦濾波器結構

本文采取聯邦濾波器對MIMU、里程輪和光電測速傳感器進行信息融合，實現對慣導誤差狀態的最優估計，濾波器結構如圖2所示。

圖2 聯邦濾波器結構Fig.2 Structure of federated filter

(9)

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

(10)

在主濾波器融合階段，主濾波器將各子濾波器輸出的狀態估計信息按式(11)進行最優融合。融合方程為

(11)

最終獲得全局最優解。

2.2 聯邦子濾波器

狀態變量為

=[δδδδδδ

(12)

狀態方程為

(13)

其中，為系統過程噪聲輸入矩陣；為系統噪聲陣；為系統狀態轉移矩陣,均可根據捷聯慣性導航系統誤差方程構造。

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

在MIMU/里程輪子系統中，量測信息由慣性導航系統解算得到的速度信息與里程輪得到的速度信息在導航坐標系上的投影做差得到，即

(14)

(15)

(16)

量測方程可以進一步表示為

(17)

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

(18)

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

在該子系統中，量測信息由慣性導航系統解算得到的速度信息與光電測速傳感器得到的速度信息在導航坐標系上的投影做差得到，即

(19)

(20)

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

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

(21)

-1=+()Δ

(22)

-1=()Δ

(23)

其中，-1表示系統從-1時刻到時刻的狀態轉移矩陣；為系統狀態向量；-1為系統噪聲輸入矩陣；-1為系統噪聲向量；為系統量測向量；為系統量測矩陣；為系統量測噪聲向量。

卡爾曼濾波包括時間更新和量測更新2個階段。卡爾曼濾波時間更新階段

(24)

(25)

濾波量測更新階段

(26)

(27)

(28)

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

2.3 基于預測殘差的自適應信息分配因子

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

根據式(9)可知

(29)

(30)

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

(31)

增益矩陣可以寫為

(32)

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

(33)

(34)

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

(35)

各子濾波器狀態信息所占比例與信息分配系數成正比。各子濾波器的信息分配因子大小體現了子濾波器結果對主濾波器的貢獻，應與其濾波精度成正比。

當子系統量測信息含有故障信息時，在主濾波器信息融合時需減小該子系統貢獻。

(36)

由于預測殘差向量,可以反映系統運動學模型誤差和量測信息質量，因此可以根據預測殘差構造自適應信息分配因子。當系統模型準確，量測信息可靠時，,滿足正態分布，即

(37)

根據,的分布特性，構造新統計量

(38)

因此，可以利用式(39)對自適應信息分配因子進行構造。

(39)

其中，是常數，一般根據工程經驗選取，這里取2.7。

由于,反映了時刻運動學模型擾動情況，,可用于確定此刻子濾波器狀態。當|,|≤時，表明子系統狀態模型準確且量測信息有效，子濾波器性能良好；當|,,|>時，表明量測信息可能含有故障信息。

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

(40)

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

2.4 正逆向加權校正

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

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

Avg()=((-)-+)

(41)

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

通過正逆向加權校正，可以進一步提高管道定位精度。

3 仿真實驗結果與分析

3.1 仿真管道軌跡及仿真參數

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

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

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

實驗中設置各傳感器技術指標如表1所示。

表1 傳感器參數

3.2 實驗結果與分析

仿真實驗中，管道定位儀對管道進行測量前，先靜止10s，利用這10s的數據完成系統初始對準。然后對管道定位儀數據使用不同的定位算法進行解算，包括MIMU/里程輪組合卡爾曼濾波、MIMU/里程輪/光電測速傳感器組合聯邦濾波以及在聯邦濾波基礎上的正逆向校正算法。圖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 不同定位方法管道定位精度比較

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

4 結論

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

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

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