張 逵， 郭 杭， 敖龍輝

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

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

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

不同的環(huán)境下，傳感器的穩(wěn)定性和可靠性會有很大的不同，甚至?xí)?dǎo)致定位結(jié)果失敗[13-15]。因此，本文設(shè)計了一種基于聯(lián)邦濾波的INS/3D LASER/單目視覺組合導(dǎo)航定位方法，利用分散式濾波的方式將IMU、3D LASER和單目視覺的信息進(jìn)行分級融合處理。本文設(shè)計了混合聯(lián)邦濾波器，通過最小二乘的思路解決了系統(tǒng)自適應(yīng)問題，在濾波過程中使用最小方差準(zhǔn)則和最優(yōu)系數(shù)加權(quán)法處理數(shù)據(jù)，進(jìn)而得到系統(tǒng)全局最優(yōu)估計結(jié)果。

1 數(shù)據(jù)融合方法

聯(lián)邦濾波能夠?qū)π枰诤系男畔⑦M(jìn)行塊估計，然后分兩步進(jìn)行級聯(lián)分散濾波。濾波系統(tǒng)由一個主濾波器和多個子級濾波器構(gòu)成，主級濾波器和子級濾波器能夠同時進(jìn)行數(shù)據(jù)處理。由于慣性導(dǎo)航部分能夠穩(wěn)定地輸出測量結(jié)果，故本文采用慣性導(dǎo)航與視覺SLAM的融合濾波器和慣性導(dǎo)航與激光SLAM的融合濾波器獨立并行進(jìn)行濾波測量更新，然后將各自融合處理的數(shù)據(jù)傳輸?shù)街鳛V波器中進(jìn)行全局最優(yōu)融合處理。該并行分散的方法能夠有效降低系統(tǒng)的融合處理計算量，同時也能夠避免因某個子級濾波器出現(xiàn)處理錯誤而產(chǎn)生的錯誤信息，極大地提高了容錯能力。本文的融合算法流程如圖1所示。

圖1 濾波算法流程圖

首先構(gòu)建慣性導(dǎo)航系統(tǒng)(INS)作為公共參考系統(tǒng)，其狀態(tài)表達(dá)為

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

(1)

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

1.1 INS與3D LASER濾波

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

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

(2)

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

(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)

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

(6)

式中，HLiDAR=-17×7,

(7)

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

1.2 INS與單目視覺組合濾波

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

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

(8)

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

(9)

(10)

觀測矩陣為

(11)

1.3 全局濾波最優(yōu)估計

(12)

式中，a和b為需要確定的權(quán)重值。取k個時刻的測量數(shù)據(jù)進(jìn)行最小二乘計算：

(13)

2 組合導(dǎo)航實驗與分析

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

圖2 實驗平臺及環(huán)境

圖3 折形軌跡定位圖

圖4 弧形軌跡定位圖

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

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

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

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

3 結(jié)束語

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