智能機(jī)器人自主導(dǎo)航與路徑規(guī)課件

智能機(jī)器人原理與實(shí)踐1智能機(jī)器人原理與實(shí)踐1

第7章智能機(jī)器人自主導(dǎo)航與路

徑規(guī)劃

2

第7章智能機(jī)器人自主導(dǎo)航與路

7.1概述

導(dǎo)航，最初是指對(duì)航海的船舶抵達(dá)目的地進(jìn)行的導(dǎo)引過程。這一術(shù)語和自主性相結(jié)合，已成為智能機(jī)器人研究的核心和熱點(diǎn)。Leonard和Durrant-Whyte將移動(dòng)機(jī)器人導(dǎo)航定義為三個(gè)子問題：

（1）“WhereamI？”——環(huán)境認(rèn)知與機(jī)器人定位；

（2）“WhereamIgoing？”——目標(biāo)識(shí)別；

（3）“HowdoIgetthere？”——路徑規(guī)劃。

為完成導(dǎo)航，機(jī)器人需要依靠自身傳感系統(tǒng)對(duì)內(nèi)部姿態(tài)和外部環(huán)境信息進(jìn)行感知，通過對(duì)環(huán)境空間信息的存儲(chǔ)、識(shí)別、搜索等操作尋找最優(yōu)或近似最優(yōu)的無碰撞路徑并實(shí)現(xiàn)安全運(yùn)動(dòng)。37.1概述導(dǎo)航，最初是指對(duì)航海的船舶抵達(dá)目的地進(jìn)行的導(dǎo)7.1.1導(dǎo)航系統(tǒng)分類

對(duì)于不同的室內(nèi)與室外環(huán)境、結(jié)構(gòu)化與非結(jié)構(gòu)化環(huán)境，機(jī)器人完成準(zhǔn)確的自身定位后，常用的導(dǎo)航方式主要有磁導(dǎo)航、慣性導(dǎo)航、視覺導(dǎo)航、衛(wèi)星導(dǎo)航等。1.磁導(dǎo)航

磁導(dǎo)航是在路徑上連續(xù)埋設(shè)多條引導(dǎo)電纜，分別流過不同頻率的電流，通過感應(yīng)線圈對(duì)電流的檢測(cè)來感知路徑信息。

2.慣性導(dǎo)航

慣性導(dǎo)航是利用陀螺儀和加速度計(jì)等慣性傳感器測(cè)量移動(dòng)機(jī)器人的方位角和加速率，從而推知機(jī)器人當(dāng)前位置和下一步的目的地。3.視覺導(dǎo)航

依據(jù)環(huán)境空間的描述方式，可將移動(dòng)機(jī)器人的視覺導(dǎo)航方式劃分為三類。47.1.1導(dǎo)航系統(tǒng)分類4

（1）基于地圖的導(dǎo)航：是完全依靠在移動(dòng)機(jī)器人內(nèi)部預(yù)先保存好的關(guān)于環(huán)境的幾何模型、拓?fù)涞貓D等比較完整的信息，在事先規(guī)劃出的全局路線基礎(chǔ)上，應(yīng)用路徑跟蹤和避障技術(shù)來實(shí)現(xiàn)的；

（2）基于創(chuàng)建地圖的導(dǎo)航：是利用各種傳感器來創(chuàng)建關(guān)于當(dāng)前環(huán)境的幾何模型或拓?fù)淠Ｐ偷貓D，然后利用這些模型來實(shí)現(xiàn)導(dǎo)航；

（3）無地圖的導(dǎo)航：是在環(huán)境信息完全未知的情況下，可通過攝像機(jī)或其他傳感器對(duì)周圍環(huán)境進(jìn)行探測(cè)，利用對(duì)探測(cè)的物體進(jìn)行識(shí)別或跟蹤來實(shí)現(xiàn)導(dǎo)航。4．衛(wèi)星導(dǎo)航

移動(dòng)機(jī)器人通過安裝衛(wèi)星信號(hào)接收裝置，可以實(shí)現(xiàn)自身定位，無論其在室內(nèi)還是室外。5（1）基于地圖的導(dǎo)航：是完全依靠在移動(dòng)機(jī)器人內(nèi)部預(yù)先保存

7.1.2導(dǎo)航系統(tǒng)體系結(jié)構(gòu)

智能機(jī)器人的導(dǎo)航系統(tǒng)是一個(gè)自主式智能系統(tǒng)，其主要任務(wù)是如何把感知、規(guī)劃、決策和行動(dòng)等模塊有機(jī)地結(jié)合起來。下圖給出了一種智能機(jī)器人自主導(dǎo)航系統(tǒng)的控制結(jié)構(gòu)。67.1.2導(dǎo)航系統(tǒng)體系結(jié)構(gòu)6

777.2環(huán)境地圖的表示

構(gòu)造地圖的目的是用于絕對(duì)坐標(biāo)系下的位姿估計(jì)。地圖的表示方法通常有四種：拓?fù)鋱D、特征圖、網(wǎng)格圖及直接表征法(Appearancebasedmethods)。不同方法具有各自的特點(diǎn)和適用范圍，其中特征圖和網(wǎng)格圖應(yīng)用較為普遍。87.2環(huán)境地圖的表示8

7.2.1拓?fù)鋱D

1）基本思想

地鐵、公交路線圖均是典型的拓?fù)涞貓D實(shí)例，其中?？空緸楣?jié)點(diǎn)，節(jié)點(diǎn)間的通道為邊。在一般的辦公環(huán)境中，拓?fù)鋯卧凶呃群头块g等，而打印機(jī)、桌椅等則是功能單元。連接器用于連接對(duì)應(yīng)的位置，如門、樓梯、電梯等。2）特點(diǎn)拓?fù)鋱D把環(huán)境建模為一張線圖表示，忽略了具體的幾何特征信息，不必精確表示不同節(jié)點(diǎn)間的地理位置關(guān)系，圖形抽象，表示方便。97.2.1拓?fù)鋱D9

7.2.2特征圖1）基本思想

結(jié)構(gòu)化環(huán)境中，最常見的特征是直線段、角、邊等。

這些特征可用它們的顏色、長度、寬度、位置等參數(shù)表示。

基于特征的地圖一般用式（7.1）的特征集合表示：其中

是一個(gè)特征(邊、線角等)，n是地圖中的特征總數(shù)。、

（7.1）

107.2.2特征圖其中是一個(gè)特征(邊、線角等)

2）特點(diǎn)

特征法定位準(zhǔn)確，模型易于由計(jì)算機(jī)描述和表示，參數(shù)化特征也適用于路徑規(guī)劃和軌跡控制，但特征法需要特征提取等預(yù)處理過程，對(duì)傳感器噪聲比較敏感，只適于高度結(jié)構(gòu)化環(huán)境。112）特點(diǎn)117.2.3網(wǎng)格圖1）基本思想

網(wǎng)格圖把機(jī)器人的工作空間劃分成網(wǎng)狀結(jié)構(gòu)，網(wǎng)格中的每一單元代表環(huán)境的一部分，每一個(gè)單元都分配了一個(gè)概率值，表示該單元被障礙物占據(jù)的可能性大小。2）特點(diǎn)

網(wǎng)格法是一種近似描述，易于創(chuàng)建和維護(hù)，對(duì)某個(gè)網(wǎng)格的感知信息可直接與環(huán)境中某個(gè)區(qū)域?qū)?yīng)，機(jī)器人對(duì)所測(cè)得的障礙物具體形狀不太敏感，特別適于處理超聲測(cè)量數(shù)據(jù)。但當(dāng)在大型環(huán)境中或網(wǎng)格單元?jiǎng)澐直容^細(xì)時(shí)，網(wǎng)格法計(jì)算量迅速增長，需要大量內(nèi)存單元，使計(jì)算機(jī)的實(shí)時(shí)處理變得很困難。127.2.3網(wǎng)格圖127.2.4直接表征法1）基本思想

通過記錄來自不同位置及方向的環(huán)境外觀感知數(shù)據(jù)，這些圖像中包括了某些坐標(biāo)、幾何特征或符號(hào)信息，利用這些數(shù)據(jù)作為在這些位置處的環(huán)境特征描述。

直接表征法與識(shí)別拓?fù)湮恢盟捎玫姆椒ㄔ砩鲜且粯拥模顒e僅在于該法試圖從所獲取的傳感器數(shù)據(jù)中創(chuàng)建一個(gè)函數(shù)關(guān)系以便更精確地確定機(jī)器人的位姿。2）特點(diǎn)

直接表征法數(shù)據(jù)存貯量大，環(huán)境噪聲干擾嚴(yán)重，特征數(shù)據(jù)的提取與匹配困難，其應(yīng)用受到一定限制。137.2.4直接表征法137.3定位

定位是確定機(jī)器人在其作業(yè)環(huán)境中所處位置。機(jī)器人可以利用先驗(yàn)環(huán)境地圖信息、位姿的當(dāng)前估計(jì)以及傳感器的觀測(cè)值等輸入信息，經(jīng)過一定處理變換，獲得更準(zhǔn)確的當(dāng)前位姿。

移動(dòng)機(jī)器人定位方式有很多種，常用的可以采用里程計(jì)、攝像機(jī)、激光雷達(dá)、聲納、速度或加速度計(jì)等。

從方法上來分，移動(dòng)機(jī)器人定位可分為相對(duì)定位和絕對(duì)定位兩種。147.3定位147.3.1相對(duì)定位

相對(duì)定位又稱為局部位置跟蹤，要求機(jī)器人在已知初始位置的條件下通過測(cè)量機(jī)器人相對(duì)于初始位置的距離和方向來確定當(dāng)前位置，通常也稱航跡推算法。

相對(duì)定位只適于短時(shí)短距離運(yùn)動(dòng)的位姿估計(jì)，長時(shí)間運(yùn)動(dòng)時(shí)必須應(yīng)用其它的傳感器配合相關(guān)的定位算法進(jìn)行校正。157.3.1相對(duì)定位151.里程計(jì)法里程計(jì)法是移動(dòng)機(jī)器人定位技術(shù)中廣泛采用的方法之一。在移動(dòng)機(jī)器人的車輪上安裝光電編碼器，通過編碼器記錄的車輪轉(zhuǎn)動(dòng)圈數(shù)來計(jì)算機(jī)器人的位移和偏轉(zhuǎn)角度。里程計(jì)法定位過程中會(huì)產(chǎn)生兩種誤差。161.里程計(jì)法161）系統(tǒng)誤差 系統(tǒng)誤差在很長的時(shí)間內(nèi)不會(huì)改變，和機(jī)器人導(dǎo)航的外界環(huán)境并沒有關(guān)系，主要由下列因素引起：（1）驅(qū)動(dòng)輪直徑不等；（2）驅(qū)動(dòng)輪實(shí)際直徑的均值和名義直徑不等；（3）驅(qū)動(dòng)輪軸心不重合；（4）驅(qū)動(dòng)輪間輪距長度不確定；（5）有限的編碼器測(cè)量精度；（6）有限的編碼器采樣頻率。171）系統(tǒng)誤差 172）非系統(tǒng)誤差非系統(tǒng)誤差是在機(jī)器人和外界環(huán)境接觸過程中，由于外界環(huán)境不可預(yù)料特性引起的。主要誤差來源如下：（1）輪子打滑；（2）地面不平；（3）地面有無法預(yù)料的物體（例如石塊）；（4）外力作用和內(nèi)力作用；（5）驅(qū)動(dòng)輪和地板是面接觸而不是點(diǎn)接觸。182）非系統(tǒng)誤差18

