基于聯邦濾波的室內多傳感器融合導航定位方法研究

張 逵， 郭 杭， 敖龍輝

(南昌大學 信息工程學院，江西 南昌 330031)

近些年來，室內導航定位技術受到了廣泛的關注，服務機器人已經成為機器人未來發展的重要方向，高精度的室內多傳感器融合導航定位技術有著廣闊的市場應用前景。目前，應用比較普遍的室內定位方法包括WiFi定位、藍牙定位、LiDAR定位等[1-3]。然而這些方法受外部環境特征影響很大，例如LiDAR定位需要具有明顯的外部環境特征，當外部環境特征不夠明顯時，便很難提供高精度的定位結果[4-5]。

針對單一的定位導航方法精度、魯棒性、容錯性不高的問題，也有許多研究者提出大量的融合方法，包括：IMU/激光雷達(LiDAR)、IMU/GPS、視覺SLAM/慣導等[6-8]。總的來說，融合導航定位技術能有效提高定位效果，但這些方法多采用集中式濾波，當系統局部出現異常時，濾波效果會受到很大影響。聯邦濾波是一種分散式濾波，由多個子級濾波器組成。在部分子級濾波器異常時，系統仍能輸出可靠的定位結果[9-12]。

不同的環境下，傳感器的穩定性和可靠性會有很大的不同，甚至會導致定位結果失敗[13-15]。因此，本文設計了一種基于聯邦濾波的INS/3D LASER/單目視覺組合導航定位方法，利用分散式濾波的方式將IMU、3D LASER和單目視覺的信息進行分級融合處理。本文設計了混合聯邦濾波器，通過最小二乘的思路解決了系統自適應問題，在濾波過程中使用最小方差準則和最優系數加權法處理數據，進而得到系統全局最優估計結果。

1 數據融合方法

聯邦濾波能夠對需要融合的信息進行塊估計，然后分兩步進行級聯分散濾波。濾波系統由一個主濾波器和多個子級濾波器構成，主級濾波器和子級濾波器能夠同時進行數據處理。由于慣性導航部分能夠穩定地輸出測量結果，故本文采用慣性導航與視覺SLAM的融合濾波器和慣性導航與激光SLAM的融合濾波器獨立并行進行濾波測量更新，然后將各自融合處理的數據傳輸到主濾波器中進行全局最優融合處理。該并行分散的方法能夠有效降低系統的融合處理計算量，同時也能夠避免因某個子級濾波器出現處理錯誤而產生的錯誤信息，極大地提高了容錯能力。本文的融合算法流程如圖1所示。

圖1 濾波算法流程圖

首先構建慣性導航系統(INS)作為公共參考系統，其狀態表達為

XINS=[δLIE,δλIN,δvIE,δvIN,φIE,φIN,φIU,▽E,▽N,εE,εN,εU]

(1)

式中，δLIE,δλIN為緯度、經度誤差；δvIE，δvIN為東向、北向速度誤差；φIE，φIN，φIU為平臺航向角誤差、橫滾角誤差和俯仰角誤差;▽E，▽N為東向加速度計零偏和北向加速度計零偏；εE、εN、εU為東向陀螺儀漂移、北向陀螺儀漂移、方位陀螺儀漂移。

1.1 INS與3D LASER濾波

3D LASER技術是通過發射激光來獲取物體表面的空間三維信息及物理信息，有著準確、快速和非接觸性的特點。3D LASER狀態變量為

XLiDAR=[δLL,δλL,δvEL,δvNL,δψL,δθL,δγL]

(2)

式中，δLL，δλL為3D LASER輸出的緯度誤差和經度誤差；δvEL，δvNL為東向速度誤差和北向速度誤差；δψL為航向角誤差;δθL為俯仰角誤差;δγL為橫滾角誤差。假設噪聲均值為0，且為隨機過程，則INS/3D LASER組合導航系統狀態方程為

(3)

W=[WδLI,WδλI,WδvEI,WδvNI,WδψI,WδθI,WδγI,0,0,0,0,0,WδLL,WδλL,WδvEL,WδvNL,WδψL,WδθL,WδγL]T

(4)

式中，W為均值為零方差為Q的白噪聲；FINS為12×12矩陣;FLiDAR為7×7矩陣;

