移動(dòng)機(jī)器人同時(shí)定位和地圖創(chuàng)建(SLAM)原理：機(jī)器人的位姿和地圖信息

機(jī)器人通過外部傳感器信息獲得 周圍環(huán)境相對位置信息來確定自身位姿的過程。在全局定位中，由于機(jī)器人只能觀測到 環(huán)境中的一些局部信息，因而環(huán)境的對稱性可能會(huì)使機(jī)器人在很多不同的位置獲得完全 相同的觀測信息，產(chǎn)生多個(gè)可能的機(jī)器人位姿假設(shè)。全局定位中機(jī)器人位姿的后驗(yàn)概率 分布是一個(gè)多峰分布，基于高斯分布的不確定性表示方法不適用于全局定位。在全局定 位過程中如果在移動(dòng)機(jī)器人里程計(jì)沒有記錄的情況下，將機(jī)器人從一個(gè)地方搬到另外一 個(gè)地方會(huì)產(chǎn)生機(jī)器人“誘拐”(Kidnapped Robot)現(xiàn)象[148]。

當(dāng)將機(jī)器人從A點(diǎn)搬到B 點(diǎn)時(shí)，機(jī)器人里程計(jì)無法記錄實(shí)際位姿變 化。根據(jù)傳感器信息，機(jī)器人也不能立刻確定自己在環(huán)境中的位置，需要根據(jù)多次觀測信 息重新定位。所以很多研究者認(rèn)為機(jī)器人“誘拐”問題也是一個(gè)機(jī)器人的全局定位問 題。與全局定位不同的是，機(jī)器人可能不知道自己什么時(shí)候發(fā)生了“誘拐”,該方法常用 來測試定位方法的魯棒性。在全局定位過程中，存在很多不確定性因素。先是機(jī)器人 本身的不確定性，如輪子打滑所造成的里程計(jì)誤差累積，傳感器噪聲所造成的讀數(shù)不可 信。其次是機(jī)器人所處的環(huán)境也是不可預(yù)知的，如人的走動(dòng)和物體的移動(dòng)所造成的環(huán)境 變化。這些不確定性會(huì)使定位變得更加困難。因此，近來越來越多的研究者一方面采用 可靠的傳感器，如采用能感知豐富環(huán)境信息的視覺傳感器和測距精度較高的激光測距器； 另一方面把概率理論應(yīng)用到移動(dòng)機(jī)器人定位中，試圖采用概率定位方法解決不確定性問 題。

SLAM的基本原理是利用已經(jīng)創(chuàng)建的地圖修正基于運(yùn)動(dòng)模型的機(jī)器人位姿估計(jì)誤差，提高定位精度；同時(shí)根據(jù)可靠的機(jī)器人位姿，創(chuàng)建出精度更高的地圖[149]。

未知環(huán)境下的SLAM方法的核心都是圍繞和針對傳感器信息和環(huán)境中存在的不確定 性而展開的。關(guān)于傳感器的不確定，以Z常用的里程計(jì)為例，其典型的誤差積累如圖1.61 所示。其中，左圖是d立利用里程計(jì)定位、d立利用激光傳感器感知環(huán)境所創(chuàng)建的地圖， 由于沒有進(jìn)行里程計(jì)誤差補(bǔ)償，幾次創(chuàng)建的地圖差異很大，與實(shí)際環(huán)境也不符；右圖是采用SLAM 創(chuàng)建的地圖，基于SLAM 可以利用已創(chuàng)建的地圖修正里程計(jì)的誤差。這樣機(jī)器 人的位姿誤差就不會(huì)隨著機(jī)器人的運(yùn)動(dòng)距離的增大而無限制增長，因此可以創(chuàng)建精度更 高的地圖，也同時(shí)解決了未知環(huán)境中的機(jī)器人定位問題。

在SLAM中，系統(tǒng)的狀態(tài)由機(jī)器人的位姿和地 圖信息(包含各特征標(biāo)志的位置信息)組成。假設(shè) 機(jī)器人在t 時(shí)刻觀測到了特征m₁, 如圖1 . 62所 示。根據(jù)觀測信息只能獲得特征m₁ 在機(jī)器人坐標(biāo) 系R 中的坐標(biāo)，機(jī)器人需要估計(jì)機(jī)器人自己本身在 世界坐標(biāo)系W中的位姿，然后通過坐標(biāo)變換才能 計(jì)算特征的世界坐標(biāo)。可見，在地圖創(chuàng)建的過程 中，需要計(jì)算機(jī)器人的位姿，也就是進(jìn)行機(jī)器人的 定位。然而，根據(jù)里程計(jì)獲得的機(jī)器人位置信息很 不準(zhǔn)確，顯然錯(cuò)誤的位置信息將會(huì)導(dǎo)致地圖的不準(zhǔn) 確。