對(duì)于機(jī)器人定位來說，非系統(tǒng)誤差是異常嚴(yán)重的問題，因?yàn)樗鼰o法預(yù)測(cè)并導(dǎo)致嚴(yán)重的方向誤差。非系統(tǒng)誤差包括方向誤差和位置誤差?？紤]機(jī)器人的定位誤差時(shí)，方向誤差是主要的誤差源。機(jī)器人導(dǎo)航過程中小的方向誤差會(huì)導(dǎo)致嚴(yán)重的位置誤差。輪子打滑和地面不平都能導(dǎo)致嚴(yán)重的方向誤差。在室內(nèi)環(huán)境中，輪子打滑對(duì)機(jī)器人定位精度的影響要比地面不平對(duì)定位精度影響要大，因?yàn)檩喿哟蚧l(fā)生的頻率更高。19193）誤差補(bǔ)償機(jī)器人定位過程中，需要利用外界的傳感器信息補(bǔ)償誤差。因此利用外界傳感器定位機(jī)器人時(shí)，主要任務(wù)在于如何提取導(dǎo)航環(huán)境的特征并和環(huán)境地圖進(jìn)行匹配。在室內(nèi)環(huán)境中，墻壁、走廊、拐角、門等特征被廣泛地用于機(jī)器人的定位研究。20202.慣性導(dǎo)航定位法慣性導(dǎo)航定位法是一種使用慣性導(dǎo)航傳感器定位的方法。它通常用陀螺儀來測(cè)量機(jī)器人的角速度，用加速度計(jì)測(cè)量機(jī)器人的加速度。對(duì)測(cè)量結(jié)果進(jìn)行一次和二次積分即可得到機(jī)器人偏移的角度和位移，進(jìn)而得出機(jī)器人當(dāng)前的位置和姿態(tài)。21217.3.2絕對(duì)定位

絕對(duì)定位又稱為全局定位，要求機(jī)器人在未知初始位置的情況下確定自己的位置。主要采用導(dǎo)航信標(biāo)、主動(dòng)或被動(dòng)標(biāo)識(shí)、地圖匹配、衛(wèi)星導(dǎo)航技術(shù)或概率方法進(jìn)行定位，定位精度較高。這幾種方法中，信標(biāo)或標(biāo)識(shí)牌的建設(shè)和維護(hù)成本較高，地圖匹配技術(shù)處理速度慢，GPS只能用于室外，目前精度還很差，絕對(duì)定位的位置計(jì)算方法包括三視角法、三視距法、模型匹配算法等。227.3.2絕對(duì)定位221.主動(dòng)燈塔法主動(dòng)燈塔是可以很可靠地被檢測(cè)到的信號(hào)發(fā)射源，將該信號(hào)進(jìn)行最少的處理就可以提供精確的定位信息。2.路標(biāo)導(dǎo)航定位法路標(biāo)導(dǎo)航定位法是利用環(huán)境中的路標(biāo)，給移動(dòng)機(jī)器人提供位置信息。路標(biāo)分為人工路標(biāo)和自然路標(biāo)。3.地圖匹配法基于地圖的定位方法稱為地圖匹配法。機(jī)器人運(yùn)用各種傳感器(如超聲波傳感器、激光測(cè)距儀、視覺系統(tǒng)等)探測(cè)環(huán)境來創(chuàng)建它所處的局部環(huán)境地圖，然后將此局部地圖與存儲(chǔ)在機(jī)器人中的己知的全局地圖進(jìn)行匹配。如果匹配成功，機(jī)器人就計(jì)算出自身在該環(huán)境中的位置。231.主動(dòng)燈塔法23

4.GPS定位GPS是適用于室外移動(dòng)機(jī)器人的一種全局定位系統(tǒng)，它是一種以空間衛(wèi)星為基礎(chǔ)的高精度導(dǎo)航與定位系統(tǒng)，是由美國國防部批準(zhǔn)研制，為海、陸、空三軍服務(wù)的一種新的軍用衛(wèi)星導(dǎo)航系統(tǒng)，該系統(tǒng)由三大部分構(gòu)成：GPS衛(wèi)星星座(空間部分)，地面監(jiān)控部分(控制部分)和GPS信號(hào)接收機(jī)(用戶部分)。GPS系統(tǒng)能夠?qū)嵤┤蛐?、全天候、?shí)時(shí)連續(xù)的三維導(dǎo)航定位服務(wù)。24247.3.3基于概率的絕對(duì)定位

概率定位中最重要的馬爾可夫定位和蒙特卡羅定位。馬爾可夫定位和蒙特卡羅定位不僅能夠?qū)崿F(xiàn)全局定位和局部位置跟蹤，而且能夠解決機(jī)器人的“綁架”問題。機(jī)器人“綁架”問題是指，由于機(jī)器人容易與外界發(fā)生碰撞而使機(jī)器人在不知情(里程計(jì)沒有記錄)的情況下發(fā)生移動(dòng)。257.3.3基于概率的絕對(duì)定位25

1.馬爾可夫定位（MarkovLocalization，ML）

爾可夫定位的基本思想是：機(jī)器人不知道它的確切位置，而是知道它可能位置的信度（Belief，即機(jī)器人在整個(gè)位置空間的概率分布，信度值之和為1）。馬爾可夫定位的關(guān)鍵之處在于信度值的計(jì)算。當(dāng)機(jī)器人收到外界傳感器信息或者利用編碼器獲得機(jī)器人移動(dòng)信息時(shí)，基于馬爾可夫假設(shè)和貝葉斯規(guī)則，每個(gè)柵格的信度值被更新。

2626

根據(jù)初始狀態(tài)概率分布和觀測(cè)數(shù)據(jù)估計(jì)系統(tǒng)的當(dāng)前狀態(tài)，其中表示機(jī)器人的位姿(由位置和方向組成)。從統(tǒng)計(jì)學(xué)的觀點(diǎn)看，的估計(jì)是一個(gè)貝葉斯濾波問題，可以通過估計(jì)后驗(yàn)密度分布來實(shí)現(xiàn)。貝葉斯濾波器假設(shè)系統(tǒng)是一個(gè)馬爾可夫過程，可以通過以下2步算得。

27

271)預(yù)測(cè)通過運(yùn)動(dòng)模型預(yù)測(cè)系統(tǒng)在下一時(shí)刻的狀態(tài)，即通過如下公式計(jì)算先驗(yàn)概率密度：

（7.2）式中：稱為系統(tǒng)的運(yùn)動(dòng)模型(狀態(tài)轉(zhuǎn)移先驗(yàn)密度)。

281)預(yù)測(cè)

282)更新通過觀測(cè)模型利用新的觀測(cè)信息更新系統(tǒng)的狀態(tài)，即通過如下公式計(jì)算后驗(yàn)概率密度：（7.3）式中：稱為系統(tǒng)的觀測(cè)模型(觀測(cè)密度)。當(dāng)機(jī)器人獲得編碼器信息或者利用外界傳感器感知環(huán)境后，馬爾可夫定位算法必須對(duì)所有的柵格進(jìn)行計(jì)算，因此需要大量的計(jì)算資源和內(nèi)存，導(dǎo)致定位處理的實(shí)時(shí)性很差。

292)更新

292.蒙特卡羅定位（Monte-CarloLocalization，MCL）基于馬爾可夫定位方法，Dellaert等人提出了蒙特卡羅定位方法。MCL也稱為粒子濾波（ParticleFilter）。MCL的主要思想是用N個(gè)帶有權(quán)重的離散采樣

來表示后驗(yàn)概率密度。其中是機(jī)器人在t時(shí)刻的一個(gè)可能狀態(tài)；是一個(gè)非負(fù)的參數(shù)稱為權(quán)重，表示t時(shí)刻機(jī)器人的狀態(tài)為的概率也就是

，且

。

302.蒙特卡羅定位（Monte-CarloLocaliMCL包括4個(gè)階段：初始化，采樣階段，權(quán)重歸一化和輸出階段。采樣階段是MCL的核心，它包括重采樣、狀態(tài)轉(zhuǎn)移和權(quán)重計(jì)算3步。實(shí)際上MCL是按照提議密度分布抽取采樣，然后利用權(quán)重來補(bǔ)償提議密度分布與后驗(yàn)密度分布之間的差距。當(dāng)機(jī)器人發(fā)生“綁架”時(shí)，要估計(jì)的后驗(yàn)密度與提議密度分布的錯(cuò)位很大，在取值較大區(qū)域的采樣數(shù)很少，需要大量的采樣才能較好地估計(jì)后驗(yàn)密度。

31MCL包括4個(gè)階段：初始化，采樣階段，權(quán)重歸一化和輸出階3.卡爾曼濾波定位（KalmanFilter，KF）kalman濾波器是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法?；舅枷胧遣捎眯盘?hào)和噪聲空間狀態(tài)模型，結(jié)合當(dāng)前時(shí)刻的觀測(cè)值和前一時(shí)刻的估計(jì)值來更新對(duì)狀態(tài)變量的估計(jì)，從而得到當(dāng)前時(shí)刻的估計(jì)值。對(duì)于非線性估計(jì)問題，可以通過線性近似去解決。相應(yīng)的方法有EKF(ExtendedKalmanFilter)、UKF(UnscentedKalmanFilter)等。323.卡爾曼濾波定位（KalmanFilter，KFkalman濾波器通過預(yù)測(cè)方程和測(cè)量方程對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)，利用遞推的方式尋找最小均方誤差下的的估計(jì)值。kalman濾波的數(shù)學(xué)模型為：狀態(tài)方程為：

