基于自適應(yīng)UKF的移動(dòng)機(jī)器人室內(nèi)定位算法研究*

許 萬(wàn), 朱 力, 張宇豪, 方德浩

(1.湖北工業(yè)大學(xué) 機(jī)械工程學(xué)院，湖北 武漢 430068；2.武漢理工大學(xué) 機(jī)電工程學(xué)院，湖北 武漢 430070)

0 引 言

移動(dòng)機(jī)器人室內(nèi)定位問(wèn)題一直是移動(dòng)機(jī)器人領(lǐng)域研究的重點(diǎn)和難點(diǎn)。傳統(tǒng)的移動(dòng)機(jī)器人室內(nèi)定位方法主要有：慣導(dǎo)定位[1]、里程計(jì)定位[2]、磁導(dǎo)定位、視覺(jué)定位[3]以及激光雷達(dá)定位[4]等。然而，上述定位方法都存在一定的缺陷，難以直接應(yīng)用于移動(dòng)機(jī)器人室內(nèi)定位。

近年來(lái)，為打破傳統(tǒng)單一傳感器系統(tǒng)在某些環(huán)境下的局限性，需運(yùn)用多傳感器融合的定位方法。目前，常用的多傳感器融合定位算法主要有卡爾曼濾波(KF)，互補(bǔ)濾波和粒子濾波(PF)等[5～7]。與互補(bǔ)濾波和粒子濾波算法相比，卡爾曼濾波算法具有更高的實(shí)時(shí)性能、更小的計(jì)算量以及更好的濾波效果。但是卡爾曼濾波算法主要是對(duì)線性系統(tǒng)中的預(yù)測(cè)值和觀測(cè)值進(jìn)行融合，而實(shí)際中系統(tǒng)往往是非線性的。因此，基于無(wú)跡變換的無(wú)跡卡爾曼濾波(unsented Kalman filtering,UKF)算法[8～10]被提出，用來(lái)解決非線性系統(tǒng)下的位姿估計(jì)。然而在進(jìn)行UKF時(shí)，系統(tǒng)噪聲和觀測(cè)噪聲統(tǒng)計(jì)特性一般都是未知的，當(dāng)實(shí)際噪聲和先驗(yàn)噪聲特性不同時(shí)，UKF算法的精度會(huì)急劇下降，甚至導(dǎo)致濾波器發(fā)散。

為解決此問(wèn)題，王力等人[11]改進(jìn)了樣本點(diǎn)的采樣方式，通過(guò)加入比例系數(shù)替代原有的固定值采樣，提高了算法的濾波精度。楊菁華等人[12]提出了一種改進(jìn)的UKF算法，以多異類傳感器觀測(cè)量擴(kuò)展融合后的信息為新觀測(cè)量，建立混合坐標(biāo)系下的非線性測(cè)量方程，有效降低了目標(biāo)的定位誤差。Luo K等人[13]根據(jù)協(xié)方差匹配技術(shù)在線估計(jì)和調(diào)整系統(tǒng)和觀測(cè)噪聲的協(xié)方差矩陣，從而提高了濾波精度。雖然以上改進(jìn)取得了一定的效果，但所考慮的系統(tǒng)大多是確定性系統(tǒng)。實(shí)際上，在復(fù)雜室內(nèi)環(huán)境下，移動(dòng)機(jī)器人定位受到外部環(huán)境影響、傳感器測(cè)量誤差等干擾具有隨機(jī)性。通常，觀測(cè)噪聲的統(tǒng)計(jì)特性可以預(yù)先由傳感器的物理特性測(cè)得，但是系統(tǒng)噪聲的統(tǒng)計(jì)特性往往是未知和時(shí)變的。

