轉載地址：http://blog.csdn.net/lanhuadechenmo/article/details/46715169
在嘗試完成用AMCL來導航和避障後，下面對Navigation的理解作下總結。本篇的主要內容包括：控制架構，三個接口，tf變換，AMCL，move_base參數解析。結合筆者的開發經驗，參考衆多前輩的經驗以及《ros by example》，給出自己的理解，最後給出自己的思考。希望對大家有所幫助，難免有錯誤和不足之處，請各位指導。
參考（謝謝各位前輩）：
http://blog.exbot.net/archives/1129
http://blog.csdn.net/heyijia0327/article/details/41823809
http://blog.csdn.net/heyijia0327/article/details/41831529
http://m.blog.csdn.net/blog/dxuehui/39347041
http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
http://wiki.ros.org/tf/Tutorials
http://blog.exbot.net/archives/1686
本文引用了不少人的博文，因爲很經典，所以沒有做太多改動。對那些曾經提供幫助的人表示感謝。
第一部分：控制架構/層次
關於機器人運動控制系統架構，在《ros by example》 chapter 7一章第二節中介紹了控制機器人的5個層次，從低到高依次是：motor controllers anddrivers-> ROS base controller ->Frame-Base Motion（move_base）->Frame-Base Motion（gmapping + amcl）->Semantic Goals。總結起來如下圖所示：
其實，我們可以理解爲以下幾個問題：電機如何控制？小車平臺怎麼跟上位機連接，如何傳送數據？如何進行路徑規劃？如何建立環境地圖？如何設定運動的目標？這裏我也參考別人的思路，把這些分爲三個層次。
1.最底層：機器人本身的電機驅動和控制部分，驅動器接收的是電機左右輪期望速度，根據期望速度對左右輪分別進行 PID控速。同時，定時採樣電機碼盤值，並轉化爲左右輪速度值上傳給電機控制器。控制器將左右輪的速度值，結合機器人的運動學模型，用航跡推演法推算出機器人當前的速度，包括線速度和角速度。控制器也可以接受機器人的控制指令（cmd_vel），將速度解算爲左右輪的期望速度，從而給電機的驅動部分，控制電機轉動。它的輸入是控制速度值，輸出是電機的轉速。
2.中間通信層：電腦端和機器人平臺的控制通信。電腦將發出的速度指令傳給平臺的控制部分，以及電腦端接受控制部分的當前速度，然後發佈/odom topic，讓其他節點訂閱，從而進行機器人位置估計。
3.決策層：與導航有關，建立地圖和實現定位，然後用move_base根據你發佈的傳感器信息做出路徑規劃，輸出機器人的速度和轉向速度。這部分主要與ROS中的Navigation stack有關，可以調用相應的package，如amcl，gmapping，move_base，map_server等。它的輸入是各種傳感器的信息和地圖，輸出是速度cmd_vel。
總結：不同的人開發平臺不同，思路也不同。但大體可以按照這三個部分來理解。
第二部分：三個接口
在瞭解了整個系統的架構之後，我們可以考慮下各部分之間實現的接口問題。包括上位機和下位機之間的通信問題，一般機器人都使用的里程計的接口問題，以及其他的外接傳感器問題。明白各個接口的輸入和輸出，以及怎樣提取數據進行轉換。
1.速度接口
在我們瞭解move_base之後，會知道導航規劃層的輸出是cmd_vel topic, 數據類型爲 geometry_msgs/Twist 。對於移動平臺來說，有差分和全向之分，對於一般的差分平臺，我們只用到了linear.x 和angular z兩個值，對於全向平臺，還多了一個linear.y。不同類型的移動平臺，運動學不同，因此將速度值解算成電機的轉速值的方式也是不同的。底層平臺一般接受的是輪子的期望轉速，如果平臺已經封裝完好，則一般接受的是上層發出的速度。上層和下層之間的通信一般通過串口。
如果是開發的下位機或者底層平臺，需要注意速度的平滑，可以用插值的方法平滑速度，避免因爲過大的加減速等造成的下危機或底層平臺無法正常運動。筆者所在的團隊開發的一個移動平臺過程中遇到此問題，後來加入插值後運動明顯平穩許多。
思路：接收上層發佈的cmd_vel topic，然後將該速度做處理，通過運動學模型解算出期望的輪子的速度，發送給下位機執行。
2.里程計接口
里程計屬於內部傳感器，一般的平臺都會裝入里程計進行航跡推測，對機器人平臺的位置進行粗略的估計。即：通過編碼器的轉動推測輪子在時間片中的位移，進一步算出機器人整體的位移與速度。導航一般都會要求一個里程計數據的輸入，里程計是相對於機器人中心的位置推測，誤差會累積。這裏需要注意的是發佈頻率，它涉及到之後的costmap更新與座標系的訪問超時問題。
3.傳感器接口
一般常用的外部傳感器有laser ,rgbd-camera,camera,gyroscope等，都可以找到相應的驅動，roslaunch相應文件就可以了。這裏要提到static tf，傳感器一般在消息的 header中都需要相應的傳感器採集座標系，當laser,camera等固定好之後，可以用靜態tf來將傳感器所在的座標系的數據變換到以機器人中心所在的座標系，便於信息的傳遞。
總結：就是在硬件要實現，就要考慮各個模塊之間的連接，即是接口問題。硬件的問題不能小看，很多時候軟件上出現的問題根本原因可能在於硬件。
第三部分：tf
介紹：
tf庫這本部分很關鍵，是很容易出錯的地方。建議在瞭解接口問題之後，寫程序之前，先把要整個系統的tf tree 理清楚，這個過程中可以確定你的信息的傳遞，如果信息的傳遞沒有問題，那麼tftree 一般也沒有問題，反過來亦然。在調試程序時也可以根據錯誤提示，逐步去解決tf 問題。
tf庫的目的是實現系統中任一個點在所有座標系之間的座標變換，也就是說，只要給定一個座標系下的一個點的座標，就能獲得這個點在其他座標系的座標。爲了達到這個目的，就需要確定各個座標系之間的關係，也就是獲得任一座標系在其他任一個座標系下的描述。
ROS的座標轉換庫，提供實時的座標轉換查詢與更改服務。它將座標轉換表示爲樹形結構，而座標轉換數據的更新速度直接影響其他節點的實時性。ROS 中對於多座標系的處理是使用樹型表示。
1.數據類型
定義了一個TF轉換樹，而且是單遍歷的樹，即關係是從父節點到子節點，tf定義和存儲這棵關係樹後，這樣框架之間的關係就很明確，tf會幫我們調用這些關係。
2.原理(引用)
基本原理：tb的類裏有個publisher，tl的類裏有個subscriber，一個發佈叫/tf的topic，一個訂閱這個topic，傳送的消息tfmessage裏包含了parent frameid和child frameid的信息。
這個機制意味着，所有的tb會發布某一特定的parent到child的變換，而所有tl會收到所有的這些變換，然後tl利用一個 tfbuffercore的數據結構維護一個完整的樹結構及其狀態。基於此，tl在使用這棵樹時，會lookuptransform或 waitfortransform來獲得任意座標系之間的變換。那就是只要是一個tl，就要跟所有tb建立連接，就要收取所有的/tf消息，來維護一棵完整的樹，並且還要 負責搜索這棵樹，找到一條變換的路徑，然後乘呀乘，並且每次都要如此。
3.tf庫的優缺點
優點：tf直接調用即可，中間計算細節不用考慮；接口簡單，廣播和監聽可以搞定；維護座標系之間的關係時容易確定問題再哪裏；考慮與時間相關的變換；支持tf-prefix，可以在多機器人上用。
缺點：
1）樹的結構很簡單，但有時笨拙，搜索時需要遍歷，一層層確定關係，計算有時很麻煩。
2）每個tl都要維護同一顆樹，網絡傳輸的負荷比較大。這一點，tf的設計者是完全承認的。
3）很難滿足實時性的要求，這一點比較顯然。也是大家都直到的。
總結：存在即合理。tf聽起來簡單用起來卻不是那麼容易。我們要理解它之後才決定要不要用，即使用了也要知道它的優缺點，順便思考有沒有什麼可以代替的。
4.base_link, odom, fixed_frame,target_frame和map的關係
在tf 中我們會涉及到幾個概念base_link, odom, fixed_frame,target_frame和map，那麼我們來看下它們在ROS中的定義。有時在調試中遇到無法解釋的現象，很可能是沒有弄清楚它的本質含義。比如筆者在使用laser 嘗試導航時，看到機器人姿態一直相對於map漂移。
（1）map
map是虛擬世界中的固定frame, 它的Z軸指向正上方，也就是天空。一個時間點上移動機器人的姿態相對於map不應該出現明顯的漂移。如果一個map是不連續穩定的那就意味着機器人的位置在任何一個時間點上都會是變化的。一般激光地位儀等設備會連續的計算map的位置因而造成它的不穩定性，這也使它不能成爲一個好的參照體frame.
一般與odom（或者odom_combined）相連，語義爲一個經過先驗（或者SLAM）地圖數據矯正過的，在地圖中的位姿信息。與odom同爲全局座標系，原點爲地圖原點（地圖原點在地圖相應的yaml文件中有規定）。
（2）odom
odom是一個很好的固定世界參照frame.機器人的姿態相對odom而言是隨時間是經常可變，所以在長遠中它不是一個好的全局參照系。但在某一時間點而言它相對機器人的位置是不變的。典型的odom frame 是通過運動源來計算出來的, 例如輪子運動，視覺偏移等。Odom frame 的準確性使它在局部參照系中是很有用的。但是不適合作全局參照frame.
一般直接與base_link 相鏈接，語義爲一個對於機器人全局位姿的粗略估計。取名來源於odometry（里程計），一般這個座標系的數據也是來源於里程計，原點爲開始計算位姿那個時刻的機器人的位置。
odom_combined 一般爲好幾種位姿估計方法的信息融合後的數據。在navigation stack中有 robot_pose_ekf 這個包是用擴展卡爾曼濾波算法（EKF）融合不同傳感器的數據。
（3）base_link
base_link參照系緊緊粘在移動機器人基座上的任何一個位置和角度。一般位於tf tree的最根部，物理語義原點一般爲表示機器人中心，爲相對機器人的本體的座標系。一般在urdf文件中都要定義base_link，它代表了機器人的主幹，其它所有的 frame都是相對於base_link定義並粘在一起的。它們一起相對於大地圖map移動，讓機器人移動就是向tf發佈geometry_msgs::TransformStamped 消息通知ros base_link相對於map的tf轉換關係。
（4）各個rfames的關係
frame之間是按樹狀結構組織的。所以每個frame只有一個父節點和任意多個子節點。上述幾個frame的關係:map->odom-->base_link。
odom到base_link的座標轉換是從運動源計算出來廣播的。
map到base_link的座標轉換是被定位模塊計算出來的. 但定位模塊並不發佈map到base_link的轉換. 相反它先接受從odom到base_link的轉換, 再計算並廣播map到odom的位置轉換關係。
（5）fixed_frame
RViz中認定的大世界就是fixed_frame。
（6）target_frame
Rviz中視覺跟蹤的frame是 target_frame。
一個完整的tf tree如下圖所示：
第四部分：AMCL
定位模塊作爲規劃層的輸入與參考數據，對於ROSnavigation 體系，因爲它先天的模塊間通訊方式實現了模塊間的完全解耦，所以對於導航規劃層用什麼定位方法，靜態還是動態的地圖，對於導航層內部幾乎沒有區別。只要有相應的數據就可以執行相應功能。我們可以使用其他的定位方式。
AMCL是機器人在二維移動過程中概率定位系統。它應用自適應的蒙特卡洛定位方式（或者KLD採樣），採用微粒過濾器來跟蹤已知地圖中機器人的位姿。關於AMCL的數學知識，可以參考《Probabolistic Robotics》書的chapter 8。目前所實施的，這個節點只和激光掃描和激光地圖工作，在base_scan的topic上利用激光數據來定位。它可以被擴展應用與其他的激光數據工作。
1.Node
AMCL需要基於激光的地圖/激光掃描/TF轉換，並且輸出位置估計。在啓動時，AMCL根據所提供的參數來初始化它的顆粒過濾器。由於默認原因，如果沒有參數設置，初始過濾狀態以一箇中等大小的粒子云爲中心（0,0,0）。有三種類型的ROS參數可以用來配置AMCL節點：整體濾波器，激光模式，odometery模型。2.Transforms
AMCL將傳入的激光掃描數據轉爲里程計結構（odom_frame_id）。因此，必須存在從激光發佈到里程計的tf樹轉換。
實現細節：在接收第一個激光掃描，AMCL查找激光結構和base計結構的TF轉換，並且鎖存。所以AMCL不能處理激光於base相對移動的情況。
下面的圖片顯示用里程計和AMCL定位的不同之處。在操作過程中，AMCL估計base結構相當於global結構TF轉換，但是，它只是放global和里程計之間的TF轉換。從本質上，這種轉換利用航位推算來處理漂移，所發佈的轉換是遠期的。
OdometryLocation - 只是通過里程計的數據來處理/base和/odom之間的TF轉換；
AMCL Map Location – 查找/base和激光的TF。/base通過/odom在/map中行走，機器人根據已知激光數據，估計/base相對於相對於/global的TF，那麼我們可以知道/map和/base之間的TF，從而估計位置。
第五部分：move_base參數解析
1.介紹
move_base提供了ROS導航的配置、運行、交互接口。它實現了一個供上層節點調用的action 接口（通過actionlib 實現），即給定座標信息與相應座標位置，執行相應的安全導航動作。對於下層控制器，輸出爲cmd_vel 速度。它規定了這個navigation的行爲。
2.這裏我們要提到ROS navigation 。navigation 導航規劃層提供一個在良好的定位條件下，安全導航到指定目標座標的功能。總體可以視爲一個慎思-反應混合範式。從層次上來分：
· 行爲層： move_base 綜合機器人狀態與上層指令，給出機器人當前行爲：正常導航，執行恢復動作，給上層節點返回失敗，終止導航。其中恢復動作可以自己定義。
· 全局規劃層：global_planner
· 局部規劃層： local_planner
· 控制器層（一般就是之前自己寫的速度發送部分）
3.navigation stack 裏面包含了很多package。我們看下與路徑規劃有關的部分。
（1）costmap_2d：這一部分可看作爲navigation的輸入處理器。不同的傳感器輸入的數據差異很大（激光雷達 & RGBD-camera）通過costmap_2d ， 不同的數據被處理成統一的格式：柵格地圖，權值用經過概率方法處理過的，表示空間中障礙物，未知與安全區域。生成出來的costmap則是planner 的輸入。
（2）global_planner：根據給定的目標位置進行總體路徑的規劃，爲navigation的全局規劃器，接受costmap生成的 global costmap 規劃出從起始點到目標點的路徑，爲local_planner作出參考。
在ROS的導航中，首先會通過全局路徑規劃，計算出機器人到目標位置的全局路線。這一功能是navfn這個包實現的。navfn通過Dijkstra最優路徑的算法，計算costmap上的最小花費路徑，作爲機器人的全局路線。
（3）local_planner：根據附近的障礙物進行躲避路線規劃，爲navigation 的局部規劃器，接受costmap 生成的local costmap 規劃出速度。
本地的實時規劃是利用base_local_planner包實現的。該包使用Trajectory Rollout 和Dynamic Windowapproaches算法計算機器人每個週期內應該行駛的速度和角度（dx，dy，dtheta velocities）。
base_local_planner這個包通過地圖數據，通過算法搜索到達目標的多條路經，利用一些評價標準（是否會撞擊障礙物，所需要的時間等等）選取最優的路徑，並且計算所需要的實時速度和角度。
其中，Trajectory Rollout 和DynamicWindow approaches算法的主要思路如下
1） 採樣機器人當前的狀態（dx,dy,dtheta）；
2） 針對每個採樣的速度，計算機器人以該速度行駛一段時間後的狀態，得出一條行駛的路線。
3） 利用一些評價標準爲多條路線打分。
4） 根據打分，選擇最優路徑。
5） 重複上面過程。
（4）recovery_behavior: 規定move_base 行爲集合中處理異常情況的行爲
以上是主體部分。
（5）nav_core：要理解ROS navigation最重要的部分是nav_core，它包含了global_planner，local_planner 與 recovery_behavior的基類的頭文件。
注：對於pluginlib,在ROS navigation中，move_base 提供的是框架，在move_base中是通過nav_core中規定的planner 與 recovery_behavior的基類的接口進行調用。與具體的實現方法隔離開來。而具體採用的方法由pluginlib 根據不同參數導入。這樣的實現方法使得navigation的可定製性大大增加。這賦予了這個框架很大的靈活性。通過不同的配置方法可以讓navigation適應很多不同的任務。
4.參數
move_base使用前需要配置一些參數：運行成本、機器人半徑、到達目標位置的距離，機器人移動的速度，這些參數在以下幾個配置文件中：
• base_local_planner_params.yaml
• costmap_common_params.yaml
• global_costmap_params.yaml
• local_costmap_params.yaml
官網上都有詳細的各個參數的含義的解釋。在配置這些參數時，首先要理解這些參數的含義，還要找出對整體性能影響比較大的幾個參數。由於參數很多，筆者理解的不夠深入，在實際調試過程中，小車效果很差。因此，建議瞭解每個配置文件中都大概有哪幾種參數，作用是什麼，參數值的大與小對系統有什麼影響，會出現何種情況。哪些參數是非常關鍵的，一定要注意這些參數是否在合理的範圍之內。並且利用調試的工具去調試程序，修改參數值大小。
（1）base_local_planner_params.yaml
分類：6種。分別爲robot configuration, goal tolerance,forward simulation, trajectory scoring, oscillation prevention, and globalplan.
1）RobotConfiguration Parameters
主要是各種速度參數以及平臺的類型。
acc_lim_x (double) ，acc_lim_y (double)，acc_lim_theta (double)，max_vel_x (double)，min_vel_x (double)，max_vel_theta (double)，min_vel_theta (double)，min_in_place_vel_theta (double)，backup_vel (double,default: -0.1) itmust be negative in order for the robot to actually reverse. escape_vel(double,default: -0.1) it must be negative in order for the robotto actually reverse.
holonomic_robot (bool,default: true)
The following parameters are only used if holonomic_robot isset to true: y_vels(list,default: [-0.3, -0.1, 0.1, 0.3]) ，需要注意。
2)GoalTolerance Parameters
主要是到達目標點的設置。
yaw_goal_tolerance (double, default:0.05) ，xy_goal_tolerance (double,default: 0.10) ，latch_xy_goal_tolerance (bool,default: false)
3)Forward Simulation Parameters
仿真時的參數。
sim_time (double, default:1.0) ，sim_granularity (double, default: 0.025)
angular_sim_granularity (double, default: ~<name>/sim_granularity)
vx_samples (integer, default:3) ，vtheta_samples (integer, default: 20)
controller_frequency (double, default:20.0)
4）Trajectory ScoringParameters
主要是軌跡範圍的設置。
meter_scoring (bool, default: false) ，pdist_scale (double,default: 0.6) ，gdist_scale (double,default: 0.8) ，occdist_scale (double,default: 0.01) ，heading_lookahead (double,default: 0.325) ，heading_scoring (bool,default: false)heading_scoring_timestep(double,default: 0.8) ，dwa (bool,default: true)，publish_cost_grid_pc (bool,default: false)，global_frame_id (string,default: odom)
5）OscillationPrevention Parameters
oscillation_reset_dist (double, default: 0.05)
6）GlobalPlan Parameters
prune_plan (bool, default: true)（2）global_costmap_params.yaml和global_costmap_params.yaml
全局代價地圖的配置和局部代價地圖的配置。分類：3種。分別爲coordinate frame and tf, rate, robot description and map management.分層：static map,obstacle map,inflation,others.
1）Coordinate frame and tf parameters
global_frame (string, default: "/map"),robot_base_frame (string,default: "base_link")，transform_tolerance (double, default: 0.2)
2）Rate parameters
update_frequency (double, default:5.0),publish_frequency (double, default: 0.0)
3）Map management parameters
rolling_window (bool, default: false)
The following parameters can beoverwritten by some layers, namely the static map layer.
width (int,),height(int),resolution(double),origin_x (double),origin_y(double)
（3）costmap_common_params.yaml
obstacle_range,raytrace_range,footprint,robot_radius,inflation_radius
observation_sources:laser_scan_sensor point_cloud_sensor
laser_scan_sensor:{sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking:true, clearing: true}
point_cloud_sensor:{sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking:true, clearing: true}
對於導航規劃層來說，整個系統的表現與實時性息息相關。制約表現好壞的最重要一部分就是costmap的生成。costmap會分別生成兩份，一個是global_costmap ，另一個是local_costmap，這兩份的參數是完全不同的。local_costmap，local_planner要求的實時性還是挺高的（特別是你把速度調高的時候），而local_costmap 所依賴的全局座標系一般是odom，繪製costmap的時候會反覆詢問odom->base_link frame的信息，tf數據延遲要是大了會影響costmap，進而導致機器人planner實時性降低，機器人移動遲緩或者撞上障礙物。所以有個參數transform_tolerance一定要慎重。如果是使用靜態先驗地圖做導航，那麼全局的costmap可以選擇使用static_map選項，這 樣的話在move_base 創建之初就會根據先驗地圖生成一次，以後不會再更新了。這樣會節省一些計算量。
而如果採用動態地圖（實時slam出來的）或者根本不使用先驗地圖，那可以將全局的costmap所依賴的全局座標系也改爲odom，rolling_window選項代替static選項，這樣costmap就會實時更新，要注意的是這樣的話你上層程序給出的目標點就不能超過 rolling_window的範圍。
總結：goal_tolerance，frequency，resolution ，obstacle_range，raytrace_range， pdist_scale，transform_tolerance等參數都是非常關鍵的，需要注意地圖的分辨率以及各種頻率，到達目標和軌跡的tolerance,這些都會影響整個系統的性能。
筆者所用的平臺爲p3-dx，底層已經封裝好了。在調試過程中會不斷積累經驗，導航中遇到的問題大概有這五個部分，希望對大家有幫助。筆者還在前進中，很多東西不完善，歡迎多多交流。