基于改進IEKF 的室內移動機器人定位算法研究

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

（西安建筑科技大學機電工程學院，西安 710055）

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

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

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

1 系統模型

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

圖1 兩輪差速移動機器人的狀態模型

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

兩輪差速移動機器人在橫坐標和縱坐標的位置變化如下

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

觀測模型和計算過程如圖2 所示。

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

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

2 改進的IEKF 定位算法

2.1 迭代擴展卡爾曼濾波

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

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

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

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

IEKF 將每個擴展卡爾曼濾波過程的后驗估計作為先驗估計，代入多次迭代的測量更新過程中，使測量信息得到充分利用，估計值在計算中接近實值。

2.2 基于遺忘因子的改進自適應IEKF 算法

由于計算誤差的累積，系統誤差協方差可能會失去其非負定性和對稱性，從而導致卡爾曼濾波增益矩陣的損失。為了解決這一問題，本文在改進自適應IEKF 的基礎上引入遺忘因子來調整系統誤差協方差，限制卡爾曼濾波的存儲長度，減少過去測量數據的內存，增加當前測量數據的應用，從而進一步提高系統的定位精度。

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

計算遺忘因子：

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

式中：Ck是k時刻新息協方差的理論值，計算如下

新息協方差一般用窗口法估計

即

式中：en是新息殘差。

為了更好地利用新的測量值來修改狀態估計值，避免模型誤差對濾波新息值的影響，本文采用公式（22）作為遺忘因子加法，因為其可以更好地把握系統預測誤差協方差的整體變化。由公式（21）可知，C^k會隨著en的增大而增大，同時遺忘因子也會增大，從而提高濾波精度。

2.3 基于LM 優化的改進自適應IEKF 算法

由于IEKF 的一階泰勒展開中的線性化誤差，系統狀態模型與實際測量結果不匹配。在迭代過程中，系統的誤差協方差將小于實際值，這將影響估計的穩定性。針對該問題，本文采用Levenberg Marquardt（LM）方法優化了IEKF 的迭代過程，提高了IEKF 算法迭代過程的穩定性和全局收斂性。

LM 方法的主要思想是引入修正參數αi，在每次迭代中調整系統誤差協方差預測矩陣，然后使用調整后的預測協方差矩陣迭代更新計算。

系統的誤差協方差預測矩陣調整如下

綜上所述，基于LM 優化的具有遺忘因子的改進自適應IEKF 算法的流程表示如下。

步驟1：初始化系統狀態估計X^0和系統誤差協方差預測矩陣P0；

步驟2通過方程（10）和（11）計算系統狀態的一步預測值和一步誤差協方差預測值，完成時間更新過程；

步驟3：Kk從系統狀態預測值和優化的系統預測誤差協方差計算卡爾曼濾波增益、新息殘差向量ek和觀測方程雅可比矩陣Hk，然后更新誤差協方差估計和狀態估計，完成測量更新過程；

步驟4：通過卡爾曼濾波增益Kk、新息殘差向量ek和觀測方程雅可比矩陣Hk，通過自適應調整狀態噪聲協方差Qk-1和測量噪聲協方差Rk，分別對步驟2 和步驟3 進行調整；

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

步驟6：利用LM 算法優化系統誤差協方差的預測值；

步驟7：重復步驟3 至步驟6，當迭代次數達到最大迭代次數時，退出迭代，輸出和。

3 仿真實驗與分析

3.1 仿真設計

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

3.2 仿真結果

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

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

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

表1 RMSE 平均值和平均運行時間

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

4 結論

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