針對(duì)以上問(wèn)題，本文在UKF算法的基礎(chǔ)上，對(duì)里程計(jì)、陀螺儀、激光雷達(dá)定位系統(tǒng)等傳感器數(shù)據(jù)進(jìn)行融合，根據(jù)激光雷達(dá)定位系統(tǒng)的誤差統(tǒng)計(jì)特性，預(yù)先校準(zhǔn)觀測(cè)噪聲的均值和協(xié)方差矩陣，并利用自適應(yīng)噪聲估計(jì)器在線估計(jì)未知系統(tǒng)噪聲的統(tǒng)計(jì)特性，以實(shí)現(xiàn)移動(dòng)機(jī)器人在復(fù)雜室內(nèi)環(huán)境下的精準(zhǔn)定位。

1 移動(dòng)機(jī)器人定位系統(tǒng)建模

1.1 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)預(yù)測(cè)模型

如圖1所示，為雙輪差速移動(dòng)機(jī)器人在全局坐標(biāo)系X-O-Y中的示意，XL-OL-YL為以激光雷達(dá)為坐標(biāo)原點(diǎn)的局部坐標(biāo)系。其中，移動(dòng)機(jī)器人的位姿由X=[xyθ]T表示，(x,y)為移動(dòng)機(jī)器人在全局坐標(biāo)系中的坐標(biāo)，其測(cè)量值來(lái)源于里程計(jì)，θ為移動(dòng)機(jī)器人與X軸正方向之間的夾角，其測(cè)量值來(lái)源于陀螺儀。移動(dòng)機(jī)器人的系統(tǒng)輸入控制量由u=[vω]T表示，其中，v，ω分別為移動(dòng)機(jī)器人運(yùn)動(dòng)的線速度和角速度，為慣性測(cè)量單元通過(guò)積分獲得的測(cè)量結(jié)果。

若移動(dòng)機(jī)器人在系統(tǒng)輸入控制作用下從t-1時(shí)刻的狀態(tài)Xt-1=[xyθ]T運(yùn)動(dòng)至t時(shí)刻的狀態(tài)Xt=[x′y′θ′]T，其計(jì)算公式可以表示如下

(1)

由于陀螺儀測(cè)量偏差以及輪子打滑導(dǎo)致的里程計(jì)航跡推算誤差，因此移動(dòng)機(jī)器人的預(yù)測(cè)模型可表示為

Xt=f(Xt-1)+εt

(2)

式中f(·)為運(yùn)動(dòng)學(xué)模型推導(dǎo)傳遞函數(shù)，Xt為t時(shí)刻的狀態(tài)量,εt為系統(tǒng)噪聲，εt～(0,Qt)，E(εt)=qt。

圖1 雙輪差速移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

1.2 激光雷達(dá)定位系統(tǒng)觀測(cè)模型

在移動(dòng)機(jī)器人多傳感器融合定位算法中，通過(guò)激光雷達(dá)觀測(cè)數(shù)據(jù)可實(shí)時(shí)校正里程計(jì)和陀螺儀等內(nèi)部傳感器的估算位姿，并消除累計(jì)誤差的影響。

假設(shè)二維激光雷達(dá)對(duì)反光板的識(shí)別都是一致性匹配的，因此不存在錯(cuò)誤的反光板匹配。根據(jù)基于最小二乘法的三邊定位算法[14]，在t時(shí)刻移動(dòng)機(jī)器人第i個(gè)反光板的測(cè)量值可以寫(xiě)為

(3)

由于激光雷達(dá)定位系統(tǒng)存在測(cè)量誤差[15]，因此移動(dòng)機(jī)器人的觀測(cè)模型可表示為

Zt=h(Xt)+δt

(4)

式中h(·)為基于反光板的激光雷達(dá)觀測(cè)函數(shù)，Zt為t時(shí)刻的觀測(cè)量，δt為觀測(cè)噪聲，δt～(0,Rt),E(δt)=rt。