（7.4）測(cè)量方程為：(7.5）

其中，是k時(shí)刻時(shí)系統(tǒng)的狀態(tài)，A是k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，是k時(shí)刻的測(cè)量值，H是觀測(cè)矩陣，為系統(tǒng)過程噪聲，為系統(tǒng)測(cè)量噪聲，假設(shè)為高斯白噪聲。

33kalman濾波器通過預(yù)測(cè)方程和測(cè)量方程對(duì)系統(tǒng)狀態(tài)進(jìn)如果不考慮觀測(cè)噪聲和輸入信號(hào)時(shí)，則k時(shí)刻的觀測(cè)值和已知的最有狀態(tài)估計(jì)值，可通過一下方程進(jìn)行求解

最優(yōu)估計(jì)值。狀態(tài)預(yù)測(cè)方程：（7.6）預(yù)測(cè)狀態(tài)下的協(xié)方差方程：（7.7）濾波器增益矩陣：（7.8）

34如果不考慮觀測(cè)噪聲和輸入信號(hào)時(shí)，則k時(shí)刻的觀測(cè)值和已狀態(tài)最優(yōu)化估計(jì)方程：（7.9）狀態(tài)最優(yōu)化估計(jì)的協(xié)方差方程：（7.10）通過kalman濾波器的公式可以看出，只要給定了和，就可以根據(jù)k時(shí)刻的觀測(cè)值，就可以通過遞推計(jì)算得出k時(shí)刻的狀態(tài)估計(jì)。

35狀態(tài)最優(yōu)化估計(jì)方程：

35下圖給出了卡爾曼濾波根據(jù)所有傳感器提供的信息，實(shí)現(xiàn)高效信息融合的一般方案。36下圖給出了卡爾曼濾波根據(jù)所有傳感器提供的信息，實(shí)現(xiàn)高kalman濾波器已經(jīng)廣泛應(yīng)用在了各個(gè)方面，比如機(jī)器人的SLAM問題，雷達(dá)系統(tǒng)的跟蹤的等等。下圖描述了卡爾曼濾波器的機(jī)器人定位架構(gòu)。37kalman濾波器已經(jīng)廣泛應(yīng)用在了各個(gè)方面，比如機(jī)器人的(1)位置預(yù)測(cè)：基于帶有高斯誤差的運(yùn)動(dòng)系統(tǒng)模型，機(jī)器人根據(jù)編碼器數(shù)據(jù)，進(jìn)行位置預(yù)測(cè)。(2)傳感器測(cè)量：機(jī)器人收集實(shí)際的傳感器數(shù)據(jù)，提取合適的環(huán)境特征，產(chǎn)生一個(gè)實(shí)際的位置。(3)匹配更新：機(jī)器人要在實(shí)際提取的特征和測(cè)量預(yù)測(cè)的期望特征之間，辨識(shí)最佳的信息?？柭鼮V波器可以將所有這些匹配所提供的信息融合，遞歸估計(jì)更新機(jī)器人的狀態(tài)。38387.4路徑規(guī)劃7.4.1路徑規(guī)劃分類路徑規(guī)劃本身可以分成不同的層次，從不同的方面有不同的劃分。根據(jù)對(duì)環(huán)境的掌握情況，機(jī)器人的路徑規(guī)劃問題可以大致分為三種類型：1.基于地圖的全局路徑規(guī)劃基于地圖的全局路徑規(guī)劃，根據(jù)先驗(yàn)環(huán)境模型找出從起始點(diǎn)到目標(biāo)點(diǎn)的符合一定性能的可行或最優(yōu)的路徑。2.基于傳感器的局部路徑規(guī)劃基于傳感器的局部路徑規(guī)劃，依賴傳感器獲得障礙物的尺寸、形狀和位置等信息。環(huán)境是未知或部分未知的。397.4路徑規(guī)劃39

3.混合型方法混合型方法試圖結(jié)合全局和局部的優(yōu)點(diǎn)，將全局規(guī)劃的“粗”路徑作為局部規(guī)劃的目標(biāo)，從而引導(dǎo)機(jī)器人最終找到目標(biāo)點(diǎn)。40407.4.2路徑規(guī)劃方法1.可視圖法可視圖（VisibilityGraph，VG）由一系列障礙物的頂點(diǎn)和機(jī)器人起始點(diǎn)及目標(biāo)點(diǎn)用直線組合相連。要求機(jī)器人和障礙物各頂點(diǎn)之間、目標(biāo)點(diǎn)和障礙物各頂點(diǎn)之間以及各障礙物頂點(diǎn)與頂點(diǎn)之間的連線均不能穿越障礙物，即直線是“可視的”。這樣，從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑轉(zhuǎn)化為從起始點(diǎn)到目標(biāo)點(diǎn)經(jīng)過這些可視直線的最短距離問題。圖中粗實(shí)線即為由VG法得到的具有最短路徑，但由于過于靠近障礙物，得到路徑的安全性較差?？梢晥D法適用于環(huán)境中的障礙物是多邊形的情況。417.4.2路徑規(guī)劃方法41可視圖法路徑規(guī)劃如下圖所示：

42可視圖法路徑規(guī)劃如下圖所示：422.Voronoi圖法Voronoi圖，又叫泰森多邊形圖。如下圖所示，它是由一組由連接兩鄰點(diǎn)直線的垂直平分線組成的連續(xù)多邊形組成。432.Voronoi圖法43由下圖可見，Voronoi圖路徑規(guī)劃盡可能遠(yuǎn)離障礙物，從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑將會(huì)增長。但采用這種控制方式時(shí)，即使產(chǎn)生位置誤差，移動(dòng)機(jī)器人也不會(huì)碰到障礙物，其缺點(diǎn)是存在較多的突變點(diǎn)。44由下圖可見，Voronoi圖路徑規(guī)劃盡可能遠(yuǎn)離障礙物

3.單元分解法如下圖所示，首先把狀態(tài)空間分解為與空間平行的許多矩形或立方體，稱為單元（Cell），其中每個(gè)cell都標(biāo)記為：1)空的：如果Cell內(nèi)每一點(diǎn)均與狀態(tài)空間的障礙物不相交；2)滿的：如果Cell內(nèi)每一點(diǎn)均與狀態(tài)空間中的障礙物相交；3)混合的：如果Cell內(nèi)點(diǎn)既有與狀態(tài)空間的障礙物相交，也有不相交的。

狀態(tài)空間分解453.單元分解法狀態(tài)空間分解45單元分解法就是要尋找一條由空的Cell所組成的包含有起點(diǎn)和目標(biāo)點(diǎn)的連通路徑，如下圖所示。如果這樣的路徑在初始劃分的狀態(tài)空間中不存在，則要找出所有混合cell，將其進(jìn)一步細(xì)分，并將劃分的結(jié)果進(jìn)行標(biāo)記，然后在空的cell中進(jìn)行搜索，如此反復(fù)，直至成功。連通路徑搜索46單元分解法就是要尋找一條由空的Cell所組成的包含有起點(diǎn)

4.人工勢(shì)場(chǎng)法傳統(tǒng)的人工勢(shì)場(chǎng)法把智能機(jī)器人在環(huán)境中的運(yùn)動(dòng)視為一種在抽象的人造受力場(chǎng)中的運(yùn)動(dòng)，目標(biāo)點(diǎn)對(duì)智能機(jī)器人產(chǎn)生“引力”，障礙物對(duì)智能機(jī)器人產(chǎn)生“斥力”，最后通過求合力來控制智能機(jī)器人的運(yùn)動(dòng)。但是，由于勢(shì)場(chǎng)法把所有信息壓縮為單個(gè)合力，這樣就存在把有關(guān)障礙物分布的有價(jià)值的信息拋棄的缺陷，且易陷入局部最小值。47475.A*算法1）A*算法原型Dijkstra算法的基本思想如圖所示：485.A*算法48從初始點(diǎn)S到目標(biāo)點(diǎn)E尋求最低花費(fèi)路徑，粗黑的箭頭代表尋找到的最優(yōu)路徑。圓圈代表節(jié)點(diǎn)，圓圈中間數(shù)字代表從初始點(diǎn)經(jīng)過最低花費(fèi)的路徑到達(dá)該點(diǎn)時(shí)的總花費(fèi)，箭頭上數(shù)字代表從箭頭始端指向末端所需的花費(fèi)，算法通過比較各條路徑選擇了一條最短的花費(fèi)作為該點(diǎn)圓圈內(nèi)的數(shù)字。2）A*算法流程A*算法具體引入了當(dāng)前結(jié)點(diǎn)的估計(jì)函數(shù)f(i)，結(jié)點(diǎn)的評(píng)價(jià)函數(shù)可以定義為：（7.11）式中：g(i)表示從起點(diǎn)到當(dāng)前結(jié)點(diǎn)的最短距離，h(i)表示從當(dāng)前結(jié)點(diǎn)到終點(diǎn)的最短距離的估計(jì)值，可取結(jié)點(diǎn)到終點(diǎn)的直線和球面距離。

49從初始點(diǎn)S到目標(biāo)點(diǎn)E尋求最低花費(fèi)路徑，粗黑的箭頭代表尋找

若，即沒有利用任何路網(wǎng)信息，這時(shí)的A*算法就變成了Dijkstra算法。可見，A*算法實(shí)質(zhì)是Dijkstra算法的改進(jìn)“算法”。對(duì)于h(i)的具體形式，也可以依據(jù)實(shí)際情況進(jìn)行選擇。例如，除了可以當(dāng)前結(jié)點(diǎn)到終點(diǎn)的最短距離之外，還可以引入方向。A*算法本身表述起來很簡單，關(guān)鍵是在代碼優(yōu)化上，基本的思路一般都是以空間(即內(nèi)存的占傭)換取時(shí)間(搜索速度)，另外還有諸如多級(jí)地圖精度和地圖分區(qū)域搜索等一些地圖預(yù)處理技術(shù)。

50

50

6.基于模糊邏輯的路徑規(guī)劃模糊方法是在線規(guī)劃中通常采用的一種規(guī)劃方法，包括建模和局部規(guī)劃?；谀：壿嫷臋C(jī)器人路徑規(guī)劃的基本思想：各個(gè)物體的運(yùn)動(dòng)狀態(tài)用模糊集的概念來表達(dá)，每個(gè)物體的隸屬函數(shù)包含該物體當(dāng)前位置、速度大小和速度方向的信息。然后通過模糊綜合評(píng)價(jià)對(duì)各個(gè)方向進(jìn)行綜合考察，得到路徑規(guī)劃結(jié)果。5151

7.基于神經(jīng)網(wǎng)絡(luò)的路徑規(guī)劃Hopfiled神經(jīng)網(wǎng)絡(luò)用于機(jī)器人路徑規(guī)劃的基本思想：障礙物中心處的空間點(diǎn)其碰撞罰函數(shù)有最大值。隨著空間點(diǎn)與障礙物中心距離的增大，其碰撞罰函數(shù)的值逐漸減小，且為單調(diào)連續(xù)變化。在障礙物區(qū)域外的空間點(diǎn)其碰撞罰函數(shù)的值近似為0。因此使整個(gè)能量函數(shù)E最小，便可以使該路徑盡可能遠(yuǎn)離障礙物，不與障礙物相碰，并使路徑的長度盡量短，即得到一條最優(yōu)路徑。5252