(5)

其中,相關時間τLL,τλL,τvEL,τvNL,τψL,τθL,τγL在100～200 s之間選取。假設位置誤差、速度誤差、姿態誤差都是一階馬爾科夫過程。由于傳統3D LASER模塊無法實時輸出高精度的姿態信息，一般的INS/3D LASER 組合系統僅能夠采用速度、位置組合方式。隨著3D LASER技術的發展，現在，高精度的姿態信息獲取不是難事，本文將姿態與速度、位置三者信息進行組合。INS/3D LASER組合的量測方程為

(6)

式中，HLiDAR=-17×7,

(7)

量測噪聲V是方差為R、均值為0的高斯白噪聲，且V與W互不相關。

1.2 INS與單目視覺組合濾波

INS/單目視覺組合系統采用速度組合模式，將INS和單目視覺解算得出的載體速度之差作為觀測量。選取狀態量為

Xk2=[δLk2,δλk2,δvNk2,δvEk2,δφNk2,δφEk2,δφDk2,δk]T

(8)

式中,δLk2，δλk2為載體緯度誤差、經度誤差;δvNk2，δvEk2為載體北向和東向速度;δφNk2，δφEk2和δφDk2分別為載體北向姿態角度誤差、東向姿態角度誤差和天向姿態角度誤差;δk為視覺相機標度因數誤差。對于δk，本文用隨機常數代替了常值誤差，而隨機誤差用一階馬爾科夫過程表示，反時間相關常數設為1/τc。系統狀態方程為

(9)

(10)

觀測矩陣為

(11)

1.3 全局濾波最優估計

(12)

式中，a和b為需要確定的權重值。取k個時刻的測量數據進行最小二乘計算：

(13)

2 組合導航實驗與分析

為了驗證本文方法的可行性，實驗采用了Turtlebot2移動機器人實驗平臺(由團隊設計開發)進行傳感器掃描和實驗數據的采集，室內試驗是在實驗室內完成的。用于實驗的機器人平臺安裝了9自由度慣性傳感器(三軸加速度計、三軸陀螺儀和三軸磁力計)、Kinect2攝像頭和R-fans16線激光雷達。IMU的采集頻率是200 Hz，相機的采集頻率是30 Hz，圖像分辨率是640像素×480像素。實驗環境位于辦公室區域。實驗平臺、環境已經機器人路線規劃布局如圖2～圖4所示。

圖2 實驗平臺及環境

圖3 折形軌跡定位圖

圖4 弧形軌跡定位圖

本文實驗比較了純視覺里程計(DSO)方法、三維激光里程表方法和本文所提出的組合導航定位方法在相同環境下兩種不同類型軌跡的定位結果。實驗中機器人以5 m/min的速度保持勻速行駛，將機器人控制在預設好的路徑上行駛40 m以上，這樣可以保證定位時間在8 min以上，以考驗系統對慣性測量部分產生的累積誤差的消減能力。

圖3和圖4為3種方法在折形與弧形路徑上的定位測試結果。圖中黑色菱形表示起點和終點，黃色實線表示地面真實軌跡，綠色虛線表示融合方法的定位軌跡，紅色虛線表示純視覺里程計方法的定位軌跡，藍色虛線表示三維激光雷達方法的定位軌跡。總體來看，純視覺里程計的直接法(DSO)的定位軌跡偏離真值最多，3D LASER里程表方法與本文方法定位效果更加準確，但本文方法的定位軌跡更加接近真實路徑值。實驗中收集了8個控制點誤差數值，對3種方法的平均誤差和第95百分位誤差進行了計算并制表，如表1和表2所示。由表1和表2可知，本文提出的方法具有更高的定位精度。

表1 3種方法的定位誤差(折形路徑)

表2 3種方法的定位誤差(弧形路徑)

3 結束語

本文提出了一種基于聯邦濾波的室內多傳感器融合的導航定位方法，將INS分別同3D LASER與單目視覺構成兩個子濾波器，系統形成分散式濾波，有效避免了局部異常導致定位失敗的問題。實驗結果表明，所提融合方法具有比純單目視覺方法或者3D LASER里程表法更高的定位效果，平均誤差可達0.22 m。