理論上，反光板的數(shù)量越多，激光雷達(dá)定位系統(tǒng)的精度越高。但是在實(shí)際應(yīng)用場(chǎng)景下，激光雷達(dá)定位系統(tǒng)存在如下問(wèn)題：1)移動(dòng)機(jī)器人在復(fù)雜室內(nèi)環(huán)境下運(yùn)行的過(guò)程中，由于不確定性障礙物的影響，會(huì)出現(xiàn)部分反光板被遮擋的情況，從而導(dǎo)致定位偏差；2)激光雷達(dá)的更新頻率一般為10 Hz左右，要遠(yuǎn)遠(yuǎn)低于算法的控制周期，這會(huì)導(dǎo)致在一個(gè)控制周期內(nèi)傳感器數(shù)據(jù)得不到實(shí)時(shí)更新。因此,在移動(dòng)機(jī)器人室內(nèi)定位中，需要融合其他傳感器數(shù)據(jù)來(lái)進(jìn)行位姿估計(jì)。

2 自適應(yīng)UKF定位算法

2.1 UKF定位算法

本文采用UKF算法，對(duì)里程計(jì)、陀螺儀和激光雷達(dá)定位系統(tǒng)的定位數(shù)據(jù)進(jìn)行融合。

對(duì)于移動(dòng)機(jī)器人非線性系統(tǒng)

(5)

UKF定位算法的步驟為：

步驟1 初始化

(6)

步驟2 Sigma點(diǎn)采樣

(7)

式中λ=α2(n+κ)-n，為尺度調(diào)節(jié)因子。α和κ為確定采樣點(diǎn)的分布比例參數(shù)，取α=0.01，κ=3-n。β為非負(fù)權(quán)系數(shù)，取β=2最優(yōu)。

步驟3 預(yù)測(cè)更新

(8)

(9)

(10)

(11)

(12)

(13)

(14)

步驟4 測(cè)量更新

(15)

(16)

(17)

(18)

(19)

由于系統(tǒng)噪聲和觀測(cè)噪聲統(tǒng)計(jì)特性一般都是未知的，當(dāng)實(shí)際噪聲和先驗(yàn)噪聲統(tǒng)計(jì)特性不同時(shí)，UKF算法的精度會(huì)急劇下降，甚至導(dǎo)致濾波器發(fā)散。因此,需要引入自適應(yīng)噪聲估計(jì)器對(duì)噪聲統(tǒng)計(jì)特性進(jìn)行估計(jì)和修正，以降低先驗(yàn)噪聲特性不符合實(shí)際而產(chǎn)生的影響。

2.2 系統(tǒng)噪聲自適應(yīng)估計(jì)

在進(jìn)行移動(dòng)機(jī)器人位姿估計(jì)時(shí)，采用自適應(yīng)噪聲估計(jì)器對(duì)系統(tǒng)噪聲的均值和協(xié)方差矩陣進(jìn)行實(shí)時(shí)估計(jì)。同時(shí)，根據(jù)激光雷達(dá)定位系統(tǒng)的誤差統(tǒng)計(jì)特性預(yù)先校準(zhǔn)觀測(cè)噪聲的均值和協(xié)方差矩陣。

對(duì)于式(2)中的未知系統(tǒng)噪聲，采用自適應(yīng)噪聲估計(jì)器對(duì)qt和Qt進(jìn)行實(shí)時(shí)估計(jì)，得到系統(tǒng)噪聲統(tǒng)計(jì)特性更新過(guò)程為

(20)

(21)

其中

(22)

式中b∈(0.95,0.99)，其目的為加大當(dāng)前觀測(cè)數(shù)據(jù)在歷史估計(jì)中的作用。

自適應(yīng)噪聲估計(jì)器能夠有效地跟蹤系統(tǒng)噪聲的統(tǒng)計(jì)特性，減小模型誤差并抑制濾波發(fā)散，從而提高算法的魯棒性和濾波精度。將系統(tǒng)噪聲統(tǒng)計(jì)特性更新過(guò)程作為步驟5，對(duì)UKF算法中的系統(tǒng)噪聲進(jìn)行實(shí)時(shí)估計(jì)，即為自適應(yīng)UKF定位算法的設(shè)計(jì)步驟。

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)平臺(tái)

