劉鈺芃，王斌宇，張淞淏，楊伊帆，王曉燕

（西安建筑科技大學(xué)機(jī)電工程學(xué)院，西安 710055）

移動機(jī)器人定位技術(shù)是衡量移動機(jī)器人自主導(dǎo)航能力的重要指標(biāo)[1]。近年來，隨著國內(nèi)外學(xué)者對多傳感器信息融合定位技術(shù)研究的深入，多傳感器信息融合定位算法得到了快速發(fā)展，包括擴(kuò)展卡爾曼濾波定位[2]、無跡卡爾曼濾波定位[3]和粒子濾波定位[4]。其中，基于擴(kuò)展卡爾曼濾波的多傳感器信息融合定位技術(shù)在無人機(jī)探測、水下搜索和無人駕駛等領(lǐng)域的應(yīng)用最為廣泛。

擴(kuò)展卡爾曼濾波可以有效解決非線性系統(tǒng)估計(jì)的問題，但線性化過程會帶來誤差。為了解決這個(gè)問題，文獻(xiàn)[5]提出了一種基于多傳感器信息融合的迭代擴(kuò)展卡爾曼濾波定位算法，將每次系統(tǒng)測量更新得到的系統(tǒng)后驗(yàn)估計(jì)作為下一個(gè)擴(kuò)展卡爾曼濾波測量更新過程的先驗(yàn)估計(jì)進(jìn)行更新計(jì)算，提高了濾波精度。文獻(xiàn)[6]提出了一種基于新息估計(jì)的AEKF 算法。該算法利用系統(tǒng)的新息矩陣，自適應(yīng)調(diào)整狀態(tài)噪聲和觀測噪聲的協(xié)方差矩陣，從而抑制狀態(tài)誤差的增加。自適應(yīng)IEKF 算法修正了噪聲協(xié)方差，但不可避免地會累積計(jì)算誤差，可能導(dǎo)致卡爾曼濾波增益失去加權(quán)調(diào)整的功能。

本文提出一種改進(jìn)的IEKF 定位算法。該算法通過新息估計(jì)方法自適應(yīng)調(diào)整系統(tǒng)的狀態(tài)噪聲協(xié)方差和觀測噪聲協(xié)方差，減小恒定噪聲協(xié)方差的影響；引入遺忘因子調(diào)整誤差協(xié)方差預(yù)測值，限制了卡爾曼濾波的長度，減小了舊數(shù)據(jù)的累積計(jì)算誤差；最后，采用Levenberg-Marquardt（LM）方法優(yōu)化IEKF 的迭代過程，以提高全局估計(jì)的收斂性。

1 系統(tǒng)模型

本文以兩輪差分移動機(jī)器人為研究對象。系統(tǒng)的狀態(tài)模型由里程計(jì)模型建立，觀測模型由激光雷達(dá)測距傳感器建立。模型圖（圖1）和計(jì)算公式如下。

圖1 兩輪差速移動機(jī)器人的狀態(tài)模型

兩輪差速移動機(jī)器人的模型如圖1 所示。R是機(jī)器人運(yùn)動的弧半徑，A為左右輪軸的中心點(diǎn)，D為移動機(jī)器人左右輪之間的距離，Δx、Δy、Δθ分別為移動機(jī)器人位置在采樣時(shí)間Δt內(nèi)的橫坐標(biāo)、縱坐標(biāo)和方向角的變化。移動機(jī)器人左右驅(qū)動輪的行駛距離為ΔSL和ΔSR。

兩輪差速移動機(jī)器人在橫坐標(biāo)和縱坐標(biāo)的位置變化如下

將系統(tǒng)的輸入定義為U（k）=[ΔS，Δθ]T，系統(tǒng)在k時(shí)刻的狀態(tài)向量由Xk∈[xk yk θk]T表示，則系統(tǒng)的狀態(tài)方程可表示為

觀測模型和計(jì)算過程如圖2 所示。

圖2 兩輪差分移動機(jī)器人的測量模型

由圖2 所知，激光雷達(dá)觀測到的地標(biāo)（xi，yi）與機(jī)器人在k時(shí)刻位置[xk yk θk]T的相對位置關(guān)系。d是地標(biāo)與移動機(jī)器人的相對距離、α是地標(biāo)相對移動機(jī)器人的夾角、α-θk是地標(biāo)相對于機(jī)器人移動方向的角度。激光雷達(dá)返回的地標(biāo)觀測信息是機(jī)器人與地標(biāo)之間的相對位置信息（d，α）。觀測模型可以表示為

2 改進(jìn)的IEKF 定位算法

2.1 迭代擴(kuò)展卡爾曼濾波

本文將兩輪差分移動機(jī)器人建立的狀態(tài)方程和觀察方程離散化后表示如下[7]