8.基于遺傳算法的路徑規(guī)劃遺傳算法用于機(jī)器人路徑規(guī)劃的基本思想：采用柵格法對(duì)機(jī)器人工作空間進(jìn)行劃分，用序號(hào)標(biāo)識(shí)柵格，并以此序號(hào)作為機(jī)器人路徑規(guī)劃參數(shù)編碼，統(tǒng)一確定其個(gè)體長度，隨機(jī)產(chǎn)生障礙物位置及數(shù)目，并在搜索到最優(yōu)路徑后，再在環(huán)境空間中隨機(jī)插入障礙物，模擬環(huán)境變化。但是，規(guī)劃空間柵格法建模還存在缺陷，即若柵格劃分過粗，則規(guī)劃精度較低；若柵格劃分太細(xì)，則數(shù)據(jù)量又會(huì)太大。53539.動(dòng)態(tài)規(guī)劃法動(dòng)態(tài)規(guī)劃法是解決多階段決策優(yōu)化問題的一種數(shù)值方法。動(dòng)態(tài)規(guī)劃算法將復(fù)雜的多變量決策問題進(jìn)行分段決策，從而將其轉(zhuǎn)化為多個(gè)單變量的決策問題。JeromeBarraquand等人以經(jīng)典的動(dòng)態(tài)規(guī)劃方法為基礎(chǔ)，對(duì)全局路徑規(guī)劃問題進(jìn)行了研究。結(jié)論表明，動(dòng)態(tài)規(guī)劃算法非常適合于動(dòng)態(tài)環(huán)境下的路徑規(guī)劃。如何改進(jìn)動(dòng)態(tài)規(guī)劃的算法，以提高計(jì)算效率，是當(dāng)前動(dòng)態(tài)規(guī)劃研究一項(xiàng)重要內(nèi)容。54547.5人工勢(shì)場(chǎng)法7.5.1人工勢(shì)場(chǎng)法基本思想人工勢(shì)場(chǎng)實(shí)際上是對(duì)機(jī)器人運(yùn)行環(huán)境的一種抽象描述。在勢(shì)場(chǎng)中包含斥力和引力極，不希望機(jī)器人進(jìn)入的區(qū)域的障礙物屬于斥力極，子目標(biāo)及建議機(jī)器人進(jìn)入的區(qū)域?yàn)橐O。引力極和斥力極的周圍由勢(shì)函數(shù)產(chǎn)生相應(yīng)的勢(shì)場(chǎng)。機(jī)器人在勢(shì)場(chǎng)中具有一定的抽象勢(shì)能，它的負(fù)梯度方向表達(dá)了機(jī)器人系統(tǒng)所受到抽象力的方向，正是這種抽象力，促使機(jī)器人繞過障礙物，朝目標(biāo)前進(jìn)。557.5人工勢(shì)場(chǎng)法557.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建傳統(tǒng)勢(shì)場(chǎng)法中勢(shì)場(chǎng)的構(gòu)造是應(yīng)用引力與斥力共同對(duì)機(jī)器人產(chǎn)生作用，總的勢(shì)場(chǎng)U可表示為：（7.12）式中：為斥力場(chǎng)；為引力場(chǎng)。勢(shì)場(chǎng)力可表示為：（7.13）式中：為引力；為斥力；為合力，決定了智能機(jī)器人的運(yùn)動(dòng)。

567.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建

56斥力與引力可分別表達(dá)為：（7.14）

（7.15）

57

57在勢(shì)場(chǎng)中智能機(jī)器人的受力圖如下圖所示。5858當(dāng)機(jī)器人到達(dá)目標(biāo)，目標(biāo)點(diǎn)對(duì)智能機(jī)器人的引力等于障礙物對(duì)其產(chǎn)生的斥力時(shí)，。算法也可能會(huì)產(chǎn)生局部極小點(diǎn)，在某個(gè)位置時(shí)，并未到達(dá)目標(biāo)。這時(shí)需要對(duì)算法進(jìn)行改進(jìn)，例如引入其他的量對(duì)機(jī)器人進(jìn)行控制。圖下圖所示，給出了一個(gè)人工勢(shì)場(chǎng)分布示意圖，從圖中可以大致了解機(jī)器人在某個(gè)位置的運(yùn)動(dòng)趨勢(shì)。59當(dāng)機(jī)器人到達(dá)目標(biāo)，目標(biāo)點(diǎn)對(duì)智能機(jī)器人的引力等于障礙物對(duì)其1.斥力場(chǎng)函數(shù)當(dāng)障礙物形狀規(guī)則時(shí)，障礙物的表面由隱函數(shù)來表示，則斥力函數(shù)可表示為

（7.16）式中：為位置增益系數(shù)，是障礙物附近一點(diǎn)。勢(shì)力場(chǎng)的影響范圍局限于和兩表面之間的空間。

601.斥力場(chǎng)函數(shù)

60當(dāng)障礙物形狀不規(guī)則時(shí)，斥力場(chǎng)函數(shù)可表示為

（7.17）

式中：為智能機(jī)器人X與障礙物O之間的最短距離，是一個(gè)常數(shù)，代表障礙物的影響距離。

61當(dāng)障礙物形狀不規(guī)則時(shí)，斥力場(chǎng)函數(shù)可表示為

61相應(yīng)地，將式（7.16）（7.17）代入到式（7.13）可求得斥力：

（7.18）

或

（7.19）

62相應(yīng)地，將式（7.16）（7.17）代入到式（7.13）式中：

（7.20）

（7.21）

63式中：

632.引力場(chǎng)函數(shù)目標(biāo)G的勢(shì)函數(shù)同樣也可以基于距離的概念。目標(biāo)G對(duì)智能機(jī)器人X起吸引作用，而且距離遠(yuǎn)，吸引作用越大，反之就越小。當(dāng)距離為零時(shí)，智能機(jī)器人的勢(shì)能為零，此時(shí)智能機(jī)器人到達(dá)終點(diǎn)。通常引力場(chǎng)函數(shù)可構(gòu)建為：（7.22）式中：kg為位置增益系數(shù)；為智能機(jī)器人X與目標(biāo)點(diǎn)之間的相對(duì)距離。

642.引力場(chǎng)函數(shù)

64

相應(yīng)地，將式（7.22）代入式（7.15）中可得到吸引力為（7.23）式中：吸引力方向指向目標(biāo)點(diǎn)，在智能機(jī)器人到達(dá)目標(biāo)的過程中，這個(gè)力線性地收斂于零。

65

657.5.3人工勢(shì)場(chǎng)法的特點(diǎn)1.優(yōu)點(diǎn)人工勢(shì)場(chǎng)法的優(yōu)點(diǎn)是應(yīng)用人工勢(shì)場(chǎng)法規(guī)劃出來的路徑一般是比較平滑并且安全的，因?yàn)槌饬?chǎng)的作用，智能機(jī)器人總是要遠(yuǎn)離障礙物的勢(shì)場(chǎng)范圍；勢(shì)場(chǎng)法結(jié)構(gòu)簡單、易于實(shí)現(xiàn)，所以在路徑規(guī)劃中被廣泛地采用。2.缺點(diǎn)勢(shì)場(chǎng)法的缺點(diǎn)是存在一個(gè)局部最優(yōu)點(diǎn)問題。為了解決這個(gè)問題，許多學(xué)者進(jìn)行了研究，如Rimon、Shahid和Khosla等。他們期望通過建立統(tǒng)一的勢(shì)能函數(shù)來解決這一問題，但是這就要求障礙物最好是規(guī)則的，否則算法的計(jì)算量很大，有時(shí)甚至是無法計(jì)算的。667.5.3人工勢(shì)場(chǎng)法的特點(diǎn)667.5.4人工勢(shì)場(chǎng)法的改進(jìn)在定義斥力場(chǎng)函數(shù)時(shí)，把智能機(jī)器人與目標(biāo)之間的相對(duì)距離也考慮進(jìn)去，從而建立一個(gè)新的斥力場(chǎng)函數(shù)。修改式（7.16）和式（7.17）如下：

（7.24）

（7.25）677.5.4人工勢(shì)場(chǎng)法的改進(jìn)67式中，為智能機(jī)器人與目標(biāo)點(diǎn)之間的距離，障礙物的影響范圍在距離之內(nèi)，是一個(gè)大于零的任意實(shí)數(shù)。與（7.16）和式（7.17）相比，改進(jìn)的勢(shì)場(chǎng)函數(shù)引入了智能機(jī)器人與目標(biāo)的相對(duì)距離，保證了整個(gè)勢(shì)場(chǎng)僅在目標(biāo)點(diǎn)全局最小。通過分析取值不同時(shí)，勢(shì)場(chǎng)函數(shù)的數(shù)學(xué)特性，證明斥力函數(shù)在目標(biāo)點(diǎn)是可微的，在此不再贅述。

68式中，為智能機(jī)器人與目標(biāo)點(diǎn)之間的距離，障礙物的影7.5.5仿真分析

假定機(jī)器人以不變的速度運(yùn)動(dòng)，仿真環(huán)境選擇Matlab，小車的運(yùn)動(dòng)由合力決定。目標(biāo)點(diǎn)（10，10）(仿真中用三角表示)，起點(diǎn)（0，0）(仿真中用小方框表示)，隨機(jī)產(chǎn)生的障礙物（仿真中用小圓圈表示）。相應(yīng)的參數(shù)選取為：（1）引力增益系數(shù)：2；（2）斥力增益系數(shù)：5；（3）小車運(yùn)動(dòng)的步長：0.5；（4）障礙物影響距離：2。697.5.5仿真分析69

單障礙物的路徑規(guī)劃仿真結(jié)果如下圖所示：

70單障礙物的路徑規(guī)劃仿真結(jié)果如下圖所示：70對(duì)多障礙物的仿真（因FIRA比賽中有5V5比賽，故障礙物選取5個(gè)）。實(shí)驗(yàn)中就不同給定障礙物的條件下，進(jìn)行了大量的仿真。絕大部分情況下，小車均能尋找到通往目標(biāo)點(diǎn)的路徑，并且順利繞開障礙物。說明了人工勢(shì)場(chǎng)法的用于機(jī)器人的路徑規(guī)劃還是可行的。7171

下圖給出了其中幾種不同條件下的路徑規(guī)劃圖。

72下圖給出了其中幾種不同條件下的路徑規(guī)劃圖。72

