智能機(jī)器人原理與應(yīng)用 課件 第7章 智能機(jī)器人自主導(dǎo)航與路徑規(guī)劃

北京信息科技大學(xué)自動(dòng)化學(xué)院智能機(jī)器人原理與應(yīng)用“智能檢測(cè)技術(shù)與模式識(shí)別”研究所北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》第七章智能機(jī)器人自主導(dǎo)航與路徑規(guī)劃

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

am

I?———環(huán)境認(rèn)知與機(jī)器人定位。(2)Where

am

I

going?———目標(biāo)識(shí)別。(3)How

do

I

get

there?———路徑規(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)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航對(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)航等.7.1.1

導(dǎo)航系統(tǒng)分類1.磁導(dǎo)航磁導(dǎo)航是在路徑上連續(xù)埋設(shè)多條引導(dǎo)電纜,分別流過不同頻率的電流,通過感應(yīng)線圈對(duì)電流的檢測(cè)來感知路徑信息。磁導(dǎo)航技術(shù)雖然簡(jiǎn)單實(shí)用,但成本高,傳感器發(fā)射和反射裝置的安裝復(fù)雜,改造和維護(hù)相對(duì)困難。2.慣性導(dǎo)航慣性導(dǎo)航是利用陀螺儀和加速度計(jì)等慣性傳感器測(cè)量智能機(jī)器人的方位角和加速率,從而推知機(jī)器人的當(dāng)前位置和下一步的目的地。由于車輪與地面存在打滑等現(xiàn)象,隨著機(jī)器人航程的增長(zhǎng),任何小的誤差經(jīng)過累計(jì)都會(huì)無限增加,定位的精度就會(huì)下降。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航7.1.1

導(dǎo)航系統(tǒng)分類3.視覺導(dǎo)航在視覺導(dǎo)航系統(tǒng)中,視覺傳感器可以是攝像頭、激光雷達(dá)等環(huán)境感知傳感器,主要完成運(yùn)行環(huán)境中障礙和特征檢測(cè)及特征辨識(shí)的功能。依據(jù)環(huán)境空間的描述方式,可將智能機(jī)器人的視覺導(dǎo)航方式劃分為3類。(1)基于地圖的導(dǎo)航(map-basednavigaion):是完全依靠在智能機(jī)器人內(nèi)部預(yù)先保存好的關(guān)于環(huán)境的幾何模型、拓?fù)涞貓D等比較完整的信息,在事先規(guī)劃出的全局路線基礎(chǔ)上應(yīng)用路徑跟蹤和避障技術(shù)來實(shí)現(xiàn)的(2)基于創(chuàng)建地圖的導(dǎo)航(map-buildingnavigation):是利用各種傳感器來創(chuàng)建關(guān)于當(dāng)前環(huán)境的幾何模型或拓?fù)淠Ｐ偷貓D,然后利用這些模型來實(shí)現(xiàn)導(dǎo)航。(3)無地圖的導(dǎo)航(maplessnavigation):是在環(huán)境信息完全未知的情況下,可通過攝像機(jī)或其他傳感器對(duì)周圍環(huán)境進(jìn)行探測(cè),利用對(duì)探測(cè)的物體進(jìn)行識(shí)別或跟蹤來實(shí)現(xiàn)導(dǎo)航。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航7.1.1

導(dǎo)航系統(tǒng)分類4.衛(wèi)星導(dǎo)航GPS全球定位系統(tǒng)是以距離作為基本的觀測(cè)量,通過對(duì)4顆GPS衛(wèi)星同時(shí)進(jìn)行位距測(cè)量計(jì)算出接收機(jī)的位置。智能機(jī)器人通過安裝衛(wèi)星信號(hào)接收裝置可以實(shí)現(xiàn)自身定位,無論其在室內(nèi)還是室外。但是該方法存在近距離定位精度低、信號(hào)障礙、多徑干擾等缺點(diǎn),在實(shí)際應(yīng)用中一般都結(jié)合其他導(dǎo)航技術(shù)一起工作。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航智能機(jī)器人的導(dǎo)航系統(tǒng)是一個(gè)自主式智能系統(tǒng),其主要任務(wù)是把感知、規(guī)劃、決策和行動(dòng)等模塊有機(jī)地結(jié)合起來。下圖給出了一種智能機(jī)器人自主導(dǎo)航系統(tǒng)的控制結(jié)構(gòu)。7.1.2

