原文地址是：https://blog.csdn.net/huapiaoxiang21/article/details/81504395
好長時間沒寫部落格了，今天繼續工作的第一件事情是做一款STM32 的ROS智慧移動機器人，構建地圖以及自主導航。在這裡筆者認為最難的事情是搞機器人底盤，由於之前沒有接觸過32板，查了一週的文件才清楚，32板是怎麼燒錄程式的以及與arduino控制電機的區別。

    搭建ROS智慧小車，筆者認為可以將移動機器人分為三層，下位機控制電機以及對把里程計資料傳遞給上位機（我們使用的是直流無刷電機），中間層負責通訊（和下位機制定通訊協議、模式的選擇、資料的轉換並以topic的形式釋出），上位機負責融合下位機傳遞的里程計資料和外感測器採集的資料，構建地圖、定位、以及導航功能。

    中間層的部分為三步，第一步在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的基本通訊。

    九月份一直在看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位元組]

選擇速度模式時

串列埠傳送
右輪速度(4位元組)
左輪速度(4位元組)
單位
mm/s
mm/s
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,這種方法只是跑了網上的資料集，增加了迴環檢測的功能，構建的地圖比較清晰，目前在看這些方法的原始碼，後期再說體會。

作者：huapiaoxiang21
來源：CSDN
原文：https://blog.csdn.net/huapiaoxiang21/article/details/81504395
版權宣告：本文為博主原創文章，轉載請附上博文連結！