式中：Xk∈[xk yk θk]T是系統(tǒng)的狀態(tài)向量；Zk∈[d，α]T是系統(tǒng)的觀察向量；Φk，k-1是系統(tǒng)的狀態(tài)轉(zhuǎn)換矩陣；h（·）是測量函數(shù)；wk-1是狀態(tài)噪聲，并且wk-1∈N（0，Qk-1）；vk是系統(tǒng)觀測噪聲，和vk∈N（0，Rk）；wk-1并且vk是高斯白噪聲，E（wk-1，vk）=0。

基于IEKF 算法的多傳感器信息融合過程如下[8]

式中：是系統(tǒng)的一步態(tài)估計(jì)是系統(tǒng)的一步誤差協(xié)方差預(yù)測是卡爾曼濾波的增益；是創(chuàng)新估計(jì)向量；是系統(tǒng)的后驗(yàn)狀態(tài)估計(jì)是系統(tǒng)的預(yù)測值是后驗(yàn)誤差協(xié)方差；是觀察方程的雅可比矩陣；i是迭代的IEKF 編號（i=1，2，3，…，n）。

IEKF 將每個(gè)擴(kuò)展卡爾曼濾波過程的后驗(yàn)估計(jì)作為先驗(yàn)估計(jì)，代入多次迭代的測量更新過程中，使測量信息得到充分利用，估計(jì)值在計(jì)算中接近實(shí)值。

2.2 基于遺忘因子的改進(jìn)自適應(yīng)IEKF 算法

由于計(jì)算誤差的累積，系統(tǒng)誤差協(xié)方差可能會失去其非負(fù)定性和對稱性，從而導(dǎo)致卡爾曼濾波增益矩陣的損失。為了解決這一問題，本文在改進(jìn)自適應(yīng)IEKF 的基礎(chǔ)上引入遺忘因子來調(diào)整系統(tǒng)誤差協(xié)方差，限制卡爾曼濾波的存儲長度，減少過去測量數(shù)據(jù)的內(nèi)存，增加當(dāng)前測量數(shù)據(jù)的應(yīng)用，從而進(jìn)一步提高系統(tǒng)的定位精度。

目前，計(jì)算遺忘因子常用的方法有2 種：一步法和簡化算法。為了減少計(jì)算量，提高濾波器的估計(jì)速度，本文選取簡化算法作為遺忘因子的計(jì)算方法，計(jì)算過程如下

計(jì)算遺忘因子：

式中：λk是遺忘因子；trace（·）是矩陣跡線的計(jì)算；Nk計(jì)算方法Mk如下

式中：Ck是k時(shí)刻新息協(xié)方差的理論值，計(jì)算如下

新息協(xié)方差一般用窗口法估計(jì)

即

式中：en是新息殘差。

為了更好地利用新的測量值來修改狀態(tài)估計(jì)值，避免模型誤差對濾波新息值的影響，本文采用公式（22）作為遺忘因子加法，因?yàn)槠淇梢愿玫匕盐障到y(tǒng)預(yù)測誤差協(xié)方差的整體變化。由公式（21）可知，C^k會隨著en的增大而增大，同時(shí)遺忘因子也會增大，從而提高濾波精度。

2.3 基于LM 優(yōu)化的改進(jìn)自適應(yīng)IEKF 算法

由于IEKF 的一階泰勒展開中的線性化誤差，系統(tǒng)狀態(tài)模型與實(shí)際測量結(jié)果不匹配。在迭代過程中，系統(tǒng)的誤差協(xié)方差將小于實(shí)際值，這將影響估計(jì)的穩(wěn)定性。針對該問題，本文采用Levenberg Marquardt（LM）方法優(yōu)化了IEKF 的迭代過程，提高了IEKF 算法迭代過程的穩(wěn)定性和全局收斂性。

LM 方法的主要思想是引入修正參數(shù)αi，在每次迭代中調(diào)整系統(tǒng)誤差協(xié)方差預(yù)測矩陣，然后使用調(diào)整后的預(yù)測協(xié)方差矩陣迭代更新計(jì)算。

系統(tǒng)的誤差協(xié)方差預(yù)測矩陣調(diào)整如下

綜上所述，基于LM 優(yōu)化的具有遺忘因子的改進(jìn)自適應(yīng)IEKF 算法的流程表示如下。

步驟1：初始化系統(tǒng)狀態(tài)估計(jì)X^0和系統(tǒng)誤差協(xié)方差預(yù)測矩陣P0；

步驟2通過方程（10）和（11）計(jì)算系統(tǒng)狀態(tài)的一步預(yù)測值和一步誤差協(xié)方差預(yù)測值，完成時(shí)間更新過程；

步驟3：Kk從系統(tǒng)狀態(tài)預(yù)測值和優(yōu)化的系統(tǒng)預(yù)測誤差協(xié)方差計(jì)算卡爾曼濾波增益、新息殘差向量ek和觀測方程雅可比矩陣Hk，然后更新誤差協(xié)方差估計(jì)和狀態(tài)估計(jì)，完成測量更新過程；

