基于擴展卡爾曼濾波器的移動機器人SLAM研究

時 也，吳懷宇，徐文霞，彭晟遠

（武漢科技大學 信息科學與工程學院，湖北 武漢 430081）

在當今社會，移動機器人應具備在未知環境下的自主導航能力，才有其實際存在價值[1]。近年來，一些特大自然災害的發生，更使人們注重機器人的實用性和準確性。機器人實現自主導航的核心問題是地圖構建與定位 （SLAM），解決SLAM問題才可以使機器人在未知環境中的工作更高效和更科學。移動機器人的SLAM算法研究已是機器人研究領域中的最熱門、最豐富、最具開拓性的課題之一[2-3]。

移動機器人的地圖創建與定位問題是機器人領域一個基礎而關鍵的問題。文獻[4]針對缺少先驗地圖支持的室內環境，提出利用激光測距儀和單目視覺的信息融合來解決室內環境中移動機器人的SLAM問題。文獻[5]提出基于單目視覺和里程計的SLAM方法，采用尺度不變特征變換算法提取特征，并用擴展卡爾曼濾波更新地圖。文獻[7]提出基于概率的移動機器人SLAM算法框架，應用貝葉斯規則作為理論基礎，建立移動機器人SLAM算法的概率表示模型。文獻[8-9]針對SLAM問題的高維特性，提出了FastSLAM解決方案。

文中以課題組的移動機器人MT-R為研究對象，給定路標的參數值，利用激光測距儀計算路標之間的關系方程。重點研究用障礙物之間的空間邏輯關系，采用障礙物作為機器人的路標并以此來代替固有路標，估算出機器人在全局坐標系中移動的距離，使機器人在線構造地圖的同時，能夠提取并使用地圖。最后通過MATLAB仿真算法和在未知環境下實驗，使移動機器人從起始點繞過不定數目的障礙物后再返回到起始點，驗證理論的正確性和算法的精確性。

1 機器人地圖創建與定位

目前廣泛使用卡爾曼濾波器或粒子濾波器來解決SLAM問題。粒子濾波雖然能夠比較精確地表達基于觀測量和控制量的后驗概率分布，但其計算量大而精度低。擴展卡爾曼濾波器可以用在非線性系統上，且計算量相比粒子濾波少，因此選擇擴展卡爾曼濾波器算法。

項目組的移動機器人MT-R裝有：雙目傳感器、聲納傳感器、激光測距儀、里程計，機器人可裝載機械臂。激光測距儀是移動機器人的基本感知裝置，用于獲得環境中障礙物距離信息，這些信息可用于辨識障礙物，并用障礙物作為路標。根據路標之間的關聯，計算路標參數的狀態方程以及觀測方程，并使之系統化。建立障礙物與路標之間的關聯方程，由此可得一個基于障礙物作為路標的全局地圖。

2 基于擴展卡爾曼濾波器的SLAM算法

2.1 狀態方程

首先用［Fx，Fy］表示全局坐標系，［Gx，Gy］表示局部坐標系來建立機器人的空間狀態方程。為了構建全局地圖，擴展卡爾曼濾波器中的狀態向量包含移動機器人的狀態部分（X）及路標狀態部分（G）。 Fx（k）和Fy（k）為機器人在時間k時在全局坐標系中的位置坐標。 ξ（k）=cosθ（k），η（k）=sinθ（k），為狀態變量則狀態向量。Xu（k）為機器人的狀態向量：

機器人的狀態向量和路標狀態向量應滿足公式（2）和公式（3）：

機器人狀態轉移矩陣FU（k）滿足公式（4）：

系統噪聲wu（k）滿足公式（5）：

假定在全局坐標系中，路標是靜止對象，因此，如果［Fxn（k）Fyn（k）］T是Gn，那么機器人的狀態方程如下：

在方程（1）中，路標的過程模型是個單位矩陣，因此GN在以及Xu（k）中的狀態向量:

2.2 觀測方程

在局部坐標系中， 路標m的局部坐標為zm（k）=［Gxm（k）Gym（k）］。 在k時與全局坐標系中路標n的坐標Gn相關聯，矩陣Hn（k）定義為：

那么對應狀態向量X（k）的觀測方程：

2.3 擴展卡爾曼濾波器的應用

擴展卡爾曼濾波器由狀態方程及觀測方程組成，工作流程為：預測步驟、檢測步驟、更新步驟。這3個步驟并不是獨立的，而是循序漸進并且循環往復的。

L（k）是2×2矩陣，它 的對角由方 差rΔd（k）和rΔθ（k）定義， rΔd（k）為Δd（k）的預測誤差，rΔθ（k）為Δθ（k）的預測誤差。