下圖給出了目標(biāo)點(diǎn)與障礙物較近時(shí)的路徑規(guī)劃情況。圖中可以看出：起初，機(jī)器人能夠完成避障并向目標(biāo)前進(jìn)；當(dāng)機(jī)器人接近目標(biāo)時(shí)，機(jī)器人被推開而達(dá)不到目標(biāo)點(diǎn)的情況，這就是所謂的局部穩(wěn)定，就是指在特殊情況下，由于障礙物的位置因素使得機(jī)器人在路徑中的某一點(diǎn)受力平衡，達(dá)到穩(wěn)定，從而使該點(diǎn)成為勢(shì)場(chǎng)的全局最小點(diǎn)，機(jī)器人陷在該點(diǎn)無法到達(dá)目標(biāo)。73下圖給出了目標(biāo)點(diǎn)與障礙物較近時(shí)的路徑規(guī)劃情況。圖中可以7.4柵格法7.6.1用柵格表示環(huán)境有些文獻(xiàn)中采用正方形柵格表示環(huán)境，每個(gè)正方形柵格有一個(gè)表征值CV，表示在此方法中障礙物對(duì)于機(jī)器人的危險(xiǎn)程度，對(duì)于高CV值的柵格位置，機(jī)器人就要優(yōu)先躲避。CV值按其距車體的距離被事先劃分成若干等級(jí)。每個(gè)等級(jí)對(duì)機(jī)器人的躲避方向會(huì)產(chǎn)生不同的影響。障礙物的位置一旦被確定，則按照一定的衰減的方式賦給障礙物本身及其周圍柵格一定的值，每個(gè)柵格的值代表了該位置有障礙物的可能性。障礙物柵格的初值和遞減速度完全是由路徑的安全性和最優(yōu)性來共同決定。下圖給出一種障礙物的賦值示例，以被檢測(cè)到的障礙物為中心向周圍八個(gè)方向進(jìn)行傳播，障礙物所在的柵格值最大。747.4柵格法7.6.1用柵格表示環(huán)境74

柵格值計(jì)算75柵格值計(jì)算757.6.2基于柵格地圖的路徑搜索

當(dāng)給定起點(diǎn)位置和目標(biāo)位置后，應(yīng)根據(jù)給定的目標(biāo)點(diǎn)位置對(duì)整個(gè)地圖進(jìn)行初始化。確定初始值的各種方法都大致相同：每個(gè)柵格的初始值等于該柵格與目標(biāo)柵格的橫向距離加上該柵格與目標(biāo)柵格縱向距離。由此形成初始地圖。初始地圖與障礙物地圖合起來就成了路徑搜索用的地圖了，在這個(gè)地圖上進(jìn)行路徑的搜索。767.6.2基于柵格地圖的路徑搜索76

傳統(tǒng)的柵格法中，路徑搜索一般是將“起始點(diǎn)柵格”作為參考柵格，從參考柵格的八個(gè)相鄰柵格中選擇值最小的柵格；再將所選柵格作為新的參考柵格，重復(fù)此步驟直到到達(dá)到了“目標(biāo)柵格”。那么為了保證路徑的平滑，要做一定的設(shè)置，即如果有多個(gè)可選柵格時(shí)，選擇使智能機(jī)器人轉(zhuǎn)動(dòng)角度最小的柵格。那么此時(shí)就要記錄智能機(jī)器人的移動(dòng)方向。77777.6.3柵格法的特點(diǎn)通過研究發(fā)現(xiàn)柵格具有簡單、實(shí)用、操作方便的特點(diǎn)，完全能夠滿足使用要求。(1)無需障礙物為規(guī)則障礙物，在動(dòng)態(tài)規(guī)劃中，更加不需要知道障礙物的形狀、大??；(2)無需考慮運(yùn)動(dòng)對(duì)象的運(yùn)動(dòng)軌跡、數(shù)目及形狀；(3)算法實(shí)現(xiàn)簡單，在很多場(chǎng)合都實(shí)用；(4)只要起始點(diǎn)與終點(diǎn)之間存在通路，那么柵格就一定能找到一條路徑從起始點(diǎn)到終點(diǎn)。787.6.3柵格法的特點(diǎn)78同時(shí)也能看到柵格大小的選擇直接影響著控制算法的性能。柵格選得小，環(huán)境分辨率高，但是抗干擾能力弱，環(huán)境信息存儲(chǔ)量大，決策速度慢；柵格選得大，抗干擾能力強(qiáng)，環(huán)境信息存儲(chǔ)量小，決策速度快，但是分辨率下降，在密集障礙物環(huán)境中發(fā)現(xiàn)路徑的能力減弱。所有單用柵格法對(duì)現(xiàn)在的移動(dòng)機(jī)器人的研究已經(jīng)行不通了。79797.7移動(dòng)機(jī)器人的同步定位與地圖

構(gòu)建機(jī)器人構(gòu)建一個(gè)環(huán)境地圖，并同時(shí)運(yùn)用這個(gè)地圖進(jìn)行機(jī)器人定位，稱作同時(shí)定位與建圖(SimultaneousLocallzationandMapPing，SLAM）或并發(fā)定位與建圖(ConcurrentLocalizationandMapping，CLM)。(1)環(huán)境建模(MaPPing)是建立機(jī)器人所工作環(huán)境的各種物體如障礙、路標(biāo)等的準(zhǔn)確的空間位置描述，即空間模型或地圖。(2)定位(Localization)是確定機(jī)器人自身在該工作環(huán)境中的精確位置。精確的環(huán)境模型(地圖)及機(jī)器人定位有助于高效地路徑規(guī)劃和決策，是保證機(jī)器人安全導(dǎo)航的基礎(chǔ)?？梢姡憾ㄎ缓徒▓D是一個(gè)“雞和蛋”的問題，環(huán)境建模需要定位，定位又依賴于環(huán)境地圖。807.7移動(dòng)機(jī)器人的同步定位與地圖

構(gòu)建7.7.1SLAM的基本問題

SLAM問題可以描述為：移動(dòng)機(jī)器人從一個(gè)未知的位置出發(fā)，在不斷運(yùn)動(dòng)過程中根據(jù)自身位姿估計(jì)和傳感器對(duì)環(huán)境的感知構(gòu)建增量式地圖，同時(shí)利用該地圖更新自己的定位。定位與增量式建圖融為一體，而不是獨(dú)立的兩個(gè)階段。817.7.1SLAM的基本問題81作為機(jī)器人導(dǎo)航領(lǐng)域的熱點(diǎn)，SLAM問題的研究主要包括以下幾個(gè)方面：（1）環(huán)境描述，即環(huán)境地圖的表示方法。地圖的表示通?？煞譃?類：柵格表示、幾何特征表示和拓?fù)鋱D表示。（2）環(huán)境信息的獲取。機(jī)器人在環(huán)境中漫游并記錄傳感器的感知數(shù)據(jù)，涉及到機(jī)器人的定位與環(huán)境特征提取問題；（3）環(huán)境信息的表示。機(jī)器人根據(jù)環(huán)境信息更新地圖，這涉及到對(duì)運(yùn)動(dòng)和感知不確定信息的描述和處理。（4）魯棒的SLAM方法。82作為機(jī)器人導(dǎo)航領(lǐng)域的熱點(diǎn)，SLAM問題的研究主要包括以下7.7.2移動(dòng)機(jī)器人SLAM系統(tǒng)模型下圖簡單描述了移動(dòng)機(jī)器人SLAM的系統(tǒng)狀態(tài)。假設(shè)機(jī)器人在未知環(huán)境中移動(dòng)，同時(shí)使用自身攜帶的傳感器探測(cè)外部未知的路標(biāo)信息以及自身的里程信息。表示t時(shí)刻移動(dòng)機(jī)器人的位姿狀態(tài)向量，表示第i個(gè)路標(biāo)的位置狀態(tài)向量，為機(jī)器人從t-1時(shí)刻到t時(shí)刻的輸入控制向量，為t時(shí)刻觀測(cè)向量。

837.7.2移動(dòng)機(jī)器人SLAM系統(tǒng)模型

83

移動(dòng)機(jī)器人SLAM系統(tǒng)狀態(tài)圖

84移動(dòng)機(jī)器人SLAM系統(tǒng)狀態(tài)圖84

若把t時(shí)刻移動(dòng)機(jī)器人SLAM系統(tǒng)的狀態(tài)記為，狀態(tài)包含了t時(shí)刻機(jī)器人的位姿(即機(jī)器人的位置和方向)和路標(biāo)的位置。從概率學(xué)來看，假定移動(dòng)機(jī)器人SLAM系統(tǒng)是先將機(jī)器人運(yùn)動(dòng)到當(dāng)前位置，然后進(jìn)行觀測(cè)，則系統(tǒng)當(dāng)前狀態(tài)與之前的系統(tǒng)狀態(tài)、觀測(cè)信息以及輸入有關(guān)，即。假設(shè)系統(tǒng)當(dāng)前的狀態(tài)僅與前一時(shí)刻的系統(tǒng)狀態(tài)和當(dāng)前的輸入有關(guān)，即前一時(shí)刻的系統(tǒng)狀態(tài)已經(jīng)包含了之前的系統(tǒng)狀態(tài)、觀測(cè)信息和輸入，則當(dāng)前系統(tǒng)狀態(tài)的分布概率為：（7.26）在此系統(tǒng)狀態(tài)估計(jì)上獲得的觀測(cè)信息的估計(jì)為（7.27）

85若把t時(shí)刻移動(dòng)機(jī)器人SLAM系統(tǒng)的狀態(tài)記為，狀態(tài)包可以看出，公式(7.26)描述了系統(tǒng)狀態(tài)轉(zhuǎn)移概率，它與公式(7.27)共同組成了移動(dòng)機(jī)器人和環(huán)境的一個(gè)隱馬爾科夫模型(HiddenMarkovModel，HMM)或動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)(DynamicBayesNetwork，DBN)，即移動(dòng)機(jī)器人SLAM問題模型如下圖所示。86可以看出，公式(7.26)描述了系統(tǒng)狀態(tài)轉(zhuǎn)移概率，它與公7.7.3移動(dòng)機(jī)器人SLAM解決方法

1．SLAM解決思想對(duì)于SLAM問題，根據(jù)之前的移動(dòng)機(jī)器人位姿、觀測(cè)信息以及控制輸入信息可以求得t時(shí)刻機(jī)器人位姿x和環(huán)境中路標(biāo)位置m的聯(lián)合后驗(yàn)概率

(7.28)假定環(huán)境服從馬爾科夫的前提，SLAM問題可分為預(yù)測(cè)、更新兩步遞歸執(zhí)行。877.7.3移動(dòng)機(jī)器人SLAM解決方法87預(yù)測(cè)：依據(jù)前一時(shí)刻狀態(tài)的后驗(yàn)信度，也即

