時(shí) 也，吳懷宇，徐文霞，彭晟遠(yuǎn)

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

在當(dāng)今社會(huì)，移動(dòng)機(jī)器人應(yīng)具備在未知環(huán)境下的自主導(dǎo)航能力，才有其實(shí)際存在價(jià)值[1]。近年來，一些特大自然災(zāi)害的發(fā)生，更使人們注重機(jī)器人的實(shí)用性和準(zhǔn)確性。機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的核心問題是地圖構(gòu)建與定位 （SLAM），解決SLAM問題才可以使機(jī)器人在未知環(huán)境中的工作更高效和更科學(xué)。移動(dòng)機(jī)器人的SLAM算法研究已是機(jī)器人研究領(lǐng)域中的最熱門、最豐富、最具開拓性的課題之一[2-3]。

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

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

1 機(jī)器人地圖創(chuàng)建與定位

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

項(xiàng)目組的移動(dòng)機(jī)器人MT-R裝有：雙目傳感器、聲納傳感器、激光測(cè)距儀、里程計(jì)，機(jī)器人可裝載機(jī)械臂。激光測(cè)距儀是移動(dòng)機(jī)器人的基本感知裝置，用于獲得環(huán)境中障礙物距離信息，這些信息可用于辨識(shí)障礙物，并用障礙物作為路標(biāo)。根據(jù)路標(biāo)之間的關(guān)聯(lián)，計(jì)算路標(biāo)參數(shù)的狀態(tài)方程以及觀測(cè)方程，并使之系統(tǒng)化。建立障礙物與路標(biāo)之間的關(guān)聯(lián)方程，由此可得一個(gè)基于障礙物作為路標(biāo)的全局地圖。

2 基于擴(kuò)展卡爾曼濾波器的SLAM算法

2.1 狀態(tài)方程

首先用［Fx，F(xiàn)y］表示全局坐標(biāo)系，［Gx，Gy］表示局部坐標(biāo)系來建立機(jī)器人的空間狀態(tài)方程。為了構(gòu)建全局地圖，擴(kuò)展卡爾曼濾波器中的狀態(tài)向量包含移動(dòng)機(jī)器人的狀態(tài)部分（X）及路標(biāo)狀態(tài)部分（G）。 Fx（k）和Fy（k）為機(jī)器人在時(shí)間k時(shí)在全局坐標(biāo)系中的位置坐標(biāo)。 ξ（k）=cosθ（k），η（k）=sinθ（k），為狀態(tài)變量則狀態(tài)向量。Xu（k）為機(jī)器人的狀態(tài)向量：

機(jī)器人的狀態(tài)向量和路標(biāo)狀態(tài)向量應(yīng)滿足公式（2）和公式（3）：

機(jī)器人狀態(tài)轉(zhuǎn)移矩陣FU（k）滿足公式（4）：

系統(tǒng)噪聲wu（k）滿足公式（5）：

假定在全局坐標(biāo)系中，路標(biāo)是靜止對(duì)象，因此，如果［Fxn（k）Fyn（k）］T是Gn，那么機(jī)器人的狀態(tài)方程如下：

在方程（1）中，路標(biāo)的過程模型是個(gè)單位矩陣，因此GN在以及Xu（k）中的狀態(tài)向量:

2.2 觀測(cè)方程

在局部坐標(biāo)系中， 路標(biāo)m的局部坐標(biāo)為zm（k）=［Gxm（k）Gym（k）］。 在k時(shí)與全局坐標(biāo)系中路標(biāo)n的坐標(biāo)Gn相關(guān)聯(lián)，矩陣Hn（k）定義為：

那么對(duì)應(yīng)狀態(tài)向量X（k）的觀測(cè)方程：

2.3 擴(kuò)展卡爾曼濾波器的應(yīng)用

擴(kuò)展卡爾曼濾波器由狀態(tài)方程及觀測(cè)方程組成，工作流程為：預(yù)測(cè)步驟、檢測(cè)步驟、更新步驟。這3個(gè)步驟并不是獨(dú)立的，而是循序漸進(jìn)并且循環(huán)往復(fù)的。