R為觀測誤差的方差矩陣，得到Dn（k）在相關的n點路標被定義為：（Gxm（k）Gym（k））T

協方差矩陣P（k｜k-1）更新為：

由檢測和更新步驟可得到全局坐標系中的路標坐標Gn和局部坐標系中坐標Zm（k）的轉換公式，當有m個路標被偵測，則檢測步驟和步驟更新重復m次。

2.4 不同坐標下參數的轉換

圖1 不同坐標系下的參數轉換Fig.1 Parameters coversion between different coordinates

3 實驗及分析

使移動機器人在環境中行走一個閉合的環路。機器人的移動速度約為0.56 m/s，光電編碼器的采樣周期為5 ms，激光測距儀的采樣周期為0.5 s。整個過程持續90.2 s。激光測距儀的掃描范圍為0°～180°，角度分辨率為0.5°，每掃描一次產生361個數據。

圖2是機器人的位姿對比曲線，激光測距儀的最大感知距離為8 m，范圍不確定度為0.03 m。圖中虛線表示在死區地帶的機器人位姿曲線，實線為經過EKF校正的機器人位姿曲線。

圖2 機器人的位姿對比曲線Fig.2 Curves of robot pose

圖3是機器人的位姿誤差曲線，可以看出：在前90次的定位中，的校正量保持在±0.9 cm之內，的校正量保持在±2.0 cm之內，的校正量在±0.8°之內。

圖3 機器人的位姿誤差曲線Fig.3 The error curve of robot pose

4 結束語

文中針對移動機器人的地圖創建與定位問題，采用基于擴展卡爾曼濾波器法，利用激光測距儀建立起參考坐標系，以障礙物作為路標，從而使機器人辨識自身位置，使用擴展卡爾曼濾波器來代替粒子濾波器以獲得實時地圖。最后通過仿真和物理實驗驗證了準確性。機器人的定位與地圖創建問題，是機器人領域的關鍵技術之一，為后續的解決機器人的SLAM導航和死區定位的精度研究提供了理論依據。

[1]蔡自興，賀漢根，陳虹.未知環境中移動機器人導航控制研究的若干問題[J].控制與決策，2002，17（4）:385-390.

CAI Zi-xing，HE Han-gen，CHEN Hong.Some issues for mobile robots navigation under unknown environments[J].Control and Decision，2002，17（4）:385-390.

[2]趙冬斌，易建強.全方位移動機器人導論[M].北京:科學出版社，2010.

[3]湯曉.基于激光測距儀的移動機器人同時定位和地圖創建[D].濟南：山東大學，2007.

[4]莊嚴，王偉.移動機器人基于激光測距和單目視覺的室內同時定位和地圖構建[J].自動化學報，2005，31（6）:925-933.

ZHUANG Yan，WANG Wei.Mobile robot based on laser range finder and monocular vision indoor simultaneous localization and map building[J].Acta Automatica Sinica，2005，31（6）:925-933.

[5]方正，佟國峰，徐心和.基于貝葉斯濾波理論的自主機器人自定位方法研究[J].控制與決策，2006，21（8）：841-846，862.

FANG Zheng，TONG Guo-feng，XU Xin-he.Study of autonomous robot self-locatization methods based on bayesian filter theory[J].Control and Decision，2006，21（8）：841-846，862.

[6]王彭林，石守東，洪小偉.基于單目視覺和里程計的SLAM算法研究[J].計算機仿真，2008，25（10）：172-175.

WANG Peng-lin，SHI Shou-dong，HONG Xiao-wei.A SLAM alogorithm based on monocular vision and adometer[J].Computer Simulation，2008，25（10）：172-175.

[7]石杏喜，趙春霞.基于概率的移動機器人SLAM算法框架[J].計算機工程，2010，36（2）：31-32.

SHI Xing-xi，ZHAO Chun-xia.SLAM algorithm framework of mobile robot based on probability[J].Computer Engineering，2010，36（2）：31-32.

[8]Newman P，Leonard J，Tardós J D，et al.Explore and return:experimental validation of real time concurrent mapping and localization[C].Proceedings of ICRA，2002.

[9]Welch G，Bishop G.An introduction to the kalman filter[R].Chapel Hill：University of North Carolina，2001.

[10]陳衛東，張飛.移動機器人的同步自定位與地圖創建研究進展[J].控制理論與應用，2005，22（3）：455-460.

CHEN Wei-dong，ZHANG Fei.Review on the achievements in simultaneous localization and mapbuilding for mobile robot[J].Control Theory and Applications，2005，22（3）：455-460.