好長時間沒寫博客了，今天繼續工作的第一件事情是做一款STM32 的ROS智能移動機器人，構建地圖以及自主導航。在這裏筆者認爲最難的事情是搞機器人底盤，由於之前沒有接觸過32板，查了一週的文檔才清楚，32板是怎麼燒錄程序的以及與arduino控制電機的區別。
搭建ROS智能小車，筆者認爲可以將移動機器人分爲三個部分（上位機、中間層以及下位機），下位機控制電機（直流無刷電機）、水泵、繼電器、LED等，最重要的是把里程計數據（X座標、Y座標、方向角、線速度、角速度）實時地傳遞給上位機；中間層負責通信（和下位機制定通信協議、模式的選擇（1.速度模式、2.LED模式、3.繼電器模式）、數據的發送（將cmd中的線速度、角速度轉換成下位機左右輪的速度）、數據的接收（將下位機上傳的odom數據以topic的形式發佈），這裏出現中間層的發佈頻率和ROS實際接收頻率不一致的問題；上位機負責融合下位機傳遞的里程計數據和外傳感器採集的數據，減少累計誤差後，實現構建地圖、定位、以及導航功能。
中間層的部分爲三步，第一步在PC機架構ROS操作系統（推薦16.04版本）比較穩定，另外不推薦虛擬機。最好是在PC機上安裝雙系統，可以通過遠程控制（ssh）到arduino、樹莓派或者工控機爲主板的主控機。第二步是在arduino、樹莓派或者工控機上架構ROS操作系統，最好是和PC機的版本一致。完成簡單的部分後，接下來到第三步，也就是最核心部分：ROS系統和下位機進行通信，通信協議中包括串口、波特率、流量控制、停止位以及字符大小的定義，在後期我會上傳串口程序的源代碼。
前段時間在調試串口通信的過程中遇到了一些問題，在博客中先一一列舉出來，例如在知乎大神的對話中，地址是：https://www.zhihu.com/question/32120691，下載了STM32的驅動，地址是：https://github.com/spiralray/stm32f1_rosserial，移植在工控機上並調試STM32的串口通信，出現了串口無法識別的問題，沒辦法由於第一次搞串口通信這塊的東西會比較費時間，網上的方法基本上都基本上嘗試了一遍沒有解決問題，然後又進入ROS官網中下載rosserial包，地址： http://wiki.ros.org/rosserial有了數據類型不匹配的問題，又在嘗試各種解決串口的工具，包括添加串口工具，搞了好幾天基本上都是在發現問題，解決問題。然後參考其他文章，下定決心自己寫一個串口通信協議。在這裏推薦一篇文章 ros與下位機通信常用的c++ boost串口應用--22 原文文章來自http://www.cnblogs.com/zxouxuewei/，
由於筆者安裝的Boost的版本是1.58,在boost_node包中的CMakList.txt中添加
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
genmsg )
add_executable(boost_node src/boost_node.cpp)
target_link_libraries(boost_node ${catkin_LIBRARIES}
此外，讀者如果沒有boost 庫，需要在編譯時鏈接自己boost庫所在的位置。可以在boost_node包中新建一個build,然後成功編譯後，在catkin_ws中用catkin_make編譯，在lib中生成一個可執行文件boost_node，執行rosrun boost_node boost_node，會出現buffer[]的返回值，當然也可以多定義buf[]={1,2};最後返回buf[0]的值，打印出來，可以完成ROS與STM32的簡單通信。
如果想更深層次的瞭解ROS和與STM32的串口通信，請大家參考大神的這篇博客中的base_controller ,可以借鑑博客中串口的收發數據的類型與格式，https://blog.csdn.net/Forrest_Z/article/details/55002484 其實串口通信部分就是實現數據正常傳輸，我們在寫ROS和與STM32的串口通信裏面添加了模式，爲了區分在不同模式下的數據發送；中間層的數據接收部分現在遇到了發送odom的頻率和ROS中顯示的頻率不一致的問題，很糟心，剛開始以爲是串口的問題，在別的小車上試了之後，顯然不是這個問題，有可能是下位機數據傳送數據太慢導致的，當然是猜測，馬上要去嘗試。有哪位大神解決了這個問題，可以留言，在此謝過。
九月份一直在看tf座標轉化這塊的內容，在做激光SLAM或者視覺SLAM中有幾個座標系會用到tf座標轉換，例如base_laser和base_footprint之間，在底盤這塊會用到base_footprint座標和odom座標之間的轉換，其實最終判斷誤差大小是根據map和odom之間的重合度來判定的。tf轉換會在ROS中調用tf包，數據直接轉換，很贊。想弄清tf之間的轉換關係直接在RVIZ中跑機器人仿真模型，然後可視化座標系，會發現中間會有兩個重要的角度，方向角和偏航角。
偏航角是機器人的初始位置和當前位置存在的角度，是累加角度，而方向角是通過轉彎角速度w和時間t計算出來. 偏航角以ROS中大量用到的四元數形式表示。其中在三維可視化RVIZ中有兩個座標odom和map，機器人原點位置時，這兩個座標是完全重合的，而當機器人運動後，odom數據會因爲輪子出現打滑，空轉以及其他問題出現累計誤差，而誤差體現在odom座標和map座標的重合程度。在做boost_node這塊，base_footprint座標是子座標，odom是父座標，讀者可以模擬一個odom數據（在ROS 機器人程序設計第二版的第八章有具體的模擬方法），可以在RVIZ中觀測到虛擬的機器人使用假的odom數據做圓周運動。關於odom數據的發佈中會涉及到機器人的位姿信息，也就包括位置和姿態的計算。
odom數據包括（X、Y座標，方向角，線速度，角速度）mm，mm,rad,mm/s,rad/s。沒有接觸底盤時，不太清楚控制電機的方式，可以參考移動中間層的解釋，個人覺得兩個電機同正時向前，同負向後運動這種方式會好點。因爲後期會根據編碼器數據計算里程計數據，這樣計算會準確點，另外這裏會牽扯到偏航角的計算，個人建議先用編碼器計算機器人相對初始位置逐漸累加的方向角信息,也就是下文出現的th=th+。X、Y座標是機器人相對初始位置經過逐漸累加計算，在odom數據中的''線速度、角速度"與通常理解的線速度、角速度不同，odom中的線速度決定機器人的前進速度，角速度決定機器人的轉彎角速度。
機器人轉彎現在基本上是三種轉彎方法：1.車輪的左右輪分別正傳和反轉。2.左右輪正傳，左右輪的速度一致。3.原地轉彎，其中一個輪有速度，另外一個輪速度爲0。在ROS機器人轉彎基本上都採用的是差速轉彎方式，在這裏筆者推薦一篇大神的博客：https://blog.csdn.net/forrest_z/article/details/55001231
移動機器人中間層解釋
1.串口發送
（1）內容：左右輪速度，單位爲mm/s
（2）格式：5個字節,[幀頭0x68,0x01 2字節][模式1字節] [幀尾0x0d,0x0a 2字節]
串口下發數據包括模式選擇，當然一般都是速度模式，但是其他模式也是上位機通過串口往下發送。可以選擇線速度或者角速度中的一個值判別。
選擇01 速度模式時
|
串口發送 |
右輪速度(4字節) |
左輪速度(4字節) |
|
單位 |
mm/s |
mm/s |
選擇02 LED模式時
選擇03 計算器模式
2.串口接收
（1）內容：小車X,Y座標(mm)，方向角(rad)，線速度(mm/s)，轉彎角速度(rad/s)
（2）格式：24字節，[幀頭0x68,0x01 2字節] [Ｘ座標４字節][Ｙ座標４字節][方向角４字節][線速度４字節][角速度４字節][ 幀尾0x0d,0x0a 2字節]
|
串口接收 |
X座標（position_x=0） |
Y座標 (position_y=0) |
方向角 (oriention=0) |
線速度（表示機器人平移速度） （vel_linear=0） |
角速度（表示機器人的轉彎速度） （vel_angular=0） |
|
單位 |
mm |
mm |
Rad |
mm/s |
Rad/s |
小車控制思想控制電機轉動。
1. 電機的控制我們分爲兩部分，一部分爲電機轉動方向的控制，另一個爲電機轉速的控制。電機轉動的方向我們用兩個MCU引腳來控制，假如PIN_A=1，PIN_B=0 時，電機正轉；PIN_A=0，PIN_B=1 時，電機反轉；PIN_A=0，PIN_B=0 時，電機停止。電機速度的控制則需要一個PWM輸出引腳，我們通過控制輸出不同的PWM值來控制電機轉動的速度。
2. PID控制。如果我們想控制小車以一米每秒的速度做直線，但由於地面的摩擦阻力的影響，會造成左右輪速度與我們想控制的速度不同，所以會走不直，這時我們就需要加入PID控制，PID控制的思想就是我實時的把輪子真正的速度採集回來和控制的速度對比，差則補，多則減。這樣基本就可以實現理想控制。
3. 小車轉彎控制。一般我們要是想控制小車以多少的速度前進或者後退，我們只需要PID控制兩個輪子的速度一致就可以基本做到。但如果要想控制小車以多少的角速度轉彎，我們需要做一定的計算。
在ROS中，機器人的位置[position: x，y，z]和方向[orientation: x，y，z，w]被定義爲姿態。該位置由x、y和z三個向量描述，而方向使用四元數形式的x、y、z和w。
圖1 導航推測所需要的信息(中心位置(x，y)，車輪間距離D，車輪半徑r)
圖2 導航推測(dead reckoning)
當有如圖1的移動機器人在運動過程中，設D是車輪之間的距離，r是車輪的半徑。如圖2所示，當機器人在時間內移動很短距離時，利用左右電機旋轉量(當前編碼器值、和之前的編碼器值 和)來計算出左右車輪的轉速（）,如式1-1和1-2所示。
如式1-3和1-4求出左右輪的移動速度（V），並如式1-5和1-6求出機器人的平移速度（linear velocity: ）和旋轉速度（angular velocity：）。
注：下位機通過串口上傳的X、Y座標和方向角，都是從初始時刻到當前時刻經過不斷累加後得到的數據。
我們底盤最大轉速是3000轉/min, 減速比是1:30，驅動輪直徑是D=125mm,軸間距是370mm，通過計算得Vmax=0.65m/s, 0.5Vmax=0.325m/s。我將移動機器人分爲三層，下位機控制電機以及對把里程計數據傳遞給上位機（我們使用的是直流無刷電機），中間層負責通信（和下位機制定通信協議、模式的選擇、數據的轉換並以topic的形式發佈），上位機負責融合下位機傳遞的里程計數據和外傳感器採集的數據，構建地圖、定位、以及導航功能。
構建地圖是首先採用開源的是hector_slam,結果發現構建的地圖根本不能用，然後想到的是gmapping_slam，這種方法比較依賴於里程計的數據，現在還在測試階段，另外的是cartgraph_slam,這種方法只是跑了網上的數據集，增加了迴環檢測的功能，構建的地圖比較清晰，目前在看這些方法的源代碼，後期再說體會。
目前遇到的問題有：1.上位機和中間層發佈的頻率不一致。
2.轉彎方式的制定（差速轉彎）
3.數據拼接問題
4.下位機最大速度是0.65,百分制問題
5.四元數計算問題
6.對tf座標的理解
補充：
ubuntu下的串口工具是cutecom,親測可用，不用再去win7系統下進行測試底盤
1. 安裝cutecom
sudo apt-get install cutecom
2. 運行cutecom
sudo cutecom
3.將/dev/ttys0改成/dev/ttyUSB0等