導(dǎo)航系統(tǒng)體系結(jié)構(gòu)北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航7.1.3視覺導(dǎo)航在早期的視覺導(dǎo)航是為自主地面智能機(jī)器人而研發(fā)。近年來,由于視覺導(dǎo)航方法具有自主性、廉價(jià)性和可靠性等特點(diǎn),已成為導(dǎo)航領(lǐng)域的研究熱點(diǎn)。視覺導(dǎo)航在無人飛行器、深空探測(cè)器和水下機(jī)器人方面獲得了廣泛應(yīng)用。1.視覺導(dǎo)航中的攝像機(jī)類型(1)被動(dòng)視覺導(dǎo)航：被動(dòng)視覺導(dǎo)航是依賴于可見光或不可見光成像技術(shù)的方法。CCD相機(jī)作為被動(dòng)成像的典型傳感器,廣泛應(yīng)用于各種視覺導(dǎo)航系統(tǒng)中。(2)主動(dòng)視覺導(dǎo)航：主動(dòng)視覺導(dǎo)航是利用激光雷達(dá)、聲吶等主動(dòng)探測(cè)方式進(jìn)行環(huán)境感知的導(dǎo)航方法。例如,1997年著陸的火星探路者號(hào)使用編碼激光條紋技術(shù)進(jìn)行前視距離探測(cè),可靠地解決了未知環(huán)境中的障礙識(shí)別問題。在視覺信息獲取過程中,主動(dòng)視覺導(dǎo)航能夠主動(dòng)調(diào)整攝像機(jī)的參數(shù),與環(huán)境動(dòng)態(tài)交互,根據(jù)具體要求分析有選擇地得到視覺數(shù)據(jù),更有效地理解視覺環(huán)境按照傳感器類型視覺導(dǎo)航可分為被動(dòng)視覺導(dǎo)航和主動(dòng)視覺導(dǎo)航。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航7.1.3視覺導(dǎo)航2.視覺導(dǎo)航中的攝像機(jī)數(shù)目(1)單目視覺導(dǎo)航：?jiǎn)文恳曈X的特點(diǎn)是結(jié)構(gòu)和數(shù)據(jù)處理較簡(jiǎn)單,研究的方向集中在如何從二維圖像中提取導(dǎo)航信息,常用的技術(shù)有閾值分割、透視圖法等。(2)立體視覺導(dǎo)航：一個(gè)完整的立體視覺系統(tǒng)分為圖像獲取、攝像機(jī)標(biāo)定、特征提取、立體匹配、深度確定及內(nèi)插重建等幾部分。立體匹配是立體視覺中最困難的一步。立體匹配方法必須解決3個(gè)問題:正確選擇圖像的匹配特征;尋找特征間的本質(zhì)屬性;建立正確的匹配策略。內(nèi)插重建是為了獲得物體的空間信息。重建算法的復(fù)雜度取決于匹配算法,根據(jù)視差圖,通過插值或擬合來重建物體。匹配和內(nèi)插之間存在一定的信息反饋,匹配結(jié)果約束內(nèi)插重建,而重建結(jié)果又引導(dǎo)正確匹配。根據(jù)攝像機(jī)數(shù)目的不同,視覺導(dǎo)航可以分為單目視覺導(dǎo)航、立體視覺導(dǎo)航。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航7.1.3視覺導(dǎo)航3.視覺導(dǎo)航中的地圖依賴性1)地圖導(dǎo)航系統(tǒng)地圖導(dǎo)航系統(tǒng)是發(fā)展較早的機(jī)器人導(dǎo)航方法。自然地標(biāo)和人工地標(biāo)是地標(biāo)跟蹤的兩個(gè)分類。(1)自然地標(biāo)導(dǎo)航算法使用相關(guān)性跟蹤選定的自然景物地標(biāo),通過立體視覺信息計(jì)算機(jī)器人自身的位置,并在機(jī)器人行進(jìn)中逐步更新景物地標(biāo)。(2)人工地標(biāo)導(dǎo)航通過機(jī)器人識(shí)別場(chǎng)景中的交通標(biāo)志,得出所處的位置、與目的地的距離等信息。視覺導(dǎo)航系統(tǒng)按照對(duì)地圖的依賴性可分為地圖導(dǎo)航系統(tǒng)、地圖生成型導(dǎo)航系統(tǒng)和無地圖導(dǎo)航系統(tǒng)。2)地圖生成型導(dǎo)航系統(tǒng)地圖生成型導(dǎo)航系統(tǒng)通過感知周圍環(huán)境,并在線生成某種表示的導(dǎo)航地圖,較好地解決了未知環(huán)境中同時(shí)完成實(shí)時(shí)定位、繪圖和自定位任務(wù)的問題。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.1導(dǎo)航7.1.3視覺導(dǎo)航3.視覺導(dǎo)航中的地圖依賴性視覺導(dǎo)航系統(tǒng)按照對(duì)地圖的依賴性可分為地圖導(dǎo)航系統(tǒng)、地圖生成型導(dǎo)航系統(tǒng)和無地圖導(dǎo)航系統(tǒng)。3)無地圖導(dǎo)航系統(tǒng)無地圖導(dǎo)航系統(tǒng)不需要對(duì)環(huán)境信息進(jìn)行全面描述。光流法、基于特征跟蹤基于模板的導(dǎo)航方法是無地圖視覺導(dǎo)航系統(tǒng)的主要研究方向。(1)光流法。通過機(jī)器人視場(chǎng)中固定特征的運(yùn)動(dòng)變化情況來估計(jì)機(jī)器人的運(yùn)動(dòng)。選擇圖像中有價(jià)值的特征點(diǎn)計(jì)算光流,可在保證運(yùn)動(dòng)估計(jì)精度的前提下降低計(jì)算量。隨著計(jì)算能力的顯著提高,基于光流法的視覺導(dǎo)航系統(tǒng)獲得了較快的發(fā)展。(2)基于特征跟蹤的視覺導(dǎo)航方法。通過跟蹤圖像序列中的特征元素(角、線、輪廓等)獲取導(dǎo)航信息。(3)基于模板的導(dǎo)航方法。使用預(yù)先獲得的圖像為模板,而模板與位置信息或控制指令相對(duì)應(yīng),導(dǎo)航過程中用當(dāng)前圖像幀與模板匹配,進(jìn)而獲取導(dǎo)航信息。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.2環(huán)境地圖的表示構(gòu)造地圖的目的是用于絕對(duì)坐標(biāo)系下的位姿估計(jì)。地圖的表示方法通常有4種:拓?fù)鋱D、特征圖、網(wǎng)格圖及直接表征法(appearancebasedmethods)。不同方法具有各自的特點(diǎn)和適用范圍,其中特征圖和網(wǎng)格圖的應(yīng)用較為普遍。7.2.1拓?fù)鋱D1)基本思想地鐵、公交路線圖均是典型的拓?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)系,圖形抽象,表示方便。拓?fù)鋱D通常是根據(jù)環(huán)境結(jié)構(gòu)定義的,由位置節(jié)點(diǎn)和連接線組成。環(huán)境的拓?fù)淠Ｐ途褪且粡堖B接線圖,其中的位置是節(jié)點(diǎn),連接線是邊。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.2環(huán)境地圖的表示7.2.2

特征圖1)基本思想在結(jié)構(gòu)化環(huán)境中,最常見的特征是直線段、角、邊等。這些特征可用它們的顏色、長(zhǎng)度、寬度、位置等參數(shù)表示。基于特征的地圖一般用下式的特征集合表示。其中fj是一個(gè)特征(邊、線、角等),n是地圖中的特征總數(shù)。利用環(huán)境特征構(gòu)造地圖是最常用的方法之一,大多數(shù)城市交通圖就是采用這種方法繪制的。2)特點(diǎn)特征法定位準(zhǔn)確,模型易于由計(jì)算機(jī)描述和表示,參數(shù)化特征也適用于路徑規(guī)劃和軌跡控制。但特征法需要特征提取等預(yù)處理過程,對(duì)傳感器噪聲比較敏感,只適于高度結(jié)構(gòu)化環(huán)境。

北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.2環(huán)境地圖的表示7.2.3