L（k）是2×2矩陣，它 的對(duì)角由方 差rΔd（k）和rΔθ（k）定義， rΔd（k）為Δd（k）的預(yù)測(cè)誤差，rΔθ（k）為Δθ（k）的預(yù)測(cè)誤差。

R為觀測(cè)誤差的方差矩陣，得到Dn（k）在相關(guān)的n點(diǎn)路標(biāo)被定義為：（Gxm（k）Gym（k））T

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

由檢測(cè)和更新步驟可得到全局坐標(biāo)系中的路標(biāo)坐標(biāo)Gn和局部坐標(biāo)系中坐標(biāo)Zm（k）的轉(zhuǎn)換公式，當(dāng)有m個(gè)路標(biāo)被偵測(cè)，則檢測(cè)步驟和步驟更新重復(fù)m次。

2.4 不同坐標(biāo)下參數(shù)的轉(zhuǎn)換

圖1 不同坐標(biāo)系下的參數(shù)轉(zhuǎn)換Fig.1 Parameters coversion between different coordinates

3 實(shí)驗(yàn)及分析

使移動(dòng)機(jī)器人在環(huán)境中行走一個(gè)閉合的環(huán)路。機(jī)器人的移動(dòng)速度約為0.56 m/s，光電編碼器的采樣周期為5 ms，激光測(cè)距儀的采樣周期為0.5 s。整個(gè)過程持續(xù)90.2 s。激光測(cè)距儀的掃描范圍為0°～180°，角度分辨率為0.5°，每掃描一次產(chǎn)生361個(gè)數(shù)據(jù)。

圖2是機(jī)器人的位姿對(duì)比曲線，激光測(cè)距儀的最大感知距離為8 m，范圍不確定度為0.03 m。圖中虛線表示在死區(qū)地帶的機(jī)器人位姿曲線，實(shí)線為經(jīng)過EKF校正的機(jī)器人位姿曲線。

圖2 機(jī)器人的位姿對(duì)比曲線Fig.2 Curves of robot pose

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

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

4 結(jié)束語

文中針對(duì)移動(dòng)機(jī)器人的地圖創(chuàng)建與定位問題，采用基于擴(kuò)展卡爾曼濾波器法，利用激光測(cè)距儀建立起參考坐標(biāo)系，以障礙物作為路標(biāo)，從而使機(jī)器人辨識(shí)自身位置，使用擴(kuò)展卡爾曼濾波器來代替粒子濾波器以獲得實(shí)時(shí)地圖。最后通過仿真和物理實(shí)驗(yàn)驗(yàn)證了準(zhǔn)確性。機(jī)器人的定位與地圖創(chuàng)建問題，是機(jī)器人領(lǐng)域的關(guān)鍵技術(shù)之一，為后續(xù)的解決機(jī)器人的SLAM導(dǎo)航和死區(qū)定位的精度研究提供了理論依據(jù)。

[1]蔡自興，賀漢根，陳虹.未知環(huán)境中移動(dòng)機(jī)器人導(dǎo)航控制研究的若干問題[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]趙冬斌，易建強(qiáng).全方位移動(dòng)機(jī)器人導(dǎo)論[M].北京:科學(xué)出版社，2010.

[3]湯曉.基于激光測(cè)距儀的移動(dòng)機(jī)器人同時(shí)定位和地圖創(chuàng)建[D].濟(jì)南：山東大學(xué)，2007.

[4]莊嚴(yán)，王偉.移動(dòng)機(jī)器人基于激光測(cè)距和單目視覺的室內(nèi)同時(shí)定位和地圖構(gòu)建[J].自動(dòng)化學(xué)報(bào)，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]方正，佟國(guó)峰，徐心和.基于貝葉斯濾波理論的自主機(jī)器人自定位方法研究[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]王彭林，石守東，洪小偉.基于單目視覺和里程計(jì)的SLAM算法研究[J].計(jì)算機(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]石杏喜，趙春霞.基于概率的移動(dòng)機(jī)器人SLAM算法框架[J].計(jì)算機(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]陳衛(wèi)東，張飛.移動(dòng)機(jī)器人的同步自定位與地圖創(chuàng)建研究進(jìn)展[J].控制理論與應(yīng)用，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.