3.5 移動機器人系統模型

相信大家的機器人平臺STM32端底層控制和機器人urdf建模都已經順利完成了，在正式開始ros端編寫機器人啟動功能包之前，我們還不得不學習一些必要的理論知識。別擔心數學不好，這里基本都是高中數學。下面我們開始，Are you ready?沒準備好也開始了。

3.5.1 常用坐標系系統模型

在移動機器人技術研究中，最為常用的坐標系統是笛卡爾坐標系統。而在SLAM算法研究中，有如下幾個比較常見的笛卡爾坐標系統，它們分別為機器人坐標系XR YR OR 、傳感器坐標系XS YS OS 、世界坐標系XW YW OW 。顧名思義，世界坐標系是描述機器人全局信息的坐標系;機器人坐標系是描述機器人自身信息的坐標系;傳感器坐標系是描述傳感器信息的坐標系。他們之間的關系如下圖所示：

從圖中我們可以知道，首先世界坐標系是固定不變的，機器人坐標系和傳感器坐標系是在世界坐標系下描述的。這里我們看到的是俯視圖，機器人坐標系和傳感器坐標系原點重合但是存在一定的角度，不同的機器人坐標系關系是不同的。當我們使用傳感器數據時，這些坐標系間的關系就是我們變換矩陣的參數，因為傳感器的數據必定是要變換到機器人坐標系或者世界坐標系中使用的。

3.5.2 移動機器人位姿模型

移動機器人的位姿模型就是機器人在世界坐標系下的狀態。常用隨機變量Xt =(xt ,yt ,θt )來描述t時刻的機器人在世界坐標系下的狀態，簡稱位姿。其中(xt ,yt )表示的在t時刻機器人在世界坐標系下的位置，θt 表示機器人的方向。默認世界坐標系的X正半軸為正方向，逆時針旋轉為旋轉正方向，初始時刻機器人坐標系和世界坐標系重合。某時刻t機器人的位姿描述如下圖所示：

3.5.3 移動機器人里程計模型

簡單的說，移動機器人的里程計就是機器人每時每刻在世界坐標系下位姿狀態。常用的激光SLAM和導航算法通常都需要移動機器人的里程計作為控制輸入。這也是我們對本章理論內容講解的必要性。

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

ROS端給機器人底盤(STM32端)發送的是機器人要達到的線速度V和角速度ω，而我們底盤控制板需要的是左右輪速VL和VR來進行速度控制。所以，我們通過高中知識可以得到他們之間的關系：

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

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

至此，里程計模型原理及計算就結束了。

3.5.4 2D激光雷觀測模型

我們做移動機器人平臺少不了使用2D激光雷達，所以了解激光雷達的觀測模型也是有必要的。

激光雷達通常由精準控制的旋轉電機、紅外激光發射器、紅外接收視覺系統和主控組成。

激光雷達的測距原理分為兩種，一種是基于三角測距，另一個是基于TOF(飛行時間)。基于三角測距的激光雷達表現出的特點，價格便宜、中近距離測距較準確、遠距離精度差?；赥OF的激光雷達表現出來的特點，價格昂貴、測距精度高、測距范圍廣、掃描頻率高。

本文采用市面上比較便宜的基于三角測距的低成本激光雷達RPLIDAR A1。該激光雷達在每次測距過程中，發射器發射紅外激光信號，視覺采集系統接收激光反射信號。在經過主控實時解算后，將激光雷達幾何中心到被測物體的距離值以及當時的角度值，通過主控的通信接口發出。RPLIDAR A1 工作原理如圖所示。

激光雷達在整個SLAM和導航中起著不可替代的作用。第一，通過激光雷達觀測數據與地圖進行匹配，估計出機器人的位姿;第二，當機器人估計出較準確的位姿時，通過激光雷達的觀測數據建立環境地圖;第三，在機器人導航過程中，檢測地圖中的未知障礙物。本文主要使用的觀測傳感器是2D激光雷達RPLIDAR A1。

RPLIDAR A1是一款360度激光雷達，角度分辨率≤1，輸出的每一個激光點的數據都是使用極坐標的方式描述，一個距離值以及一個對應的角度值。

3.5.5 總結

其實關于差分輪式機器人搭建的數學知識就這些。對，當然這里只是說的搭建，而不是以后的算法。

好好理解這些內容吧，下一節，我們就開始ROS小車的啟動文件編寫了，我們要通過C++代碼，完成一個機器人啟動功能包。在ROS中啟動該功能包，我們就可以通過ROS的鍵盤功能包或者ROS app控制我們的機器人了。

同時，我們也會啟動傳感器獲取導航和建圖需要的深度信息，發布里程計odom，發布tf維護base_link到odom的tf變換。完成下一章我們ros端的第一大步就結束了，也就是說大家基本的ROS機器人平臺搭建完畢了，之后可以根據ROS學習自行發揮了，不過還是可以繼續看我的文章滴。