網(wǎng)格圖1)基本思想網(wǎng)格圖把機(jī)器人的工作空間劃分為網(wǎng)狀結(jié)構(gòu),網(wǎng)格中的每一個(gè)單元代表環(huán)境的一部分,每一個(gè)單元都分配了一個(gè)概率值,表示該單元被障礙物占據(jù)的可能性大小。特征圖的一個(gè)缺點(diǎn)是對(duì)所應(yīng)用的特征信息必須有精確的模型描述。另一種替代的方法是應(yīng)用網(wǎng)格圖。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ì)算量迅速增長(zhǎng),需要大量?jī)?nèi)存單元,使計(jì)算機(jī)的實(shí)時(shí)處理變得很困難。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.2環(huán)境地圖的表示7.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ī)器人的位姿。直接表征法是直接應(yīng)用傳感器讀入的數(shù)據(jù)來描述環(huán)境。由于傳感器數(shù)據(jù)本身比特征或網(wǎng)格這一中間表示環(huán)節(jié)包含了更豐富的環(huán)境描述信息2)特點(diǎn)直接表征法數(shù)據(jù)存貯量大,環(huán)境噪聲干擾嚴(yán)重,特征數(shù)據(jù)的提取與匹配困難,其應(yīng)用受到一定限制。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位定位是確定機(jī)器人在其作業(yè)環(huán)境中所處的位置。機(jī)器人可以利用先驗(yàn)環(huán)境地圖信息、位姿的當(dāng)前估計(jì)以及傳感器的觀測(cè)值等輸入信息,經(jīng)過一定處理變換,獲得更準(zhǔn)確的當(dāng)前位姿。智能機(jī)器人的定位方式有很多種,常用的有里程計(jì)、攝像機(jī)、激光雷達(dá)、聲吶、速度或加速度計(jì)等。從方法上來分,智能機(jī)器人的定位可分為相對(duì)定位和絕對(duì)定位兩種。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位相對(duì)定位又稱為局部位置跟蹤,要求機(jī)器人在已知初始位置的條件下通過測(cè)量機(jī)器人相對(duì)于初始位置的距離和方向來確定當(dāng)前位置,通常也稱為航跡推算法.7.3.1相對(duì)定位相對(duì)定位的優(yōu)點(diǎn)是結(jié)構(gòu)簡(jiǎn)單、價(jià)格低廉;機(jī)器人的位置進(jìn)行自我推算,不需要對(duì)外界感知信息。其缺點(diǎn)在于漂移誤差會(huì)隨時(shí)間積累,不能精確定位。因此,相對(duì)定位只適于短時(shí)、短距離運(yùn)動(dòng)的位姿估計(jì),長(zhǎng)時(shí)間運(yùn)動(dòng)時(shí)必須應(yīng)用其他的傳感器配合相關(guān)的定位算法進(jìn)行校正。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位里程計(jì)法是智能機(jī)器人定位技術(shù)中廣泛采用的方法之一。在智能機(jī)器人的車輪上安裝光電編碼器,通過編碼器記錄的車輪轉(zhuǎn)動(dòng)圈數(shù)計(jì)算機(jī)器人的位移和偏轉(zhuǎn)角度.7.3.1相對(duì)定位1)系統(tǒng)誤差系統(tǒng)誤差在很長(zhǎ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)輪間輪距長(zhǎng)度不確定。(5)有限的編碼器測(cè)量精度。(6)有限的編碼器采樣頻率。1.里程計(jì)法里程計(jì)法定位過程中會(huì)產(chǎn)生兩種誤差。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位7.3.1相對(duì)定位非系統(tǒng)誤差是在機(jī)器人和外界環(huán)境接觸過程中,由于外界環(huán)境的不可預(yù)料特性引起的。主要誤差來源如下。(1)輪子打滑。(2)地面不平。(3)地面有無法預(yù)料的物體(例如石塊)。(4)外力作用和內(nèi)力作用。(5)驅(qū)動(dòng)輪和地板是面接觸而不是點(diǎn)接觸。1.里程計(jì)法2)非系統(tǒng)誤差北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位2)非系統(tǒng)誤差7.3.1相對(duì)定位1.里程計(jì)法非系統(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ā)生的頻率更高。對(duì)于機(jī)器人定位來說,非系統(tǒng)誤差是非常嚴(yán)重的問題,因?yàn)樗鼰o法預(yù)測(cè)并導(dǎo)致嚴(yán)重的方向誤差。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位3)誤差補(bǔ)償7.3.1相對(duì)定位1.里程計(jì)法在機(jī)器人定位過程中,需要利用外界的傳感器信息補(bǔ)償誤差。因此利用外界傳感器定位機(jī)器人時(shí),主要任務(wù)在于提取導(dǎo)航環(huán)境的特征,并和環(huán)境地圖匹配。在室內(nèi)環(huán)境中,墻壁、走廊、拐角、門等特征被實(shí)際地用于機(jī)器人的定位研究。廣泛用于機(jī)器人定位的外界傳感器有陀螺儀、電磁羅盤、紅外線、超聲波傳感器、聲吶、激光測(cè)距儀、視覺系統(tǒng)等。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位慣性導(dǎo)航定位法是一種使用慣性導(dǎo)航傳感器定位的方法。它通常用陀螺儀來測(cè)量機(jī)器人的角速度,用加速度計(jì)測(cè)量機(jī)器人的加速度。對(duì)測(cè)量結(jié)果進(jìn)行一次和二次積分,即可得到機(jī)器人偏移的角度和位移,進(jìn)而得出機(jī)器人當(dāng)前的位置和姿態(tài)。7.3.1相對(duì)定位2.慣性導(dǎo)航定位法用慣性導(dǎo)航定位法進(jìn)行定位不需要外部環(huán)境信息,但是由于常量誤差經(jīng)積分運(yùn)算會(huì)產(chǎn)生誤差的累積,因此該方法也不適用于長(zhǎng)時(shí)間的精確定位。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位絕對(duì)定位又稱為全局定位,要求機(jī)器人在未知初始位置的情況下確定自己的位置。主要采用主動(dòng)燈塔法、路標(biāo)導(dǎo)航定位法、地圖匹配、衛(wèi)星導(dǎo)航技術(shù)(比如GPS)進(jìn)行定位,定位精度較高。在這幾種方法中,信標(biāo)或標(biāo)識(shí)牌的建設(shè)和維護(hù)成本較高,地圖匹配技術(shù)處理速度慢,GPS只能用于室外,目前精度還很差,絕對(duì)定位的位置計(jì)算方法包括三視角法、三視距法、模型匹配算法等。7.3.2絕對(duì)定位北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位主動(dòng)燈塔是可以很可靠地被檢測(cè)到的信號(hào)發(fā)射源,將該信號(hào)進(jìn)行最少的處理,就可以提供精確的定位信息。主動(dòng)燈塔法的采樣率可以很高,從而產(chǎn)生很高的可靠性。缺點(diǎn)是主動(dòng)燈塔的安裝和維護(hù)會(huì)導(dǎo)致很高的費(fèi)用。7.3.2絕對(duì)定位1.主動(dòng)燈塔法2.路標(biāo)導(dǎo)航定位法路標(biāo)導(dǎo)航定位法是利用環(huán)境中的路標(biāo)給智能機(jī)器人提供位置信息。路標(biāo)分為人工路標(biāo)和自然路標(biāo)。人工路標(biāo)是為了實(shí)現(xiàn)機(jī)器人定位而人為放置于機(jī)器人工作環(huán)境中的物體或標(biāo)識(shí)。自然路標(biāo)是機(jī)器人的工作環(huán)境中固有的物體或自然特征?；诼窐?biāo)的定位精度取決于機(jī)器人與路標(biāo)間的距離和角度。當(dāng)機(jī)器人遠(yuǎn)離路標(biāo)時(shí),定位精度較低,靠近時(shí),定位精度較高。另外,不管是人工路標(biāo)還是自然路標(biāo),路標(biāo)的位置都應(yīng)是已知的。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位基于地圖的定位方法稱為地圖匹配法。機(jī)器人運(yùn)用各種傳感器(如超聲波傳感器、激光測(cè)距儀、視覺系統(tǒng)等)探測(cè)環(huán)境來創(chuàng)建它所處的局部環(huán)境地圖,然后將此局部地圖與存儲(chǔ)在機(jī)器人中的己知的全局地圖進(jìn)行匹配。如果匹配成功,機(jī)器人就計(jì)算出自身在該環(huán)境中的位置。7.3.2絕對(duì)定位3.地圖匹配法4.GPS定位GPS是適用于室外智能機(jī)器人的一種全局定位系統(tǒng)。它是一種以空間衛(wèi)星為基礎(chǔ)的高精度導(dǎo)航與定位系統(tǒng),由美國(guó)國(guó)防部批準(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ù)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位馬爾可夫定位的基本思想是:機(jī)器人不知道它的確切位置,而是知道它可能位置的信度(belief,即機(jī)器人在整個(gè)位置空間的概率分布,信度值之和為1)。馬爾可夫定位的關(guān)鍵之處在于信度值的計(jì)算。當(dāng)機(jī)器人收到外界傳感器信息或利用編碼器獲得機(jī)器人移動(dòng)信息時(shí),基于馬爾可夫假設(shè)和貝葉斯規(guī)則,每個(gè)柵格的信度值被更新。7.3.3基于概率的絕對(duì)定位1.馬爾可夫定位(Markovlocalization,ML)根據(jù)初始狀態(tài)概率分布p(x0)和觀測(cè)數(shù)據(jù)Yt={yt|t=0,1,…,t}估計(jì)系統(tǒng)的當(dāng)前狀態(tài)xt,其中xt=(wx,wy,θ)T表示機(jī)器人的位姿(由位置和方向組成)。從統(tǒng)計(jì)學(xué)的觀點(diǎn)看,xt的估計(jì)是一個(gè)貝葉斯濾波問題,可以通過估計(jì)后驗(yàn)密度分布p(xt|Yt)實(shí)現(xiàn)。貝葉斯濾波器假設(shè)系統(tǒng)是一個(gè)馬爾可夫過程,p(xt|Yt)可以通過以下2個(gè)步驟算得。在概率定位中,最重要的是馬爾可夫定位和蒙特卡洛定位。馬爾可夫定位和蒙特卡洛定位不僅能夠?qū)崿F(xiàn)全局定位和局部位置跟蹤,而且能夠解決機(jī)器人的“綁架”問題。機(jī)器人的“綁架”問題是指,由于機(jī)器人容易與外界發(fā)生碰撞而使機(jī)器人在不知情(里程計(jì)沒有記錄)的情況下發(fā)生移動(dòng)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位1)預(yù)測(cè)通過運(yùn)動(dòng)模型預(yù)測(cè)系統(tǒng)在下一時(shí)刻的狀態(tài),即通過如下公式計(jì)算先驗(yàn)概率密度p(xt|Yt-1):7.3.3基于概率的絕對(duì)定位1.馬爾可夫定位(Markovlocalization,ML)式中:p(xt|xt-1,ut-1)稱為系統(tǒng)的運(yùn)動(dòng)模型(狀態(tài)轉(zhuǎn)移先驗(yàn)密度)。2)更新通過觀測(cè)模型,利用新的觀測(cè)信息更新系統(tǒng)的狀態(tài),即通過如下公式計(jì)算后驗(yàn)概率密度p(xt|Yt):式中:p(yt|xt)稱為系統(tǒng)的觀測(cè)模型(觀測(cè)密度)。當(dāng)機(jī)器人獲得編碼器信息或利用外界傳感器感知環(huán)境后,馬爾可夫定位算法必須對(duì)所有的柵格進(jìn)行計(jì)算,因此需要大量的計(jì)算資源和內(nèi)存,導(dǎo)致定位處理的實(shí)時(shí)性很差。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位基于馬爾可夫定位方法,Dellaert等人提出了蒙特卡洛定位方法。MCL也稱為粒子濾波(particlefilter)。7.3.3基于概率的絕對(duì)定位2.蒙特卡洛定位(Monte-Carlolocalization,MCL)MCL的主要思想是用N個(gè)帶有權(quán)重的離散采樣St={(xt(j),wt(j))|j=1,…,N}來表示后驗(yàn)概率密度p(xt|Yt)。其中xt(j)是機(jī)器人在t時(shí)刻的一個(gè)可能狀態(tài);wt(j)是一個(gè)非負(fù)的參數(shù)稱為權(quán)重,表示t時(shí)刻機(jī)器人的狀態(tài)為xt(j)的概率,也就是p(xt(j)|Yt}≈wt(j)

