SLAM+語音機器人DIY系列：（三）感知與大腦——3.輪式里程計與運動控制
摘要
在我的想象中機器人首先應該能自由的走來走去，然後應該能流利的與主人對話。朝着這個理想，我準備設計一個能自由行走，並且可以與人語音對話的機器人。實現的關鍵是讓機器人能通過傳感器感知周圍環境，並通過機器人大腦處理並輸出反饋和執行動作。本章節涉及到的傳感器有激光雷達、IMU、輪式里程計、麥克風、音響、攝像頭，和用於處理信息的嵌入式主板。關於傳感器的ROS驅動程序開發和在機器人上的使用在後面的章節會展開，本章節重點對機器人傳感器和嵌入式主板進行講解，主要內容：
3.輪式里程計與運動控制
底盤提供輪式里程計與運動控制，是機器人SLAM建圖與避障導航的基礎。所以對底盤進行了解，和熟悉輪式里程計與運動控制的底層原理是很有益處的。這裏以我們的miiboo機器人的底盤爲例，對底盤上的輪式里程計和運動控制的原理進行分析。
3.1.硬件概述
（圖20）miiboo機器人底盤
底盤主要由電機控制板和帶編碼器的減速電機構成，如圖20。電機控制板通過串口與機器人的大腦（如樹莓派3）相連接，通過接收大腦下發的控制指令，利用PID算法對電機進行控制；同時，採集電機上的編碼器數據發送給大腦，利用航跡推演算法得到底盤的里程計信息。
3.2.輪式里程計與運動控制
（圖21）輪式里程計與運動控制
如圖21，爲輪式里程計與運動控制的系統框圖。首先是機器人大腦發送控制命令，其實就是期望左、右電機達到的目標轉速，我們都知道在一個控制系統中，被控對象很難完全按照期望目標來運行，這就需要引入反饋對被控對象進行實時的閉環控制，讓被控對象儘量逼近期望目標，電機控制板主要就是用來實現這個過程。同時，電機控制板還負責對電機編碼信號進行採樣，將單位採樣時間（一般爲10ms）內的編碼脈衝累計值作爲里程數據發送給機器人大腦，機器人大腦利用航跡推演算法求解出里程計信息。
通信協議：
電機控制板與機器人大腦之間採用串口通信。電機左、右輪期望轉速被封裝到串口的字符串中，作爲控制命令發送給電機控制板；單位時間（一般10ms）內採樣到的電機編碼脈衝累計值（等效爲實際電機速度）作爲里程數據，以同樣的方式被封裝到串口的字符串中發送給機器人大腦。可以看出，控制命令與里程數據遵循一樣的封裝協議，協議具體形式如圖22。
（圖22）通信協議
電機控制：
（圖23）PID算法流程
電機控制最常用的就是PID控制算法，如圖23爲PID算法流程。以電機轉速控制爲例，r(t)就是給定的目標轉速，c(t)就是電機實際運行時的轉速，通過閉環反饋可以求得r(t)與c(t)的偏差值e(t)，PID控制算法中的比例（P）、積分（I）、微分（D）調節器利用e(t)生成新的控制量u(t)，u(t)通過執行機構（電機驅動器）作用於被控對象（電機），電機的實際運行速度c(t)通過閉環反饋，進入下一次PID調節。就這樣，不斷的通過閉環反饋調節，使電機實際運行速度c(t)最終逼近給定的目標速度r(t)。
在連續和離散時間域上PID會有不同的表現形式，在連續時間域上積分、微分調節通過積分計算、微分計算實現，而在離散時間域上積分、微分調節通過累加和、差分計算實現。由於電機控制需要在程序上進行實現，所以需要採用離散域的PID。按照PID算法生成的調節量的形式，又可以分爲位置式PID和增量式PID；位置式PID生成的u(t)爲直接的控制量，增量式PID生成的Δu(t)是控制量的修正量，需要疊加上一次的u(t-1)才能作爲控制量。離散形式的位置式與增量式PID數學表達如圖24。
（圖24）離散形式的位置式與增量式PID數學表達
可以看出，利用位置PID的數學表達式經過簡單的變形就能得到增量PID的數學表達式。增量算法不需要做累加，控制量增量的確定僅與最近幾次誤差採樣值有關，計算誤差或計算精度問題，對控制量的計算影響較小。而位置算法要用到過去的誤差的累加值，容易產生大的累加誤差。增量式算法得出的是控制量的增量，例如閥門控制中、只輸出閥門開度的變化部分，誤動作影響小，必要時通過邏輯判斷限制或禁止本次輸出，不會嚴重影響系統的工作。而位置算法的輸出是控制量的全量輸出，誤動作影響大。增量型PID的程序實現，如如圖25。關於PID參數的整定，將放在後面的文章進行詳細講解。
（圖25）增量型PID的程序實現
差動兩輪底盤輪式里程計：
輪式里程計是機器人底盤的重要組成部分，採用航跡推演算法對機器人的位姿進行估計，並對機器人當前的速度、旋轉速度、左右輪速度進行轉換。無論是機器人的定位導航還是普通的運動控制，都需要輪式里程計。
（圖26）通過航跡推演計算里程計
如圖26，爲通過航跡推演計算里程計的過程。隨時間推移機器人底盤的實時位姿p1、p2、p3、...、pn連接起來就形成了機器人的航跡，考慮很短的時間內兩相鄰機器人位姿p1和p2，在已知機器人位姿p1和機器人當前左右輪速度vl、vr的條件下，利用微積分的思想可以推算出機器人在下一個時刻的位姿p2，通過這樣不斷的推演，就可以計算出機器人當前的位姿以及速度、角速度等信息，這就是所謂的航跡推演。關於航跡推演的具體數學推導和程序實現，將在後面的文章中進行展開講解。
底盤電機控制板軟件框架：
（圖27）stm32電機控制板軟件架構
如圖27，爲stm32電機控制板軟件架構。底盤中的電機控制與里程數據採集的程序在stm32單片機上實現，TIM1定時器產生週期性的循環，循環中進行電機編碼器數據採集、PID計算、電機速度pwm控制，剩下的就是usart1串口與usart2串口跟機器人大腦之間的通信了，底盤debug接口是用於stm32程序開發階段使用的，所以在機器人正常運行的過程中只需要使用底盤控制接口。關於stm32部分的代碼和對應機器人大腦中ROS驅動代碼將在後面介紹。
3.3.在機器人中使用底盤
通過前面的講解，我們已經對機器人底盤的用途及工作原理有了一定的瞭解，並且知道了電機控制和里程計的工作過程。這時候肯定很想知道如何在機器人中把底盤使用起來呢？其實很簡單，和激光雷達、IMU這些傳感器一樣，底盤也可以當做一個傳感器來使用，只不過不同之處是這個傳感器與機器人大腦是雙向交互的，機器人大腦向底盤發送控制命令，底盤反饋里程數據給機器人大腦。但是，不論交互的細節如何，只需要裝上底盤的ROS驅動包，上層算法只需要發佈和訂閱相應的主題就能達到使用底盤的目的。
（圖28）在機器人中使用底盤
底盤通過串口與機器人相連接，機器人中通過運行底盤控制ROS驅動，來實現讀取串口的速度反饋，利用航跡推演算法計算得到里程計併發布到/odom這個主題；底盤控制ROS驅動訂閱/cmd_vel主題的運動控制數據，並轉換爲速度控制指令通過串口發送給底盤。這樣機器人上的其它節點就可以通過發佈/cmd_vel主題來對底盤進行控制，通過訂閱/odom主題獲取底盤的里程計。關於底盤控制ROS驅動、底盤里程計標定、底盤的debug，將在後面的章節中具體講解。
後記
如果大家對博文的相關類容感興趣，或有什麼技術疑問，歡迎加入下面的《SLAM+語音機器人DIY》QQ技術交流羣，一起討論學習^_^