本文所使用的實(shí)驗(yàn)平臺(tái)為搭載激光雷達(dá)R2000的雙輪差速車MIR100，主要包括3個(gè)部分：上位機(jī)、移動(dòng)機(jī)器人底盤(pán)和激光雷達(dá)定位系統(tǒng)。實(shí)驗(yàn)環(huán)境為室內(nèi)測(cè)試環(huán)境，在水平桌面上擺放半徑為55 mm的圓柱形反光板，實(shí)驗(yàn)地面為平坦瓷磚地面。實(shí)驗(yàn)平臺(tái)如圖2所示。激光雷達(dá)定位系統(tǒng)通過(guò)局域網(wǎng)將定位數(shù)據(jù)傳給移動(dòng)機(jī)器人。

圖2 實(shí)驗(yàn)平臺(tái)

3.2 實(shí)驗(yàn)結(jié)果

為驗(yàn)證本文提出的自適應(yīng)UKF定位算法的定位效果, 在系統(tǒng)噪聲統(tǒng)計(jì)特性未知的情況下，將激光雷達(dá)定位系統(tǒng)的觀測(cè)值和未加自適應(yīng)噪聲估計(jì)器的UKF定位算法作為對(duì)照組實(shí)驗(yàn)。實(shí)驗(yàn)中使用的反光板位置數(shù)據(jù)，如表1所示。

表1 反光板位置數(shù)據(jù)

設(shè)定自適應(yīng)UKF定位算法的估計(jì)初始值為

P0=diag[1 1 1],r0=[0 0 0]T,q0=[0 0 0]T

R0=diag[0.013 1 0.010 5 0.042 6]

Q0=diag[0.001 0.001 0.011]

(23)

分別選擇直線與曲線兩種具有代表性的移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡進(jìn)行實(shí)驗(yàn)，如圖3所示。

圖3 移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡

移動(dòng)機(jī)器人運(yùn)動(dòng)過(guò)程中，對(duì)移動(dòng)機(jī)器人施加一次隨機(jī)擾動(dòng)，同時(shí)采集3種算法的定位數(shù)據(jù)，并計(jì)算其距離誤差以及角度誤差，繪制定位誤差圖，如圖4所示。

圖4 移動(dòng)機(jī)器人定位誤差

根據(jù)距離誤差和角度誤差，計(jì)算出定位誤差，如表2所示。

表2 各算法軌跡誤差對(duì)比

由圖4和表2可知，自適應(yīng)UKF定位算法相比于UKF定位算法：在位置估計(jì)上，直線運(yùn)動(dòng)位移精度提高約34.2 %，曲線運(yùn)動(dòng)位移精度提高約40.0 %；在姿態(tài)估計(jì)上，直線運(yùn)動(dòng)角度精度提高約38.5 %，曲線運(yùn)動(dòng)角度精度提高約34.4 %。并且在隨機(jī)擾動(dòng)的影響下，自適應(yīng)UKF定位算法比UKF定位算法的抗干擾能力更強(qiáng)。

4 結(jié) 論

在復(fù)雜的室內(nèi)環(huán)境下，針對(duì)實(shí)際噪聲和先驗(yàn)噪聲統(tǒng)計(jì)特性不同而導(dǎo)致UKF算法精度急劇下降的問(wèn)題，本文提出了一種自適應(yīng)UKF定位算法。該方法首先以UKF算法為基礎(chǔ)，融合里程計(jì)、陀螺儀、激光雷達(dá)定位系統(tǒng)等傳感器數(shù)據(jù)，然后根據(jù)激光雷達(dá)定位系統(tǒng)的誤差統(tǒng)計(jì)特性預(yù)先校準(zhǔn)觀測(cè)噪聲的均值和協(xié)方差矩陣，并利用自適應(yīng)噪聲估計(jì)器在線估計(jì)未知系統(tǒng)噪聲的統(tǒng)計(jì)特性，提高了濾波的數(shù)值穩(wěn)定性，減小了狀態(tài)估計(jì)誤差。最后實(shí)驗(yàn)結(jié)果表明，相比于UKF定位算法，自適應(yīng)UKF定位算法具有較強(qiáng)的魯棒性和較高的定位精度，適用于復(fù)雜室內(nèi)環(huán)境下的移動(dòng)機(jī)器人位姿估計(jì)。