,且MCL包括4個(gè)階段:初始化、采樣階段、權(quán)重歸一化和輸出階段。采樣階段是MCL的核心,它包括重采樣、狀態(tài)轉(zhuǎn)移和權(quán)重計(jì)算3步。實(shí)際上,MCL是按照提議密度分布抽取采樣,然后利用權(quán)重來補(bǔ)償提議密度分布與后驗(yàn)密度分布p(xt|Yt)之間的差距。當(dāng)機(jī)器人發(fā)生“綁架”時(shí),要估計(jì)的后驗(yàn)密度p(xt|Yt)與提議密度分布的錯(cuò)位很大,在p(xt|Yt)取值較大區(qū)域的采樣數(shù)很少,需要大量的采樣才能較好地估計(jì)后驗(yàn)密度。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位卡爾曼濾波器是一個(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)等。7.3.3基于概率的絕對(duì)定位3.卡爾曼濾波(Kalmanfilter,KF)定位卡爾曼濾波器通過預(yù)測(cè)方程和測(cè)量方程對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì),利用遞推的方式尋找最小均方誤差下的XK的估計(jì)值X^k。該濾波器的數(shù)學(xué)模型如下。狀態(tài)方程如下:測(cè)量方程如下:其中,XK是k時(shí)刻系統(tǒng)的狀態(tài),A是k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣,ZK是k時(shí)刻的測(cè)量值,H是觀測(cè)矩陣,WK-1為系統(tǒng)過程噪聲,VK為系統(tǒng)測(cè)量噪聲,假設(shè)為高斯白噪聲。7.3定位如果不考慮觀測(cè)噪聲和輸入信號(hào),則k時(shí)刻的觀測(cè)值ZK和已知的最優(yōu)狀態(tài)估計(jì)值X^k-1可通過以下方程求解X^k的最優(yōu)估計(jì)值狀態(tài)預(yù)測(cè)方程如下：7.3.3基于概率的絕對(duì)定位3.卡爾曼濾波(Kalmanfilter,KF)定位預(yù)測(cè)狀態(tài)下的協(xié)方差方程如下:

濾波器增益矩陣如下:

北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位狀態(tài)最優(yōu)化估計(jì)方程如下：7.3.3基于概率的絕對(duì)定位3.卡爾曼濾波(Kalmanfilter,KF)定位狀態(tài)最優(yōu)化估計(jì)的協(xié)方差方程如下：