步驟4：通過卡爾曼濾波增益Kk、新息殘差向量ek和觀測方程雅可比矩陣Hk，通過自適應(yīng)調(diào)整狀態(tài)噪聲協(xié)方差Qk-1和測量噪聲協(xié)方差Rk，分別對步驟2 和步驟3 進(jìn)行調(diào)整；

步驟5：通過方程（16），（17），（20）和方程（21）從新息殘差向量ek和觀測方程雅可比矩陣Hk計(jì)算遺忘因子λk；然后將遺忘因子λk應(yīng)用于步驟2；

步驟6：利用LM 算法優(yōu)化系統(tǒng)誤差協(xié)方差的預(yù)測值；

步驟7：重復(fù)步驟3 至步驟6，當(dāng)?shù)螖?shù)達(dá)到最大迭代次數(shù)時(shí)，退出迭代，輸出和。

3 仿真實(shí)驗(yàn)與分析

3.1 仿真設(shè)計(jì)

假設(shè)在靜態(tài)室內(nèi)環(huán)境（障礙物保持靜止）下，兩輪差分移動機(jī)器人的運(yùn)動速度為v=3.0 m/s、角速度為rate G=60×π/180 rad，激光雷達(dá)測距傳感器的最大觀測距離20 m，觀察角度范圍360°。狀態(tài)噪聲為σv=0.6 m/s，σg=1.0×π/180 rad，觀測噪聲σR=0.2 m/s，σB=1.0×π/180 rad，兩輪差速移動機(jī)器人2 個(gè)驅(qū)動輪之間的距離D=0.5 m?？偛蓸訒r(shí)間為60 s，采樣間隔為0.1 s，即ΔT=0.1 s 步長。設(shè)置LM 校正參數(shù)αi=0.1，滑動窗口S=5。開展50 次蒙特卡羅實(shí)驗(yàn)，以采樣時(shí)間內(nèi)IEKF、LM-IEKF、LM-AIEKF、LM-AIEKF 和LM-FAIEKF 的估計(jì)值和實(shí)際值的RMSE（均方根誤差）平均值為標(biāo)準(zhǔn)，測量算法的定位精度，RMSE 越小，定位精度越高。RMSE 平均值計(jì)算如下

3.2 仿真結(jié)果

隨著定位過程的進(jìn)行，狀態(tài)噪聲和觀測噪聲會受到環(huán)境因素的影響。在本實(shí)驗(yàn)中，前300 個(gè)仿真步長中狀態(tài)噪聲和觀測噪聲的協(xié)方差設(shè)置為原始步驟的一半，即0.5Q 和0.5R；后300 個(gè)仿真步長中的狀態(tài)噪聲協(xié)方差和觀測噪聲協(xié)方差與初始值一致，通過50 次蒙特卡羅實(shí)驗(yàn)驗(yàn)證了每種算法的收斂性和定位精度。結(jié)果如圖3 所示。

圖3 噪聲變化情況下不同算法定位誤差

圖3 比較了狀態(tài)噪聲和觀測噪聲變化時(shí)IEKF、LM-IEKF、LM-AIEKF 和LM-FAIEKF 的定位均方根誤差。從圖3 可以看出，4 種算法的定位誤差隨著系統(tǒng)狀態(tài)噪聲誤差和觀測噪聲誤差的增加而增大。其中，LM-IEKF 算法比IEKF 算法具有更好的收斂性和穩(wěn)定性；LM-AIEKF 算法的定位精度優(yōu)于LM-IEKF 算法。50 次蒙特卡羅實(shí)驗(yàn)中不同算法的RMSE 平均值和平均運(yùn)行時(shí)間見表1。

表1 RMSE 平均值和平均運(yùn)行時(shí)間

由表1 可知，通過50 次蒙特卡羅實(shí)驗(yàn)，與IEKF算法相比，LM-IEKF、LM-AIEKF 和LM-FAIEKF 的定位精度分別提高了39.59%、46.57%、84.62%。本文提出的改進(jìn)IEKF 算法（LM-FAIEKF）的定位精度明顯優(yōu)于其他3 種算法。雖然該算法的平均運(yùn)行時(shí)間高于IEKF和LM-IEKF 算法，略小于LM-AIEKF 算法，但具有良好的收斂性和魯棒性，符合預(yù)期要求。

4 結(jié)論

本文提出一種改進(jìn)的室內(nèi)移動機(jī)器人多傳感器融合定位IEKF 算法。解決了姿態(tài)估計(jì)中恒定噪聲協(xié)方差誤差、前一數(shù)據(jù)累積計(jì)算誤差、IEKF 算法收斂性差等問題。在2 種仿真場景中，比較了IEKF、LM-IEKF、LM-AIEKF 和LM-FAIEKF 的性能。仿真結(jié)果表明，所提算法具有較高的穩(wěn)定性和定位精度，符合室內(nèi)移動機(jī)器人的定位要求。