，結(jié)合運(yùn)動(dòng)模型來預(yù)測(cè)當(dāng)前t時(shí)刻狀態(tài)

的先驗(yàn)信度。

(7.29)式中：為運(yùn)動(dòng)模型，為后驗(yàn)信度。

88預(yù)測(cè)：依據(jù)前一時(shí)刻狀態(tài)的后驗(yàn)信度，也即

更新：利用感知模型，結(jié)合當(dāng)前的感知測(cè)量信息來更新當(dāng)前t時(shí)刻狀態(tài)的后驗(yàn)概率分布。

(7.30)

為標(biāo)準(zhǔn)化因子，為觀測(cè)模型，為先驗(yàn)信度。

89

89

一般而言，SLAM由傳感數(shù)據(jù)的獲取、數(shù)據(jù)關(guān)聯(lián)、定位和地圖構(gòu)建等環(huán)節(jié)構(gòu)成，其典型流程如右圖。

圖中，表示傳感器測(cè)量所獲取的數(shù)據(jù)，表示第k-1時(shí)刻的局部地圖，表示k時(shí)刻機(jī)器人的位姿。傳感數(shù)據(jù)獲取包括移動(dòng)機(jī)器人本身的數(shù)據(jù)采集和環(huán)境的數(shù)據(jù)。

90一般而言，SLAM由傳感數(shù)據(jù)的獲取、數(shù)據(jù)關(guān)數(shù)據(jù)關(guān)聯(lián)是SLAM的關(guān)鍵步驟，用于當(dāng)前特征與已有特征的匹配。對(duì)于數(shù)據(jù)關(guān)聯(lián)中沒有關(guān)聯(lián)上的特征，可先加人到臨時(shí)特征存儲(chǔ)區(qū)，并對(duì)下一次觀測(cè)進(jìn)行預(yù)測(cè)，在下一次觀測(cè)中把觀測(cè)特征與其關(guān)聯(lián)。如果一個(gè)特征連續(xù)兩次沒有關(guān)聯(lián)成功，則作為假觀測(cè)從臨時(shí)特征存儲(chǔ)區(qū)剔除。如果一個(gè)特征第一次關(guān)聯(lián)不成功，但第二次關(guān)聯(lián)成功，則將該特征加人狀態(tài)向量，同時(shí)對(duì)狀態(tài)進(jìn)行擴(kuò)維。定位和地圖構(gòu)建是一個(gè)相互交互的過程，定位的結(jié)果用于地圖構(gòu)建，而已經(jīng)構(gòu)建的地圖又用于機(jī)器人的定位。事實(shí)上，目前移動(dòng)機(jī)器人SLAM經(jīng)典的算法主要包括擴(kuò)展卡爾曼濾波器、最大似然估計(jì)、粒子濾波器以及Markov定位等，而其中擴(kuò)展卡爾曼濾波器EKF和粒子濾波的方法是來自于上述Bayes濾波器的估計(jì)狀態(tài)后驗(yàn)概率分布的思想的。91數(shù)據(jù)關(guān)聯(lián)是SLAM的關(guān)鍵步驟，用于當(dāng)前特征與已有特征的匹

2.卡爾曼濾波卡爾曼濾波在數(shù)學(xué)上是一種統(tǒng)計(jì)估算方法，通過處理一系列帶有誤差的實(shí)際量測(cè)數(shù)據(jù)而得到的物理參數(shù)的最佳估算，其思想是利用前一時(shí)刻的數(shù)據(jù)和誤差信息來估計(jì)當(dāng)前時(shí)刻的數(shù)據(jù)。卡爾曼濾波器用反饋控制的方法估計(jì)過程狀態(tài)：濾波器估計(jì)過程某一時(shí)刻的狀態(tài)，然后以(含噪聲的)測(cè)量變量的方式獲得反饋。因此卡爾曼濾波器可分為兩個(gè)部分：預(yù)測(cè)和更新。預(yù)測(cè)負(fù)責(zé)及時(shí)向前推算當(dāng)前狀態(tài)變量和誤差協(xié)方差估計(jì)的值，以便為下一個(gè)時(shí)間狀態(tài)構(gòu)造先驗(yàn)估計(jì)。更新方程負(fù)責(zé)反饋，也就是說，它將先驗(yàn)估計(jì)和新的測(cè)量變量結(jié)合以構(gòu)造改進(jìn)的后驗(yàn)估計(jì)。

922.卡爾曼濾波923.粒子濾波粒子濾波器是一種典型的采用蒙特卡羅數(shù)值模擬求解貝葉斯濾波問題的方法，其基本思想是利用一組帶有相關(guān)權(quán)值的隨機(jī)樣本，用這些樣本的估計(jì)來表示系統(tǒng)狀態(tài)的后驗(yàn)概率密度。當(dāng)樣本數(shù)非常大時(shí)，這種估計(jì)將等同于后驗(yàn)概率密度。粒子濾波通過非參數(shù)化的蒙特卡羅模擬方法來實(shí)現(xiàn)遞推Bayes濾波，適用于任何能用狀態(tài)空間模型表示的非線性系統(tǒng)，以及傳統(tǒng)卡爾曼濾波無法表示的非線性系統(tǒng)，精度可以逼進(jìn)最優(yōu)估計(jì)。933.粒子濾波937.7.4SLAM的難點(diǎn)和技術(shù)關(guān)鍵l.不確定性和計(jì)算量大的問題無論是感知外部環(huán)境的或感知機(jī)器人運(yùn)動(dòng)的傳感器測(cè)量都帶有不確定性，即測(cè)量噪聲。機(jī)器人的運(yùn)動(dòng)控制也同樣帶有不確定性。各種測(cè)量誤差之間并非完全獨(dú)立，因此SLAM對(duì)地圖的估計(jì)和對(duì)機(jī)器人的位姿估計(jì)都有很強(qiáng)的不確定性。為了處理不確定性，無論是擴(kuò)展卡爾曼濾波方法或是粒子濾波方法，都需要很大的計(jì)算量。擴(kuò)展卡爾曼濾波SLAM方法的計(jì)算量主要在于地圖的更新計(jì)算，即協(xié)方差矩陣的計(jì)算；而粒子濾波SLAM方法的計(jì)算量是隨著粒子數(shù)增多而增大。如何減少SLAM過程的計(jì)算量是SLAM研究的重要課題，對(duì)于動(dòng)態(tài)環(huán)境下的SLAM技術(shù)更是緊迫的問題。947.7.4SLAM的難點(diǎn)和技術(shù)關(guān)鍵942.數(shù)據(jù)關(guān)聯(lián)問題數(shù)據(jù)關(guān)聯(lián)問題也稱一致性問題，是指建立在不同時(shí)間、不同地段獲得的傳感器測(cè)量之間、傳感器測(cè)量與地圖特征之間或者地圖特征之間的對(duì)應(yīng)關(guān)系，以確定它們是否源于環(huán)境中同一物理實(shí)體的過程。數(shù)據(jù)關(guān)聯(lián)問題是SLAM本身面臨的挑戰(zhàn)之一，其正確與否對(duì)于SLAM的狀態(tài)估計(jì)至關(guān)重要。在動(dòng)態(tài)環(huán)境下，由于動(dòng)態(tài)目標(biāo)的影響，數(shù)據(jù)關(guān)聯(lián)問題就顯得更為困難和重要。952.數(shù)據(jù)關(guān)聯(lián)問題95

3.動(dòng)態(tài)目標(biāo)檢側(cè)與處理問題移動(dòng)機(jī)器人成功構(gòu)建地圖必須具備識(shí)別動(dòng)態(tài)障礙物和靜態(tài)障礙物的能力。目前相關(guān)的研究大部分也就如何檢測(cè)動(dòng)態(tài)目標(biāo)而展開，但是相關(guān)方法都有如下問題：檢測(cè)的準(zhǔn)確性，闡值難以確定，動(dòng)態(tài)目標(biāo)在成功檢測(cè)出來后，如何在SLAM過程中進(jìn)行處理都是難點(diǎn)之一。

9696智能機(jī)器人原理與實(shí)踐97智能機(jī)器人原理與實(shí)踐1

第7章智能機(jī)器人自主導(dǎo)航與路

徑規(guī)劃

98

第7章智能機(jī)器人自主導(dǎo)航與路

7.1概述

導(dǎo)航，最初是指對(duì)航海的船舶抵達(dá)目的地進(jìn)行的導(dǎo)引過程。這一術(shù)語和自主性相結(jié)合，已成為智能機(jī)器人研究的核心和熱點(diǎn)。Leonard和Durrant-Whyte將移動(dòng)機(jī)器人導(dǎo)航定義為三個(gè)子問題：

（1）“WhereamI？”——環(huán)境認(rèn)知與機(jī)器人定位；

（2）“WhereamIgoing？”——目標(biāo)識(shí)別；

（3）“HowdoIgetthere？”——路徑規(guī)劃。

為完成導(dǎo)航，機(jī)器人需要依靠自身傳感系統(tǒng)對(duì)內(nèi)部姿態(tài)和外部環(huán)境信息進(jìn)行感知，通過對(duì)環(huán)境空間信息的存儲(chǔ)、識(shí)別、搜索等操作尋找最優(yōu)或近似最優(yōu)的無碰撞路徑并實(shí)現(xiàn)安全運(yùn)動(dòng)。997.1概述導(dǎo)航，最初是指對(duì)航海的船舶抵達(dá)目的地進(jìn)行的導(dǎo)7.1.1導(dǎo)航系統(tǒng)分類

對(duì)于不同的室內(nèi)與室外環(huán)境、結(jié)構(gòu)化與非結(jié)構(gòu)化環(huán)境，機(jī)器人完成準(zhǔn)確的自身定位后，常用的導(dǎo)航方式主要有磁導(dǎo)航、慣性導(dǎo)航、視覺導(dǎo)航、衛(wèi)星導(dǎo)航等。1.磁導(dǎo)航

磁導(dǎo)航是在路徑上連續(xù)埋設(shè)多條引導(dǎo)電纜，分別流過不同頻率的電流，通過感應(yīng)線圈對(duì)電流的檢測(cè)來感知路徑信息。

2.慣性導(dǎo)航

慣性導(dǎo)航是利用陀螺儀和加速度計(jì)等慣性傳感器測(cè)量移動(dòng)機(jī)器人的方位角和加速率，從而推知機(jī)器人當(dāng)前位置和下一步的目的地。3.視覺導(dǎo)航

依據(jù)環(huán)境空間的描述方式，可將移動(dòng)機(jī)器人的視覺導(dǎo)航方式劃分為三類。1007.1.1導(dǎo)航系統(tǒng)分類4