通過卡爾曼濾波器的公式可以看出,只要給定了X0和P0,就可以根據(jù)k時(shí)刻的觀測(cè)值Zk,通過遞推計(jì)算得出k時(shí)刻的狀態(tài)估計(jì)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位下圖給出了卡爾曼濾波器根據(jù)所有傳感器提供的信息實(shí)現(xiàn)高效信息融合的一般方案。7.3.3基于概率的絕對(duì)定位3.卡爾曼濾波(Kalmanfilter,KF)定位系統(tǒng)有一個(gè)控制信號(hào)和作為輸入的系統(tǒng)誤差源。測(cè)量裝置能夠測(cè)量帶有誤差的某些系統(tǒng)狀態(tài)?？柭鼮V波器是根據(jù)系統(tǒng)知識(shí)和測(cè)量裝置產(chǎn)生系統(tǒng)狀態(tài)最優(yōu)估計(jì)數(shù)學(xué)機(jī)制,是對(duì)系統(tǒng)噪聲和觀測(cè)誤差不確定性的描述。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位卡爾曼濾波器已經(jīng)廣泛應(yīng)用在了各個(gè)方面,比如機(jī)器人的同步定位與地圖構(gòu)建(SLAM)問題、雷達(dá)系統(tǒng)的跟蹤等等。下圖描述了卡爾曼濾波器的機(jī)器人定位架構(gòu)7.3.3基于概率的絕對(duì)定位3.卡爾曼濾波(Kalmanfilter,KF)定位北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.3定位機(jī)器人通常通過大量異質(zhì)傳感器提供機(jī)器人定位的消息。顯然,各傳感器也都是存在誤差的。機(jī)器人的總傳感器信號(hào)輸入處理成了提取的特征信息集合。7.3.3基于概率的絕對(duì)定位3.卡爾曼濾波(Kalmanfilter,KF)定位(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)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃路徑規(guī)劃就是按照一定的性能指標(biāo)(如工作代價(jià)最小、行走路線最短、行走時(shí)間最短,安全、無碰撞地通過所有障礙物等),機(jī)器人從所處的環(huán)境中搜索到一條從初始位置開始的實(shí)現(xiàn)其自身目的的最優(yōu)或次優(yōu)路徑。路徑規(guī)劃本身可以分成不同的層次,從不同的方面有不同的劃分。根據(jù)對(duì)環(huán)境的掌握情況,機(jī)器人的路徑規(guī)劃問題可以以下分為3種類型。7.4.1路徑規(guī)劃分類1.基于地圖的全局路徑規(guī)劃基于地圖的全局路徑規(guī)劃,根據(jù)先驗(yàn)環(huán)境模型找出從起始點(diǎn)到目標(biāo)點(diǎn)的符合一定性能的可行或最優(yōu)的路徑。全局規(guī)劃的主要方法有柵格法、可視圖法、概率路徑圖法、拓?fù)浞ā⑸窠?jīng)網(wǎng)絡(luò)法等。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.1路徑規(guī)劃分類3.混合型方法混合型方法試圖結(jié)合全局和局部的優(yōu)點(diǎn),將全局規(guī)劃的“粗”路徑作為局部規(guī)劃的目標(biāo),從而引導(dǎo)機(jī)器人最終找到目標(biāo)點(diǎn)。2.基于傳感器的局部路徑規(guī)劃基于傳感器的局部路徑規(guī)劃依賴傳感器獲得障礙物的尺寸、形狀和位置等信息。環(huán)境是未知或部分未知的。局部路徑規(guī)劃算法的主要方法有模糊邏輯算法、遺傳算法、人工勢(shì)場(chǎng)法等。也可以把兩類算法結(jié)合使用,有效地實(shí)現(xiàn)機(jī)器人的路徑規(guī)劃。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃現(xiàn)今的路徑規(guī)劃問題具有如下特點(diǎn)。7.4.1路徑規(guī)劃分類(1)復(fù)雜性。在復(fù)雜環(huán)境尤其是動(dòng)態(tài)時(shí)變環(huán)境中,機(jī)器人路徑規(guī)劃非常復(fù)雜,且需要很大的計(jì)算量。(2)隨機(jī)性。復(fù)雜環(huán)境的變化往往存在很多隨機(jī)性和不確定因素。(3)多約束機(jī)器人的運(yùn)動(dòng)存在幾何約束和物理約束。幾何約束是指受機(jī)器人的形狀制約,物理約束是指受機(jī)器人的速度和加速度制約。(4)多目標(biāo)機(jī)器人運(yùn)動(dòng)過程對(duì)路徑性能存在多方面的要求,如路徑最短、時(shí)間最優(yōu)、安全性能最好、能源消耗最小,但它們之間往往存在沖突。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.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)之間的連線均不能穿越障礙物,即直線是“可視的”?？梢晥D法路徑規(guī)劃如圖所示：這樣,從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑轉(zhuǎn)化為從起始點(diǎn)到目標(biāo)點(diǎn)經(jīng)過這些可視直線的最短距離問題。圖中的粗實(shí)線即為由VG法得到的最短路徑,但由于過于靠近障礙物,得到的路徑安全性較差?？梢晥D法適用于環(huán)境中的障礙物是多邊形的情況。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法2.Voronoi圖法Voronoi圖,又叫泰森多邊形圖。如圖所示,它由一組由連接兩鄰點(diǎn)直線的垂直平分線組成的連續(xù)多邊形組成。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法2.Voronoi圖法由下圖可見,Voronoi圖的路徑規(guī)劃盡可能遠(yuǎn)離障礙物,從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑將會(huì)增長(zhǎng)。但采用這種控制方式,即使產(chǎn)生位置誤差,智能機(jī)器人也不會(huì)碰到障礙物,其缺點(diǎn)是存在較多的突變點(diǎn)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法3.單元分解法如圖所示,首先把狀態(tài)空間分解為與空間平行的許多矩形或立方體,稱為單元(cell),每個(gè)cell都標(biāo)記為以下幾項(xiàng)。(1)空的:如果cell內(nèi)的每一點(diǎn)均與狀態(tài)空間的障礙物不相交。(2)滿的:如果cell內(nèi)的每一點(diǎn)均與狀態(tài)空間中的障礙物相交。(3)混合的:如果cell內(nèi)的點(diǎn)既有與狀態(tài)空間的障礙物相交的,也有不相交的。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法3.單元分解法單元分解法就是要尋找一條由空的cell所組成的包含有起始點(diǎn)和目標(biāo)點(diǎn)的連通路徑,如下圖所示。如果這樣的路徑在初始劃分的狀態(tài)空間中不存在,則要找出所有的混合cell,將其進(jìn)一步細(xì)分,并將劃分的結(jié)果進(jìn)行標(biāo)記,然后在空的cell中搜索,如此反復(fù),直至成功。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法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à)值的信息拋棄的缺陷,且易陷入局部最小值。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法5.A*算法1)A*算法原型Dijkstra算法的基本思想如圖所示,從初始點(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ù)字。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法5.A*算法2)A*算法流程A*算法具體引入了當(dāng)前節(jié)點(diǎn)的估計(jì)函數(shù)f(i),節(jié)點(diǎn)的評(píng)價(jià)函數(shù)可以定義為:

式中:g(i)表示從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)的最短距離,h(i)表示從當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的最短距離的估計(jì)值,可取節(jié)點(diǎn)到終點(diǎn)的直線和球面距離。若h(i)=0,即沒有利用任何路網(wǎng)信息,這時(shí)A*算法就變成了Dijkstra算法。可見,A*算法的實(shí)質(zhì)是Dijkstra算法的改進(jìn)“算法”。對(duì)于h(i)的具體形式,也可以依據(jù)實(shí)際情況選擇。例如,除了可以用當(dāng)前到終點(diǎn)的最短距離外,還可以引入方向。A*算法本身表述起來很簡(jiǎn)單,關(guān)鍵是在代碼優(yōu)化上,基本的思路一般都是以空間(即內(nèi)存的占傭)換取時(shí)間(搜索速度),另外還有諸如多級(jí)地圖精度和地圖分區(qū)域搜索等一些地圖預(yù)處理技術(shù)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法6.基于模糊邏輯的路徑規(guī)劃模糊方法是在線規(guī)劃中通常采用的一種規(guī)劃方法,包括建模和局部規(guī)劃。基于模糊邏輯的機(jī)器人路徑規(guī)劃的基本思想是:各個(gè)物體的運(yùn)動(dòng)狀態(tài)用模糊集的概念來表達(dá),每個(gè)物體的隸屬函數(shù)包含該物體當(dāng)前位置、速度大小和速度方向的信息。然后通過模糊綜合評(píng)價(jià)對(duì)各個(gè)方向進(jìn)行綜合考查,得到路徑規(guī)劃結(jié)果。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法7.基于神經(jīng)網(wǎng)絡(luò)的路徑規(guī)劃神經(jīng)網(wǎng)絡(luò)具有并行處理性、信息分布式存儲(chǔ)、自組織等特性。Hopfield神經(jīng)網(wǎng)絡(luò)可用于求解優(yōu)化問題,其能量函數(shù)的定義利用了神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)。Hopfield神經(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)離障礙物,不與障礙物相碰,并使路徑的長(zhǎng)度盡量短,即得到一條最優(yōu)路徑。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法8.基于遺傳算法的路徑規(guī)劃遺傳算法是一種基于自然選擇和基因遺傳學(xué)原理的搜索算法。遺傳算法用于機(jī)器人路徑規(guī)劃的基本思想:采用柵格法對(duì)機(jī)器人工作空間進(jìn)行劃分,用序號(hào)標(biāo)識(shí)柵格,并以此序號(hào)作為機(jī)器人路徑規(guī)劃參數(shù)編碼,統(tǒng)一確定其個(gè)體長(zhǎng)度,隨機(jī)產(chǎn)生障礙物位置及數(shù)目,并在搜索到最優(yōu)路徑后再在環(huán)境空間中隨機(jī)插入障礙物,模擬環(huán)境變化。但是,規(guī)劃空間柵格法建模還存在缺陷,即若柵格劃分過粗,則規(guī)劃精度較低;若柵格劃分太細(xì),則數(shù)據(jù)量又會(huì)太大。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.4路徑規(guī)劃7.4.2路徑規(guī)劃方法9.動(dòng)態(tài)規(guī)劃法動(dòng)態(tài)規(guī)劃法是解決多階段決策優(yōu)化問題的一種數(shù)值方法。動(dòng)態(tài)規(guī)劃法將復(fù)雜的多變量決策問題進(jìn)行分段決策,從而將其轉(zhuǎn)化為多個(gè)單變量的決策問題。Barraquand等人以經(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)容。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.1人工勢(shì)場(chǎng)法的基本思想勢(shì)場(chǎng)的方法是由Khatib最先提出的,他把機(jī)械手是智能機(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ù)合力來確定機(jī)器人的運(yùn)動(dò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)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.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可表示為式中:Fg為引力;Fo為斥力;F為合力,決定了智能機(jī)器人的運(yùn)動(dòng)。

在式中:Uo為斥力場(chǎng);Ug為引力場(chǎng)。

勢(shì)場(chǎng)力可表示為:北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建斥力Fo與引力Fg可分別表達(dá)為在勢(shì)場(chǎng)中,智能機(jī)器人的受力圖如圖所示。

北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建當(dāng)機(jī)器人到達(dá)目標(biāo),目標(biāo)點(diǎn)對(duì)智能機(jī)器人的引力等于障礙物對(duì)其產(chǎn)生的斥力時(shí),F=0。算法也可能產(chǎn)生局部極小點(diǎn),在某個(gè)位置F=0時(shí),并未到達(dá)目標(biāo)。這時(shí)需要改進(jìn)算法,例如引入其他的量對(duì)機(jī)器人進(jìn)行控制。如圖給出了一個(gè)人工勢(shì)場(chǎng)分布示意圖,從圖中可以大致了解機(jī)器人在某個(gè)位置的運(yùn)動(dòng)趨勢(shì)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建1.斥力場(chǎng)函數(shù)當(dāng)障礙物形狀規(guī)則時(shí),障礙物的表面由隱函數(shù)f(X)=0來表示,則斥力函數(shù)可表示為式中:η為位置增益系數(shù),Xo是障礙物附近一點(diǎn)。勢(shì)力場(chǎng)的影響范圍局限于f(X)=0和f(Xo)=0兩表面之間的空間。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建1.斥力場(chǎng)函數(shù)當(dāng)障礙物形狀不規(guī)則時(shí),斥力場(chǎng)函數(shù)可表示為式中:ρ為智能機(jī)器人X與障礙物O之間的最短距離,ρo是一個(gè)常數(shù),代表障礙物的影響距離。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建1.斥力場(chǎng)函數(shù)相應(yīng)地,將上式代入勢(shì)場(chǎng)力函數(shù)F=Fg+Fo中,可求得斥力或

北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》式中

7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建2.引力場(chǎng)函數(shù)目標(biāo)G的勢(shì)函數(shù)Ug同樣也可以基于距離的概念。目標(biāo)G對(duì)智能機(jī)器人X起吸引作用,而且距離越遠(yuǎn),吸引作用越大,反之就越小。當(dāng)距離為0時(shí),智能機(jī)器人的勢(shì)能為0,此時(shí)智能機(jī)器人到達(dá)終點(diǎn)。通常引力場(chǎng)函數(shù)可構(gòu)建為式中:kg為位置增益系數(shù);X-Xg為智能機(jī)器人X與目標(biāo)點(diǎn)Xg之間的相對(duì)距離。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》

7.5人工勢(shì)場(chǎng)法7.5.2勢(shì)場(chǎng)函數(shù)的構(gòu)建2.引力場(chǎng)函數(shù)目標(biāo)G的勢(shì)函數(shù)Ug同樣也可以基于距離的概念。目標(biāo)G對(duì)智能機(jī)器人X起吸引作用,而且距離越遠(yuǎn),吸引作用越大,反之就越小。當(dāng)距離為0時(shí),智能機(jī)器人的勢(shì)能為0,此時(shí)智能機(jī)器人到達(dá)終點(diǎn)。通常引力場(chǎng)函數(shù)可構(gòu)建為式中:kg為位置增益系數(shù);X-Xg為智能機(jī)器人X與目標(biāo)點(diǎn)Xg之間的相對(duì)距離。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》

相應(yīng)地,將上式代入引力公式中,可得到吸引力為