在初始時(shí)刻，地圖中沒有任何特征。當(dāng)機(jī)器人在t 時(shí)刻觀測到特征m 時(shí)，可以根據(jù)機(jī) 器人的位姿(x,y:,θ.), 以及觀測到的特征m₁ 在機(jī)器人坐標(biāo)系下的坐標(biāo)計(jì)算出特征的世 界坐標(biāo)(x¹,y!), 并且將特征m₁ 加入到地圖中。當(dāng)機(jī)器人運(yùn)動(dòng)一步之后，根據(jù)里程計(jì)信息 可以預(yù)測到機(jī)器人的位姿將變?yōu)?x+1,Y:+1,0,+1)。根據(jù)特征m₁ 的世界坐標(biāo)(x¹,y) 可 以 計(jì)算出當(dāng)機(jī)器人位姿為(x:+1,y:+1,0.+1) 時(shí) m₁ 在機(jī)器人坐標(biāo)系下的坐標(biāo)。然而，機(jī)器人同 時(shí)也會(huì)再次觀測到特征m, 而且獲得m₁ 在新的機(jī)器人坐標(biāo)系下的坐標(biāo)(x+1,y+1) 。 實(shí) 際 觀測到的特征坐標(biāo)與計(jì)算獲得的坐標(biāo)(x^+1,y+1) 將存在差別。這種差別是由兩種原因引 起的，其一是因?yàn)闄C(jī)器人的預(yù)測位姿(x:+1,y:+1,0+1)不準(zhǔn)確，其二是因?yàn)楦鶕?jù)以前的觀測 信息計(jì)算獲得的特征的世界坐標(biāo)(x¹,y¹) 不準(zhǔn)確。在SLAM中，根據(jù)這種差別重新計(jì)算特 征的世界坐標(biāo)(也就是地圖創(chuàng)建),同時(shí)重新估計(jì)機(jī)器人的位姿(也就是機(jī)器人的定位)。 當(dāng)機(jī)器人繼續(xù)運(yùn)動(dòng)時(shí)，它將觀測到更多的特征，根據(jù)同樣的方法，機(jī)器人會(huì)把它們加入到 地圖中，并且根據(jù)觀測到的信息更新機(jī)器人的位姿以及它們的世界坐標(biāo)。簡單地說， SLAM利用觀測到的特征計(jì)算它們的世界坐標(biāo)以實(shí)現(xiàn)地圖創(chuàng)建，同時(shí)更新機(jī)器人的位姿 以實(shí)現(xiàn)機(jī)器人的定位，如圖1.63所示。

SLAM 的概率描述為：p(81,MI z1:,uo:,k1)=p(x,I z1:,u₀:-1,k1:),其中，s1:=81, S2, … ,s, 和z1: 分別表示機(jī)器人從1到t 時(shí)刻的運(yùn)動(dòng)路徑和感知信息，u₁-1表 示t-1 到 t 時(shí) 刻的運(yùn)動(dòng)控制信息，z, 表示機(jī)器人的當(dāng)前感知信息。當(dāng)機(jī)器人穿過一個(gè)未知環(huán)境時(shí)，設(shè)t 時(shí)刻機(jī)器人位姿s,=[x₁,y:,θ,], 已經(jīng)觀測到的地圖為M, 其中，m 表示第k 個(gè)路標(biāo)，K表 示已經(jīng)觀測的路標(biāo)數(shù)，k,∈{1,…,N} 表示t 時(shí)刻感知到的路標(biāo)索引號。

系統(tǒng)的完整狀態(tài) 可以表示為x,=[s1,M]T,SLAM 的圖形模式如圖1.64所示，機(jī)器人從位姿s。開始通過控 制命令序列uo,u₁,…,u1-1 移動(dòng)，隨著機(jī)器人的移動(dòng)，附近的路標(biāo)被感知到，時(shí)刻t=1, 感 知 到路標(biāo)m₁, 并獲得測量數(shù)據(jù)z₁ (包括距離和方向),時(shí)刻t=2, 感知到路標(biāo)m₂, 并在時(shí)刻t=3,重新感知到路標(biāo)m₁, 現(xiàn)在已經(jīng)形成的地圖為：M={m₁,m₂,m 。SLAM 的輸入信息是， 路標(biāo)觀測信息z1:, 以及運(yùn)動(dòng)控制信息u₀:-1 。SLAM 的目的是，根據(jù)輸入信息估計(jì)機(jī)器人 運(yùn)動(dòng)路徑s1 以及地圖M。