（1）基于地圖的導(dǎo)航：是完全依靠在移動(dòng)機(jī)器人內(nèi)部預(yù)先保存好的關(guān)于環(huán)境的幾何模型、拓?fù)涞貓D等比較完整的信息，在事先規(guī)劃出的全局路線基礎(chǔ)上，應(yīng)用路徑跟蹤和避障技術(shù)來實(shí)現(xiàn)的；

（2）基于創(chuàng)建地圖的導(dǎo)航：是利用各種傳感器來創(chuàng)建關(guān)于當(dāng)前環(huán)境的幾何模型或拓?fù)淠Ｐ偷貓D，然后利用這些模型來實(shí)現(xiàn)導(dǎo)航；

（3）無地圖的導(dǎo)航：是在環(huán)境信息完全未知的情況下，可通過攝像機(jī)或其他傳感器對(duì)周圍環(huán)境進(jìn)行探測(cè)，利用對(duì)探測(cè)的物體進(jìn)行識(shí)別或跟蹤來實(shí)現(xiàn)導(dǎo)航。4．衛(wèi)星導(dǎo)航

移動(dòng)機(jī)器人通過安裝衛(wèi)星信號(hào)接收裝置，可以實(shí)現(xiàn)自身定位，無論其在室內(nèi)還是室外。101（1）基于地圖的導(dǎo)航：是完全依靠在移動(dòng)機(jī)器人內(nèi)部預(yù)先保存

7.1.2導(dǎo)航系統(tǒng)體系結(jié)構(gòu)

智能機(jī)器人的導(dǎo)航系統(tǒng)是一個(gè)自主式智能系統(tǒng)，其主要任務(wù)是如何把感知、規(guī)劃、決策和行動(dòng)等模塊有機(jī)地結(jié)合起來。下圖給出了一種智能機(jī)器人自主導(dǎo)航系統(tǒng)的控制結(jié)構(gòu)。1027.1.2導(dǎo)航系統(tǒng)體系結(jié)構(gòu)6

10377.2環(huán)境地圖的表示

構(gòu)造地圖的目的是用于絕對(duì)坐標(biāo)系下的位姿估計(jì)。地圖的表示方法通常有四種：拓?fù)鋱D、特征圖、網(wǎng)格圖及直接表征法(Appearancebasedmethods)。不同方法具有各自的特點(diǎn)和適用范圍，其中特征圖和網(wǎng)格圖應(yīng)用較為普遍。1047.2環(huán)境地圖的表示8

7.2.1拓?fù)鋱D

1）基本思想

地鐵、公交路線圖均是典型的拓?fù)涞貓D實(shí)例，其中?？空緸楣?jié)點(diǎn)，節(jié)點(diǎn)間的通道為邊。在一般的辦公環(huán)境中，拓?fù)鋯卧凶呃群头块g等，而打印機(jī)、桌椅等則是功能單元。連接器用于連接對(duì)應(yīng)的位置，如門、樓梯、電梯等。2）特點(diǎn)拓?fù)鋱D把環(huán)境建模為一張線圖表示，忽略了具體的幾何特征信息，不必精確表示不同節(jié)點(diǎn)間的地理位置關(guān)系，圖形抽象，表示方便。1057.2.1拓?fù)鋱D9

7.2.2特征圖1）基本思想

結(jié)構(gòu)化環(huán)境中，最常見的特征是直線段、角、邊等。

這些特征可用它們的顏色、長度、寬度、位置等參數(shù)表示。

基于特征的地圖一般用式（7.1）的特征集合表示：其中

是一個(gè)特征(邊、線角等)，n是地圖中的特征總數(shù)。、

（7.1）

1067.2.2特征圖其中是一個(gè)特征(邊、線角等)

2）特點(diǎn)

特征法定位準(zhǔn)確，模型易于由計(jì)算機(jī)描述和表示，參數(shù)化特征也適用于路徑規(guī)劃和軌跡控制，但特征法需要特征提取等預(yù)處理過程，對(duì)傳感器噪聲比較敏感，只適于高度結(jié)構(gòu)化環(huán)境。1072）特點(diǎn)117.2.3網(wǎng)格圖1）基本思想

網(wǎng)格圖把機(jī)器人的工作空間劃分成網(wǎng)狀結(jié)構(gòu)，網(wǎng)格中的每一單元代表環(huán)境的一部分，每一個(gè)單元都分配了一個(gè)概率值，表示該單元被障礙物占據(jù)的可能性大小。2）特點(diǎn)

網(wǎng)格法是一種近似描述，易于創(chuàng)建和維護(hù)，對(duì)某個(gè)網(wǎng)格的感知信息可直接與環(huán)境中某個(gè)區(qū)域?qū)?yīng)，機(jī)器人對(duì)所測(cè)得的障礙物具體形狀不太敏感，特別適于處理超聲測(cè)量數(shù)據(jù)。但當(dāng)在大型環(huán)境中或網(wǎng)格單元?jiǎng)澐直容^細(xì)時(shí)，網(wǎng)格法計(jì)算量迅速增長，需要大量內(nèi)存單元，使計(jì)算機(jī)的實(shí)時(shí)處理變得很困難。1087.2.3網(wǎng)格圖127.2.4直接表征法1）基本思想

通過記錄來自不同位置及方向的環(huán)境外觀感知數(shù)據(jù)，這些圖像中包括了某些坐標(biāo)、幾何特征或符號(hào)信息，利用這些數(shù)據(jù)作為在這些位置處的環(huán)境特征描述。

直接表征法與識(shí)別拓?fù)湮恢盟捎玫姆椒ㄔ砩鲜且粯拥?，差別僅在于該法試圖從所獲取的傳感器數(shù)據(jù)中創(chuàng)建一個(gè)函數(shù)關(guān)系以便更精確地確定機(jī)器人的位姿。2）特點(diǎn)

直接表征法數(shù)據(jù)存貯量大，環(huán)境噪聲干擾嚴(yán)重，特征數(shù)據(jù)的提取與匹配困難，其應(yīng)用受到一定限制。1097.2.4直接表征法137.3定位

定位是確定機(jī)器人在其作業(yè)環(huán)境中所處位置。機(jī)器人可以利用先驗(yàn)環(huán)境地圖信息、位姿的當(dāng)前估計(jì)以及傳感器的觀測(cè)值等輸入信息，經(jīng)過一定處理變換，獲得更準(zhǔn)確的當(dāng)前位姿。

移動(dòng)機(jī)器人定位方式有很多種，常用的可以采用里程計(jì)、攝像機(jī)、激光雷達(dá)、聲納、速度或加速度計(jì)等。

從方法上來分，移動(dòng)機(jī)器人定位可分為相對(duì)定位和絕對(duì)定位兩種。1107.3定位147.3.1相對(duì)定位

相對(duì)定位又稱為局部位置跟蹤，要求機(jī)器人在已知初始位置的條件下通過測(cè)量機(jī)器人相對(duì)于初始位置的距離和方向來確定當(dāng)前位置，通常也稱航跡推算法。

相對(duì)定位只適于短時(shí)短距離運(yùn)動(dòng)的位姿估計(jì)，長時(shí)間運(yùn)動(dòng)時(shí)必須應(yīng)用其它的傳感器配合相關(guān)的定位算法進(jìn)行校正。1117.3.1相對(duì)定位151.里程計(jì)法里程計(jì)法是移動(dòng)機(jī)器人定位技術(shù)中廣泛采用的方法之一。在移動(dòng)機(jī)器人的車輪上安裝光電編碼器，通過編碼器記錄的車輪轉(zhuǎn)動(dòng)圈數(shù)來計(jì)算機(jī)器人的位移和偏轉(zhuǎn)角度。里程計(jì)法定位過程中會(huì)產(chǎn)生兩種誤差。1121.里程計(jì)法161）系統(tǒng)誤差 系統(tǒng)誤差在很長的時(shí)間內(nèi)不會(huì)改變，和機(jī)器人導(dǎo)航的外界環(huán)境并沒有關(guān)系，主要由下列因素引起：（1）驅(qū)動(dòng)輪直徑不等；（2）驅(qū)動(dòng)輪實(shí)際直徑的均值和名義直徑不等；（3）驅(qū)動(dòng)輪軸心不重合；（4）驅(qū)動(dòng)輪間輪距長度不確定；（5）有限的編碼器測(cè)量精度；（6）有限的編碼器采樣頻率。1131）系統(tǒng)誤差 172）非系統(tǒng)誤差非系統(tǒng)誤差是在機(jī)器人和外界環(huán)境接觸過程中，由于外界環(huán)境不可預(yù)料特性引起的。主要誤差來源如下：（1）輪子打滑；（2）地面不平；（3）地面有無法預(yù)料的物體（例如石塊）；（4）外力作用和內(nèi)力作用；（5）驅(qū)動(dòng)輪和地板是面接觸而不是點(diǎn)接觸。1142）非系統(tǒng)誤差18

對(duì)于機(jī)器人定位來說，非系統(tǒng)誤差是異常嚴(yán)重的問題，因?yàn)樗鼰o法預(yù)測(cè)并導(dǎo)致嚴(yán)重的方向誤差。非系統(tǒng)誤差包括方向誤差和位置誤差?？紤]機(jī)器人的定位誤差時(shí)，方向誤差是主要的誤差源。機(jī)器人導(dǎo)航過程中小的方向誤差會(huì)導(dǎo)致嚴(yán)重的位置誤差。輪子打滑和地面不平都能導(dǎo)致嚴(yán)重的方向誤差。在室內(nèi)環(huán)境中，輪子打滑對(duì)機(jī)器人定位精度的影響要比地面不平對(duì)定位精度影響要大，因?yàn)檩喿哟蚧l(fā)生的頻率更高。115193）誤差補(bǔ)償機(jī)器人定位過程中，需要利用外界的傳感器信息補(bǔ)償誤差。因此利用外界傳感器定位機(jī)器人時(shí)，主要任務(wù)在于如何提取導(dǎo)航環(huán)境的特征并和環(huán)境地圖進(jìn)行匹配。在室內(nèi)環(huán)境中，墻壁、走廊、拐角、門等特征被廣泛地用于機(jī)器人的定位研究。116202.慣性導(dǎo)航定位法慣性導(dǎo)航定位法是一種使用慣性導(dǎo)航傳感器定位的方法。它通常用陀螺儀來測(cè)量機(jī)器人的角速度，用加速度計(jì)測(cè)量機(jī)器人的加速度。對(duì)測(cè)量結(jié)果進(jìn)行一次和二次積分即可得到機(jī)器人偏移的角度和位移，進(jìn)而得出機(jī)器人當(dāng)前的位置和姿態(tài)。117217.3.2絕對(duì)定位

