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

相信大家的機(jī)器人平臺(tái)STM32端底層控制和機(jī)器人urdf建模都已經(jīng)順利完成了，在正式開(kāi)始ros端編寫(xiě)機(jī)器人啟動(dòng)功能包之前，我們還不得不學(xué)習(xí)一些必要的理論知識(shí)。別擔(dān)心數(shù)學(xué)不好，這里基本都是高中數(shù)學(xué)。下面我們開(kāi)始，Are you ready?沒(méi)準(zhǔn)備好也開(kāi)始了。

3.5.1 常用坐標(biāo)系系統(tǒng)模型

在移動(dòng)機(jī)器人技術(shù)研究中，最為常用的坐標(biāo)系統(tǒng)是笛卡爾坐標(biāo)系統(tǒng)。而在SLAM算法研究中，有如下幾個(gè)比較常見(jiàn)的笛卡爾坐標(biāo)系統(tǒng)，它們分別為機(jī)器人坐標(biāo)系XR YR OR 、傳感器坐標(biāo)系XS YS OS 、世界坐標(biāo)系XW YW OW 。顧名思義，世界坐標(biāo)系是描述機(jī)器人全局信息的坐標(biāo)系;機(jī)器人坐標(biāo)系是描述機(jī)器人自身信息的坐標(biāo)系;傳感器坐標(biāo)系是描述傳感器信息的坐標(biāo)系。他們之間的關(guān)系如下圖所示：

從圖中我們可以知道，首先世界坐標(biāo)系是固定不變的，機(jī)器人坐標(biāo)系和傳感器坐標(biāo)系是在世界坐標(biāo)系下描述的。這里我們看到的是俯視圖，機(jī)器人坐標(biāo)系和傳感器坐標(biāo)系原點(diǎn)重合但是存在一定的角度，不同的機(jī)器人坐標(biāo)系關(guān)系是不同的。當(dāng)我們使用傳感器數(shù)據(jù)時(shí)，這些坐標(biāo)系間的關(guān)系就是我們變換矩陣的參數(shù)，因?yàn)閭鞲衅鞯臄?shù)據(jù)必定是要變換到機(jī)器人坐標(biāo)系或者世界坐標(biāo)系中使用的。

3.5.2 移動(dòng)機(jī)器人位姿模型

移動(dòng)機(jī)器人的位姿模型就是機(jī)器人在世界坐標(biāo)系下的狀態(tài)。常用隨機(jī)變量Xt =(xt ,yt ,θt )來(lái)描述t時(shí)刻的機(jī)器人在世界坐標(biāo)系下的狀態(tài)，簡(jiǎn)稱位姿。其中(xt ,yt )表示的在t時(shí)刻機(jī)器人在世界坐標(biāo)系下的位置，θt 表示機(jī)器人的方向。默認(rèn)世界坐標(biāo)系的X正半軸為正方向，逆時(shí)針旋轉(zhuǎn)為旋轉(zhuǎn)正方向，初始時(shí)刻機(jī)器人坐標(biāo)系和世界坐標(biāo)系重合。某時(shí)刻t機(jī)器人的位姿描述如下圖所示：

3.5.3 移動(dòng)機(jī)器人里程計(jì)模型

簡(jiǎn)單的說(shuō)，移動(dòng)機(jī)器人的里程計(jì)就是機(jī)器人每時(shí)每刻在世界坐標(biāo)系下位姿狀態(tài)。常用的激光SLAM和導(dǎo)航算法通常都需要移動(dòng)機(jī)器人的里程計(jì)作為控制輸入。這也是我們對(duì)本章理論內(nèi)容講解的必要性。

不同底盤(pán)的里程計(jì)模型有所不同，本文以兩輪差分輪式機(jī)器人為例介紹里程計(jì)模型以及如何計(jì)算里程計(jì)。在介紹里程計(jì)模型之前，要先介紹差分輪式機(jī)器人的運(yùn)動(dòng)學(xué)模型，這樣可以先了解一下機(jī)器人的物理特性。首先明確差分模型的機(jī)器人始終做的是以R為半徑的圓弧運(yùn)動(dòng)。如下圖所示，機(jī)器人的線速度V、角速度ω，左右輪速用VL和VR表示，用D表示輪間距，D=2d，右輪到旋轉(zhuǎn)中心的距離為L(zhǎng)。

ROS端給機(jī)器人底盤(pán)(STM32端)發(fā)送的是機(jī)器人要達(dá)到的線速度V和角速度ω，而我們底盤(pán)控制板需要的是左右輪速VL和VR來(lái)進(jìn)行速度控制。所以，我們通過(guò)高中知識(shí)可以得到他們之間的關(guān)系：

上面的公式是由角速度和線速度的關(guān)系得來(lái)，如果不知道這個(gè)關(guān)系，那就自行百度吧，我相信你一定知道。從這些公式我們是不是發(fā)現(xiàn)了什么，機(jī)器人的輪間距影響著我們向左右輪分發(fā)速度以及合成角速度。