式中:吸引力Fg的方向指向目標(biāo)點(diǎn),在智能機(jī)器人到達(dá)目標(biāo)的過程中,這個(gè)力線性地收斂于零7.5人工勢(shì)場(chǎng)法7.5.3人工勢(shì)場(chǎng)法的特點(diǎn)1.優(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)簡(jiǎn)單、易于實(shí)現(xiàn),所以在路徑規(guī)劃中被廣泛地采用。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》2.缺點(diǎn)勢(shì)場(chǎng)法的缺點(diǎn)是存在一個(gè)局部最優(yōu)點(diǎn)問題。許多學(xué)者進(jìn)行了研究,他們期望通過建立統(tǒng)一的勢(shì)能函數(shù)來解決這一問題,但是這就要求障礙物最好是規(guī)則的,否則算法的計(jì)算量很大,有時(shí)甚至是無法計(jì)算的。針對(duì)勢(shì)場(chǎng)法存在的大計(jì)算量和局部最優(yōu)點(diǎn)問題等缺點(diǎn),可應(yīng)用柵格法與勢(shì)場(chǎng)法的結(jié)合降低勢(shì)場(chǎng)法的計(jì)算復(fù)雜度;應(yīng)用障礙物構(gòu)造勢(shì)場(chǎng),避免局部最優(yōu)點(diǎn)的問題。7.5人工勢(shì)場(chǎng)法7.5.4人工勢(shì)場(chǎng)法的改進(jìn)為了解決局部最優(yōu)點(diǎn)問題,一些文獻(xiàn)提出了一種改進(jìn)的勢(shì)場(chǎng)函數(shù)。以前的目標(biāo)不可達(dá)問題的主要原因是當(dāng)目標(biāo)在障礙物的影響范圍之內(nèi)時(shí),整個(gè)勢(shì)場(chǎng)的全局最小點(diǎn)并不是目標(biāo)點(diǎn)。因?yàn)楫?dāng)智能機(jī)器人向目標(biāo)逼近時(shí),障礙物勢(shì)場(chǎng)快速增加。如果智能機(jī)器人向目標(biāo)逼近時(shí),斥力場(chǎng)趨于0,那么目標(biāo)點(diǎn)將是整個(gè)勢(shì)場(chǎng)的全局最小點(diǎn)。因此,定義斥力場(chǎng)函數(shù)時(shí),把智能機(jī)器人與目標(biāo)之間的相對(duì)距離也考慮進(jìn)去,從而建立一個(gè)新的斥力場(chǎng)函數(shù)。修改斥力場(chǎng)函數(shù)如下:北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.4人工勢(shì)場(chǎng)法的改進(jìn)在式中,X-Xg為智能機(jī)器人與目標(biāo)點(diǎn)之間的距離,障礙物的影響范圍在距離ρo之內(nèi),η是一個(gè)大于零的任意實(shí)數(shù)。與原式相比,改進(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)是可微的,在此不再贅述。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法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ù)選取為:①引力增益系數(shù):2;②斥力增益系數(shù):5;③小車運(yùn)動(dòng)的步長(zhǎng):0.5;④障礙物影響距離:2。單障礙物的路徑規(guī)劃仿真結(jié)果如圖所示。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.5仿真分析對(duì)多障礙物的仿真(因FIRA比賽中有5vs5比賽,故障礙物選取5個(gè))。實(shí)驗(yàn)中就不同給定障礙物的條件下進(jìn)行了大量的仿真。在絕大部分情況下,小車均能尋找到通往目標(biāo)點(diǎn)的路徑,并且順利繞開障礙物。這說明人工勢(shì)場(chǎng)法用于機(jī)器人的路徑規(guī)劃還是可行的。如圖給出了其中幾種不同條件下的路徑規(guī)劃圖。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.5人工勢(shì)場(chǎng)法7.5.5仿真分析如圖給出了目標(biāo)點(diǎn)與障礙物較近時(shí)的路徑規(guī)劃情況。由圖中可以看出:起初,機(jī)器人能夠完成避障并向目標(biāo)前進(jìn);當(dāng)接近目標(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)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.6柵格法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)行傳播,障礙物所在的柵格值最大。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.6柵格法7.6.1用柵格表示環(huán)境北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.6柵格法7.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)行路徑的搜索。在傳統(tǒng)的柵格法中,路徑搜索一般是將“起始點(diǎn)柵格”作為參考柵格,從參考柵格的8個(gè)相鄰柵格中選擇值最小的柵格;再將所選柵格作為新的參考柵格,重復(fù)此步驟,直到到達(dá)“目標(biāo)柵格”。為了保證路徑的平坦,要做一定的設(shè)置,即如果有多個(gè)可選柵格時(shí),選擇使智能機(jī)器人轉(zhuǎn)動(dòng)角度最小的柵格。那么此時(shí)就要記錄智能機(jī)器人的移動(dòng)方向。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.6柵格法7.6.3柵格法的特點(diǎn)柵格具有簡(jiǎ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)簡(jiǎn)單,在很多場(chǎng)合都實(shí)用。(4)只要起始點(diǎn)與終點(diǎn)之間存在通路,柵格就一定能找到一條從起始點(diǎn)到終點(diǎn)的路徑。同時(shí)也能看到柵格大小的選擇直接影響著控制算法的性能。柵格選得小,環(huán)境分辨率高,但是抗干擾能力弱,環(huán)境信息存儲(chǔ)量大,決策速度慢;柵格選得大,抗干擾能力強(qiáng),環(huán)境信息存儲(chǔ)量小,決策速度快,但是分辨率下降,在密集障礙物環(huán)境中發(fā)現(xiàn)路徑的能力減弱。所有單用柵格法對(duì)現(xiàn)在的智能機(jī)器人的研究已經(jīng)行不通了。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7智能機(jī)器人的同步定位與地圖構(gòu)建智能機(jī)器人的同步定位與地圖構(gòu)建(simultaneouslocalizationandmapping,SLAM),是指機(jī)器人在不確定自身位置的條件下,在部分已知或完全未知的環(huán)境中運(yùn)動(dòng)時(shí),根據(jù)位置估計(jì)和傳感器探測(cè)數(shù)據(jù)進(jìn)行自定位,同時(shí)建造增量式地圖。這種構(gòu)建地圖,同時(shí)利用地圖進(jìn)行自主定位和導(dǎo)航的能力,被認(rèn)為是機(jī)器人真正實(shí)現(xiàn)自主作業(yè)的關(guān)鍵和基礎(chǔ)。(1)環(huán)境建模(mapping)是建立機(jī)器人工作環(huán)境的各種物體,如障礙、路標(biāo)等的準(zhǔn)確的空間位置描述,即空間模型或地圖。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》(2)定位(localization)是確定機(jī)器人自身在該工作環(huán)境中的精確位置。精確的環(huán)境模型(地圖)及機(jī)器人定位有助于高效的路徑規(guī)劃和決策,是保證機(jī)器人安全導(dǎo)航的基礎(chǔ)?？梢?定位和建圖是一個(gè)“雞和蛋”的問題,環(huán)境建模需要定位,定位又依賴于環(huán)境地圖。為此,一些研究者提出了同步定位與地圖構(gòu)建的可能性。SLAM被認(rèn)為是實(shí)現(xiàn)真正全自主智能機(jī)器人的關(guān)鍵,是智能機(jī)器人導(dǎo)航領(lǐng)域的基本問題與研究熱點(diǎn)。7.7智能機(jī)器人的同步定位與地圖構(gòu)建SLAM問題可以描述為:智能機(jī)器人從一個(gè)未知的位置出發(fā),在不斷運(yùn)動(dòng)的過程中,根據(jù)自身位姿估計(jì)和傳感器對(duì)環(huán)境的感知,構(gòu)建增量式地圖,同時(shí)利用該地圖更新自己的定位。定位與增量式建圖融為一體,而不是獨(dú)立的兩個(gè)階段。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》自主智能機(jī)器人靠各種傳感器獲得信息,但傳感器信息的獲得與機(jī)器人的運(yùn)動(dòng)具有不確定性,同時(shí)也缺乏環(huán)境的先驗(yàn)信息。作為機(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方法。7.7.1SLAM的基本問題7.7智能機(jī)器人的同步定位與地圖構(gòu)建下圖簡(jiǎn)單描述了智能機(jī)器人SLAM的系統(tǒng)狀態(tài)。假設(shè)機(jī)器人在未知環(huán)境中移動(dòng),同時(shí)使用自身攜帶的傳感器探測(cè)外部未知的路標(biāo)信息及自身的里程信息。xt表示t時(shí)刻智能機(jī)器人的位姿狀態(tài)向量,mi表示第i個(gè)路標(biāo)的位置狀態(tài)向量,ut為機(jī)器人從t-1時(shí)刻到t時(shí)刻的輸入控制向量,zt為t時(shí)刻觀測(cè)向量。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.2智能機(jī)器人SLAM系統(tǒng)模型7.7智能機(jī)器人的同步定位與地圖構(gòu)建若把t時(shí)刻智能機(jī)器人SLAM系統(tǒng)的狀態(tài)記為st,狀態(tài)st包含了t時(shí)刻機(jī)器人的位姿(即機(jī)器人的位置和方向)和路標(biāo)的位置。從概率學(xué)來看,假定智能機(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),即p(st|s0:t-1,z0:t,u1:t)。假設(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)的分布概率為北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.2智能機(jī)器人SLAM系統(tǒng)模型

在此系統(tǒng)狀態(tài)估計(jì)上獲得的觀測(cè)信息的估計(jì)為

7.7智能機(jī)器人的同步定位與地圖構(gòu)建可以看出,式(7.26)描述了系統(tǒng)狀態(tài)的轉(zhuǎn)移概率,它與公式(7.27)共同組成了智能機(jī)器人和環(huán)境的一個(gè)隱馬爾可夫模型(hiddenmarkovmodel,HMM)或動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)(dynamicBayesnetwork,DBN),即智能機(jī)器人SLAM問題模型如下圖所示。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.2智能機(jī)器人SLAM系統(tǒng)模型7.7智能機(jī)器人的同步定位與地圖構(gòu)建1.SLAM解決思想對(duì)于SLAM問題,根據(jù)之前的智能機(jī)器人位姿、觀測(cè)信息以及控制輸入信息,可以求得t時(shí)刻機(jī)器人位姿x和環(huán)境中路標(biāo)位置m的聯(lián)合后驗(yàn)概率北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.3智能機(jī)器人SLAM解決方法假定環(huán)境服從馬爾科夫的前提，SLAM問題可分為預(yù)測(cè)、更新兩步遞歸執(zhí)行。7.7智能機(jī)器人的同步定位與地圖構(gòu)建1.SLAM解決思想預(yù)測(cè)：依據(jù)前一時(shí)刻狀態(tài)的后驗(yàn)信度Bel(xt-1,mt-1),也即p(xt-1,mt-1|zt-1,xt-2,ut-1),結(jié)合運(yùn)動(dòng)模型預(yù)測(cè)當(dāng)前t時(shí)刻狀態(tài)xt的先驗(yàn)信度Bel-(xt,mt)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.3智能機(jī)器人SLAM解決方法式中:p(xt|xt-1,ut)為運(yùn)動(dòng)模型,Bel(xt-1,mt-1)為后驗(yàn)信度。7.7智能機(jī)器人的同步定位與地圖構(gòu)建1.SLAM解決思想更新：利用感知模型,結(jié)合當(dāng)前的感知測(cè)量信息zt來更新當(dāng)前t時(shí)刻狀態(tài)xt的后驗(yàn)概率分布Bel(xt,mt)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.3智能機(jī)器人SLAM解決方法η為標(biāo)準(zhǔn)化因子,p(zt|xt)為觀測(cè)模型,Bel-(xt,mt)為先驗(yàn)信度。7.7智能機(jī)器人的同步定位與地圖構(gòu)建1.SLAM解決思想一般而言,SLAM由傳感數(shù)據(jù)的獲取、數(shù)據(jù)關(guān)聯(lián)、定位和地圖構(gòu)建等環(huán)節(jié)構(gòu)成,典型流程如圖所示圖中,Sk表示傳感器測(cè)量所獲取的數(shù)據(jù),Mk-1表示第k-1時(shí)刻的局部地圖,Tk表示k時(shí)刻機(jī)器人的位姿。傳感數(shù)據(jù)獲取包括智能機(jī)器人本身的數(shù)據(jù)采集和環(huán)境的數(shù)據(jù)。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.3智能機(jī)器人SLAM解決方法7.7智能機(jī)器人的同步定位與地圖構(gòu)建1.SLAM解決思想數(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í)上,目前智能機(jī)器人SLAM經(jīng)典的算法主要包括擴(kuò)展卡爾曼濾波器、最大似然估計(jì)、粒子濾波器以及馬爾可夫定位等,而其中擴(kuò)展卡爾曼濾波器(EKF)和粒子濾波的方法是來自于上述貝葉斯濾波器的估計(jì)狀態(tài)后驗(yàn)概率分布的思想。北京信息科技大學(xué)自動(dòng)化學(xué)院課程《智能機(jī)器人原理與應(yīng)用》7.7.3智能機(jī)器人SLAM解決方法7.7智能機(jī)器人的同步定位與地圖構(gòu)建2.卡爾曼濾波卡爾曼濾波在數(shù)學(xué)上是一種統(tǒng)計(jì)估算方法,是通過處理一系列帶有誤差的實(shí)際量測(cè)數(shù)據(jù)而得到的物理參數(shù)的最佳估算,其思想是利用前一時(shí)刻的數(shù)據(jù)和誤差信息來估計(jì)當(dāng)前時(shí)刻的數(shù)據(jù)?？柭鼮V波器用反饋控制的方法估計(jì)過程狀態(tài):濾波器估計(jì)過程某一時(shí)刻的狀態(tài),然后以(含噪聲的)測(cè)量變量的方式獲得反饋。因此卡爾曼濾波器可分為兩部分:預(yù)測(cè)和更新。預(yù)測(cè)負(fù)責(zé)及時(shí)