絕對(duì)定位又稱為全局定位，要求機(jī)器人在未知初始位置的情況下確定自己的位置。主要采用導(dǎo)航信標(biāo)、主動(dòng)或被動(dòng)標(biāo)識(shí)、地圖匹配、衛(wèi)星導(dǎo)航技術(shù)或概率方法進(jìn)行定位，定位精度較高。這幾種方法中，信標(biāo)或標(biāo)識(shí)牌的建設(shè)和維護(hù)成本較高，地圖匹配技術(shù)處理速度慢，GPS只能用于室外，目前精度還很差，絕對(duì)定位的位置計(jì)算方法包括三視角法、三視距法、模型匹配算法等。1187.3.2絕對(duì)定位221.主動(dòng)燈塔法主動(dòng)燈塔是可以很可靠地被檢測(cè)到的信號(hào)發(fā)射源，將該信號(hào)進(jìn)行最少的處理就可以提供精確的定位信息。2.路標(biāo)導(dǎo)航定位法路標(biāo)導(dǎo)航定位法是利用環(huán)境中的路標(biāo)，給移動(dòng)機(jī)器人提供位置信息。路標(biāo)分為人工路標(biāo)和自然路標(biāo)。3.地圖匹配法基于地圖的定位方法稱為地圖匹配法。機(jī)器人運(yùn)用各種傳感器(如超聲波傳感器、激光測(cè)距儀、視覺系統(tǒng)等)探測(cè)環(huán)境來創(chuàng)建它所處的局部環(huán)境地圖，然后將此局部地圖與存儲(chǔ)在機(jī)器人中的己知的全局地圖進(jìn)行匹配。如果匹配成功，機(jī)器人就計(jì)算出自身在該環(huán)境中的位置。1191.主動(dòng)燈塔法23

4.GPS定位GPS是適用于室外移動(dòng)機(jī)器人的一種全局定位系統(tǒng)，它是一種以空間衛(wèi)星為基礎(chǔ)的高精度導(dǎo)航與定位系統(tǒng)，是由美國國防部批準(zhǔn)研制，為海、陸、空三軍服務(wù)的一種新的軍用衛(wèi)星導(dǎo)航系統(tǒng)，該系統(tǒng)由三大部分構(gòu)成：GPS衛(wèi)星星座(空間部分)，地面監(jiān)控部分(控制部分)和GPS信號(hào)接收機(jī)(用戶部分)。GPS系統(tǒng)能夠?qū)嵤┤蛐浴⑷旌颉?shí)時(shí)連續(xù)的三維導(dǎo)航定位服務(wù)。120247.3.3基于概率的絕對(duì)定位

概率定位中最重要的馬爾可夫定位和蒙特卡羅定位。馬爾可夫定位和蒙特卡羅定位不僅能夠?qū)崿F(xiàn)全局定位和局部位置跟蹤，而且能夠解決機(jī)器人的“綁架”問題。機(jī)器人“綁架”問題是指，由于機(jī)器人容易與外界發(fā)生碰撞而使機(jī)器人在不知情(里程計(jì)沒有記錄)的情況下發(fā)生移動(dòng)。1217.3.3基于概率的絕對(duì)定位25

1.馬爾可夫定位（MarkovLocalization，ML）

爾可夫定位的基本思想是：機(jī)器人不知道它的確切位置，而是知道它可能位置的信度（Belief，即機(jī)器人在整個(gè)位置空間的概率分布，信度值之和為1）。馬爾可夫定位的關(guān)鍵之處在于信度值的計(jì)算。當(dāng)機(jī)器人收到外界傳感器信息或者利用編碼器獲得機(jī)器人移動(dòng)信息時(shí)，基于馬爾可夫假設(shè)和貝葉斯規(guī)則，每個(gè)柵格的信度值被更新。

12226

根據(jù)初始狀態(tài)概率分布和觀測(cè)數(shù)據(jù)估計(jì)系統(tǒng)的當(dāng)前狀態(tài)，其中表示機(jī)器人的位姿(由位置和方向組成)。從統(tǒng)計(jì)學(xué)的觀點(diǎn)看，的估計(jì)是一個(gè)貝葉斯濾波問題，可以通過估計(jì)后驗(yàn)密度分布來實(shí)現(xiàn)。貝葉斯濾波器假設(shè)系統(tǒng)是一個(gè)馬爾可夫過程，可以通過以下2步算得。

123

271)預(yù)測(cè)通過運(yùn)動(dòng)模型預(yù)測(cè)系統(tǒng)在下一時(shí)刻的狀態(tài)，即通過如下公式計(jì)算先驗(yàn)概率密度：

（7.2）式中：稱為系統(tǒng)的運(yùn)動(dòng)模型(狀態(tài)轉(zhuǎn)移先驗(yàn)密度)。

1241)預(yù)測(cè)

282)更新通過觀測(cè)模型利用新的觀測(cè)信息更新系統(tǒng)的狀態(tài)，即通過如下公式計(jì)算后驗(yàn)概率密度：（7.3）式中：稱為系統(tǒng)的觀測(cè)模型(觀測(cè)密度)。當(dāng)機(jī)器人獲得編碼器信息或者利用外界傳感器感知環(huán)境后，馬爾可夫定位算法必須對(duì)所有的柵格進(jìn)行計(jì)算，因此需要大量的計(jì)算資源和內(nèi)存，導(dǎo)致定位處理的實(shí)時(shí)性很差。

1252)更新

292.蒙特卡羅定位（Monte-CarloLocalization，MCL）基于馬爾可夫定位方法，Dellaert等人提出了蒙特卡羅定位方法。MCL也稱為粒子濾波（ParticleFilter）。MCL的主要思想是用N個(gè)帶有權(quán)重的離散采樣

來表示后驗(yàn)概率密度。其中是機(jī)器人在t時(shí)刻的一個(gè)可能狀態(tài)；是一個(gè)非負(fù)的參數(shù)稱為權(quán)重，表示t時(shí)刻機(jī)器人的狀態(tài)為的概率也就是

，且

。

1262.蒙特卡羅定位（Monte-CarloLocaliMCL包括4個(gè)階段：初始化，采樣階段，權(quán)重歸一化和輸出階段。采樣階段是MCL的核心，它包括重采樣、狀態(tài)轉(zhuǎn)移和權(quán)重計(jì)算3步。實(shí)際上MCL是按照提議密度分布抽取采樣，然后利用權(quán)重來補(bǔ)償提議密度分布與后驗(yàn)密度分布之間的差距。當(dāng)機(jī)器人發(fā)生“綁架”時(shí)，要估計(jì)的后驗(yàn)密度與提議密度分布的錯(cuò)位很大，在取值較大區(qū)域的采樣數(shù)很少，需要大量的采樣才能較好地估計(jì)后驗(yàn)密度。

127MCL包括4個(gè)階段：初始化，采樣階段，權(quán)重歸一化和輸出階3.卡爾曼濾波定位（KalmanFilter，KF）kalman濾波器是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法?；舅枷胧遣捎眯盘?hào)和噪聲空間狀態(tài)模型，結(jié)合當(dāng)前時(shí)刻的觀測(cè)值和前一時(shí)刻的估計(jì)值來更新對(duì)狀態(tài)變量的估計(jì)，從而得到當(dāng)前時(shí)刻的估計(jì)值。對(duì)于非線性估計(jì)問題，可以通過線性近似去解決。相應(yīng)的方法有EKF(ExtendedKalmanFilter)、UKF(UnscentedKalmanFilter)等。1283.卡爾曼濾波定位（KalmanFilter，KFkalman濾波器通過預(yù)測(cè)方程和測(cè)量方程對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)，利用遞推的方式尋找最小均方誤差下的的估計(jì)值。kalman濾波的數(shù)學(xué)模型為：狀態(tài)方程為：

（7.4）測(cè)量方程為：(7.5）

其中，是k時(shí)刻時(shí)系統(tǒng)的狀態(tài)，A是k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，是k時(shí)刻的測(cè)量值，H是觀測(cè)矩陣，為系統(tǒng)過程噪聲，為系統(tǒng)測(cè)量噪聲，假設(shè)為高斯白噪聲。

129kalman濾波器通過預(yù)測(cè)方程和測(cè)量方程對(duì)系統(tǒng)狀態(tài)進(jìn)如果不考慮觀測(cè)噪聲和輸入信號(hào)時(shí)，則k時(shí)刻的觀測(cè)值和已知的最有狀態(tài)估計(jì)值，可通過一下方程進(jìn)行求解

最優(yōu)估計(jì)值。狀態(tài)預(yù)測(cè)方程：（7.6）預(yù)測(cè)狀態(tài)下的協(xié)方差方程：（7.7）濾波器增益矩陣：（7.8）

130如果不考慮觀測(cè)噪聲和輸入信號(hào)時(shí)，則k時(shí)刻的觀測(cè)值和已狀態(tài)最優(yōu)化估計(jì)方程：（7.9）狀態(tài)最優(yōu)化估計(jì)的協(xié)方差方程：（7.10）通過kalman濾波器的公式可以看出，只要給定了和，就可以根據(jù)k時(shí)刻的觀測(cè)值，就可以通過遞推計(jì)算得出k時(shí)刻的狀態(tài)估計(jì)。

131狀態(tài)最優(yōu)化估計(jì)方程：

35下圖給出了卡爾曼濾波根據(jù)所有傳感器提供的信息，實(shí)現(xiàn)高效信息融合的一般方案。132下圖給出了卡爾曼濾波根據(jù)所有傳感器提供的信息，實(shí)現(xiàn)高kalman濾波器已經(jīng)廣泛應(yīng)用在了各個(gè)方面，比如機(jī)器人的SLAM問題，雷達(dá)系統(tǒng)的跟蹤的等等。下圖描述了卡爾曼濾波器的機(jī)器人定位架構(gòu)。133kalman濾波器已經(jīng)廣泛應(yīng)用在了各個(gè)方面，比如機(jī)器人的(1)位置預(yù)測(cè)：基于帶有高斯誤差的運(yùn)動(dòng)系統(tǒng)模型，機(jī)器人根據(jù)編碼器數(shù)據(jù)，進(jìn)行位置預(yù)測(cè)。(2)傳感器測(cè)量：機(jī)器人收集實(shí)際的傳感器數(shù)據(jù)，提取合適的環(huán)境特征，產(chǎn)生一個(gè)實(shí)際的位置。(3)匹配更新：機(jī)器人要在實(shí)際提取的特征和測(cè)量預(yù)測(cè)的期望特征之間，辨識(shí)最佳的信息。卡爾曼濾波器可以將所有這些匹配所提供的信息融合，遞歸估計(jì)更新機(jī)器人的狀態(tài)。134387.4路徑規(guī)劃7.4.1路徑規(guī)劃分類路徑規(guī)劃本身可以分成不同的層次，從不同的方面有不同的劃分。