有了上面坐標(biāo)系系統(tǒng)模型、位姿模型的基礎(chǔ)，里程計(jì)就非常簡(jiǎn)單了。里程計(jì)的計(jì)算是指以機(jī)器人上電時(shí)刻為世界坐標(biāo)系的起點(diǎn)(機(jī)器人的航向角是世界坐標(biāo)系X正方向)開(kāi)始累積計(jì)算任意時(shí)刻機(jī)器人在世界坐標(biāo)系下的位姿。通常計(jì)算里程計(jì)方法是速度積分推算：通過(guò)左右電機(jī)的編碼器測(cè)得機(jī)器人的左右輪的速度VL和VR，在一個(gè)短的時(shí)刻△t內(nèi)，認(rèn)為機(jī)器人是勻速運(yùn)動(dòng)，并且根據(jù)上一時(shí)刻機(jī)器人的航向角計(jì)算得出機(jī)器人在該時(shí)刻內(nèi)世界坐標(biāo)系上X和Y軸的增量，然后將增量進(jìn)行累加處理，關(guān)于航向角θ采用的IMU的yaw值。然后根據(jù)以上描述即可得到機(jī)器人的里程計(jì)。具體計(jì)算如下圖所示：

至此，里程計(jì)模型原理及計(jì)算就結(jié)束了。

3.5.4 2D激光雷觀測(cè)模型

我們做移動(dòng)機(jī)器人平臺(tái)少不了使用2D激光雷達(dá)，所以了解激光雷達(dá)的觀測(cè)模型也是有必要的。

激光雷達(dá)通常由精準(zhǔn)控制的旋轉(zhuǎn)電機(jī)、紅外激光發(fā)射器、紅外接收視覺(jué)系統(tǒng)和主控組成。

激光雷達(dá)的測(cè)距原理分為兩種，一種是基于三角測(cè)距，另一個(gè)是基于TOF(飛行時(shí)間)。基于三角測(cè)距的激光雷達(dá)表現(xiàn)出的特點(diǎn)，價(jià)格便宜、中近距離測(cè)距較準(zhǔn)確、遠(yuǎn)距離精度差。基于TOF的激光雷達(dá)表現(xiàn)出來(lái)的特點(diǎn)，價(jià)格昂貴、測(cè)距精度高、測(cè)距范圍廣、掃描頻率高。

本文采用市面上比較便宜的基于三角測(cè)距的低成本激光雷達(dá)RPLIDAR A1。該激光雷達(dá)在每次測(cè)距過(guò)程中，發(fā)射器發(fā)射紅外激光信號(hào)，視覺(jué)采集系統(tǒng)接收激光反射信號(hào)。在經(jīng)過(guò)主控實(shí)時(shí)解算后，將激光雷達(dá)幾何中心到被測(cè)物體的距離值以及當(dāng)時(shí)的角度值，通過(guò)主控的通信接口發(fā)出。RPLIDAR A1 工作原理如圖所示。

激光雷達(dá)在整個(gè)SLAM和導(dǎo)航中起著不可替代的作用。第一，通過(guò)激光雷達(dá)觀測(cè)數(shù)據(jù)與地圖進(jìn)行匹配，估計(jì)出機(jī)器人的位姿;第二，當(dāng)機(jī)器人估計(jì)出較準(zhǔn)確的位姿時(shí)，通過(guò)激光雷達(dá)的觀測(cè)數(shù)據(jù)建立環(huán)境地圖;第三，在機(jī)器人導(dǎo)航過(guò)程中，檢測(cè)地圖中的未知障礙物。本文主要使用的觀測(cè)傳感器是2D激光雷達(dá)RPLIDAR A1。

RPLIDAR A1是一款360度激光雷達(dá)，角度分辨率≤1，輸出的每一個(gè)激光點(diǎn)的數(shù)據(jù)都是使用極坐標(biāo)的方式描述，一個(gè)距離值以及一個(gè)對(duì)應(yīng)的角度值。

3.5.5 總結(jié)

其實(shí)關(guān)于差分輪式機(jī)器人搭建的數(shù)學(xué)知識(shí)就這些。對(duì)，當(dāng)然這里只是說(shuō)的搭建，而不是以后的算法。

好好理解這些內(nèi)容吧，下一節(jié)，我們就開(kāi)始ROS小車(chē)的啟動(dòng)文件編寫(xiě)了，我們要通過(guò)C++代碼，完成一個(gè)機(jī)器人啟動(dòng)功能包。在ROS中啟動(dòng)該功能包，我們就可以通過(guò)ROS的鍵盤(pán)功能包或者ROS app控制我們的機(jī)器人了。

同時(shí)，我們也會(huì)啟動(dòng)傳感器獲取導(dǎo)航和建圖需要的深度信息，發(fā)布里程計(jì)odom，發(fā)布tf維護(hù)base_link到odom的tf變換。完成下一章我們r(jià)os端的第一大步就結(jié)束了，也就是說(shuō)大家基本的ROS機(jī)器人平臺(tái)搭建完畢了，之后可以根據(jù)ROS學(xué)習(xí)自行發(fā)揮了，不過(guò)還是可以繼續(xù)看我的文章滴。