base_local_planner
參考鏈接：https://github.com/ros-planning/navigation
概述
這個包使用Trajectory Rollout and Dynamic Window approaches來做平面上運動的機器人局部導航，控制器基於給定的路徑規劃和costmap生成速度命令後發送給移動基座。
該包適用於全向移動和非全向移動機器人，機器人輪廓可以表示爲凸多邊形或者圓。
這個包進行了ROS封裝，繼承了BaseLocalPlanner接口，且可以在啓動文件中設置ROS參數。
base_local_planner包提供了驅動底座在平面移動的控制器，控制器可以連接路徑規劃器和機器人基座。
爲了讓機器人從起始位置到達目標位置，路徑規劃器使用地圖創建運動軌跡。
向目標移動的路上，路徑規劃器至少需要在地圖上的機器人周圍創建一個可以表示成柵格地圖的評價函數。
This value function encodes the costs of traversing through the grid cells.
該控制器任務就是用這個評價函數確定發送速度和角度（DWA的父類）（dx，dy，dtheta velocities）給機器人基座。
Trajectory Rollout and Dynamic Window Approach (DWA)算法基本理念如下:
- 採樣機器人當前的狀態。
Discretely sample in the robot's control space (dx,dy,dtheta) - 用採樣的離散點做前向模擬，基於機器人當前狀態，預測如果使用空間採樣點的速度運動一段時間可能出現的情況。
- 評價前向模擬的每條軌跡，評價標準包括（接近障礙，接近目標，接近全局路徑和速度）。丟棄不合法的軌跡（如可能碰到障礙物的軌跡）。
- 根據打分，選擇最優路徑，並將其對應速度發送給基座。
- 重複上面步驟。
DWA與Trajectory Rollout的區別主要是在機器人的控制空間採樣差異。Trajectory Rollout採樣點來源於整個前向模擬階段所有可用速度集合，而DWA採樣點僅僅來源於一個模擬步驟中的可用速度集合。這意味着相比之下DWA是一種更加有效算法，因爲其使用了更小採樣空間；然而對於低加速度的機器人來說可能Trajectory Rollout更好，因爲DWA不能對常加速度做前向模擬。
在實踐中，我們經過多次實驗發現2種方法性能基本相當，這樣的話我們推薦使用效率更高的DWA算法。
一些有用的參考鏈接:
在LAGR機器人上使用的Trajectory Rollout算法的討論：
Brian P. Gerkey and Kurt Konolige. "Planning and Control in
Unstructured Terrain ".
The Dynamic Window Approach to local control： D. Fox, W. Burgard, and
S. Thrun. “The dynamic window approach to collision avoidance”.
An previous system that takes a similar approach to control：Alonzo
Kelly. “An Intelligent Predictive Controller for Autonomous Vehicles”.
( 1 ) Map Grid
- 爲了有效地評價軌跡，使用了地圖柵格。
- 每個控制週期，都會在機器人周圍創建柵格（大小爲局部costmap）區域，並且全局路徑會被映射（相關函數）到這個區域上。 這意味着有一定柵格將被標記爲到路徑點距離爲0，到目標距離爲0。接着利用傳播算法可以將剩下的標記，記錄它們到各自最近的標記爲0的點的距離。
- 然後，利用地圖柵格進行軌跡評價。
全局目標一般不會出現在地圖柵格標記的小區域內，所以爲接近目標進行軌跡評價時，這個目標應該是個局部目標，這意味着該小區域內第一個路徑點一定是該區域外還有其連續點的那個點。 該小區域的尺寸大小由move_base確定。
震盪抑制
當在x, y, or theta維度出現震盪後, 正負值會連續出現。因此，爲了抑制震盪影響，當機器人在某方向移動時，對下一個週期的與其相反方向標記爲無效，直到機器人從標記震盪的位置處離開一定距離。
通用局部路徑規劃
從ROS groovy版本開始有了一種新的局部路徑規劃包（DWA）。 這種實現更模塊化，其可複用的代碼使客製化一種局部路徑規劃器時候更加容易。 base_local_planner基礎代碼已經被擴展，增加了幾個頭文件和新的類。
局部規劃器的工作原則是在每一個控制週期搜索最佳的局部路徑規劃。首先局部規劃器會生成一些候選軌跡。
其次在檢測生成的軌跡是否會與障礙物碰撞。如果沒有，規劃器就會評價且比較選擇出最好的軌跡。
很顯然，由於機器人外形（和制動器）以及應用領域的差異，這種工作原則的實例化會不盡相同。
以下的類和接口遵照通用局部路徑規劃器工作原則允許進行不同實例化。可以以dwa_local_planner爲模板加入自己的代價函數或者軌跡生成器，來創建自定義的局部路徑規劃器。
( 1 ) TrajectorySampleGenerator
該接口描述了一種可以生成很多軌跡發生器，每調用一次nextTrajectory()就會返回一個新的軌跡。
SimpleTrajectoryGenerator類可以使用trajectory rollout或DWA原理來生成概述中描述的軌跡。
( 2 ) TrajectoryCostFunction
這個接口包含了最重要的函數scoreTrajectory(Trajectory &traj), 該函數輸入軌跡後會輸出軌跡評價分數。
如果輸出負分數意味着軌跡無效；輸出分數爲正值，對於cost函數來說值越小越好。
每個cost函數有一個比例因子，與其它cost函數比較時候，通過調節比例因子，cost函數影響可以被改變。
base_local_planner包附帶了一些在PR2上使用過的cost函數，如下所述。
( 3 ) SimpleScoredSamplingPlanner
這是軌跡搜索的一種簡單實現，利用了TrajectorySampleGenerator產生的軌跡和一系列TrajectoryCostFunction。 它會一直調用nextTrajectory()直到發生器停止生成軌跡。對於每一個生成的軌跡，將會把列表中的cost函數都循環調用，並把cost函數返回的正值，負值丟棄。
利用cost函數的比例因子， 最佳軌跡就是cost函數加權求和後最好的的那條軌跡。
( 4 ) Helper classes
( 4.1 ) LocalPlannerUtil()管理器
該幫助接口提供了通用的接口可供所有規劃器使用。它管理當前的全局規劃，當前的運動約束，以及當前的cost地圖（感知障礙物的局部cost地圖）。
( 4.2 ) OdometryHelperRos
該類爲機器人提供odometry信息。
( 4.3 ) LatchedStopRotateController
理想情況下，局部路徑規劃器可以讓機器人準確停到它應該停止地方。然而在現實中，由於傳感器噪聲和執行器的不穩定性，機器人會接近到達目標，但其會繼續移動，這不是我們想要的結果。
Ideally a local planner will make a robot stop exactly where it
should. In practice however, due to sensor noise and actuator
uncertainty, it may happen that the robot approaches the target spot
but moves on. This can lead to undesired robot behavior of oscilatting
on a spot.
LatchedStopRotateController是一個不錯的控制器，當機器人足夠靠近目標時可以迅速啓用。
然後，控制器將執行完全停止和原地旋轉朝向目標方向的操作，無論在停止後的機器人位置是否超出目標位置公差範圍。
The LatchedStopRotateController is a Controller that can be used as
soon as the robot is close enough to the goal. The Controller will
then just perform a full stop and a rotation on the spot towards the
goal orientation, regardless of whether the robot position after the
full stop leads the robot outside the goal position tolerance.
( 5 ) Cost Functions
( 5.1 ) ObstacleCostFunction
該cost函數類基於感知到的障礙物評價軌跡。如果軌跡經過障礙物則返回負值，其它返回0。
( 5.2 ) MapGridCostFunction
該cost函數類基於軌跡與全局路徑或者目標點的接近程度來評價軌跡。它嘗試對所有軌跡使用相同的到某個目標或者路徑距離的預計算圖來優化計算速度。
在dwa_local_planner中, 因目的不同（爲讓軌跡儘可能接近全局路徑，爲讓機器人朝着局部目標前進，還有爲讓機器人的頭保持指向局部目標），該cost函數具體實現也會不盡相同。因該cost函數使用了試探機制，因此如果使用了不合適的參數可能給出的結果很差甚至根本不能工作。
( 5.3 ) OscillationCostFunction
該cost函數類用以減少一定程度的震盪。雖然該cost函數能有效防止震盪，但是如果使用不合適的參數也可能會得不到一些好的解決方案。
( 5.4 ) PreferForwardCostFunction
該cost函數類適用於類似PR2那種在機器人前方有很好傳感器（如tilting laser）佈置的機器人。
該cost函數鼓勵前向運動，懲罰後向或者其它周圍方向性運動。但是這種特性在某些領域機器人可能並不是所期望的，所以僅適合於特定應用的機器人。
TrajectoryPlannerROS
base_local_planner::TrajectoryPlannerROS是對base_local_planner::TrajectoryPlanner的ROS封裝。
它在初始化時確定的ROS命名空間內運行，該接口繼承了nav_core包的nav_core::BaseLocalPlanner接口。
如下是base_local_planner::TrajectoryPlannerROS的一個應用案例:
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>
...
tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS costmap("my_costmap", tf);
base_local_planner::TrajectoryPlannerROS tp;
tp.initialize("my_trajectory_planner", &tf, &costmap);
1 ) API Stability
( 1.1 ) Published Topics
~<name>/global_plan ([nav_msgs/Path])
表示的是局部路徑規劃器目前正在跟隨的全局路徑規劃中的一部分，主要用於可視化。
~<name>/local_plan ([nav_msgs/Pat])
表示的是上一個週期局部規劃或者軌跡得分最高者，主要用於可視化。
~
<name>/cost_cloud ([sensor_msgs/PointCloud2][8])
用於表示規劃的cost柵格，也是用於可視化目的。參數publish_cost_grid_pc用來使用或者關閉該可視化。New in navigation 1.4.0
( 1.2 ) Subscribed Topics
odom ([nav_msgs/Odometry]
該Odometry信息用於向局部規劃器提供當前機器人的速度。
這個里程消息中的速度信息被假定使用的座標系與TrajectoryPlannerROS對象內cost地圖的
robot_base_frame參數指定的座標系相同。
有關該參數詳細介紹請參照 costmap_2d 包。
( 2 ) Parameters
有許多ROS參數可以用來自定義base_local_planner::TrajectoryPlannerROS的行爲。（說明這個函數對機器人最後運動影響比較大嘍）
這些參數分爲幾類：機器人配置，目標公差，前向仿真，軌跡評分，防振和全局計劃。
( 2.1 ) Robot Configuration Parameters
~/acc_lim_x (double, default: 2.5)
機器人在x方向的最大加速度，單位meters/sec^2 。
~/acc_lim_y (double, default: 2.5)
機器人在y方向的最大加速度，單位meters/sec^2 。
~/acc_lim_theta (double, default: 3.2)
機器人的最大角加速度，單位radians/sec^2 。
~/max_vel_x (double, default: 0.5)
基座允許的最大線速度，單位meters/sec 。
~/min_vel_x (double, default: 0.1)
基座允許的最小線速度，單位meters/sec 。
設置的最小速度需要保證基座能夠克服摩擦。
~/max_vel_theta (double, default: 1.0)
基座允許的最大角速度，單位 radians/sec 。
~/min_vel_theta (double, default: -1.0)
基座允許的最小角速度，單位 radians/sec 。
~/min_in_place_vel_theta (double, default: 0.4)
原地旋轉時，基座允許的最小角速度，單位 radians/sec 。
~/backup_vel (double, default: -0.1)
DEPRECATED (use escape_vel):Speed used for backing up during escapes
in meters/sec. Note that it must be negative in order for the robot to
actually reverse. A positive speed will cause the robot to move
forward while attempting to escape.
~<name>/escape_vel (double, default: -0.1)
表示機器人的逃離速度，即背向相反方向行走速度，單位爲 meters/sec 。
該值必需設爲負值，但若設置爲正值，機器人會在執行逃離操作時向前移動。
~<name>/holonomic_robot (bool, default: true)
確定是否爲全向輪或非全向輪機器人生成速度指令。
對於全向輪機器人，可以向基座發出施加速度命令。 對於非全向輪機器人，不會發出速度指令。
以下參數僅在holonomic_robot設置爲true時使用：
~<name>/y_vels (list, default: [-0.3, -0.1, 0.1, 0.3])
The strafing velocities that a holonomic robot will consider in meters/sec
( 2.2 ) 目標公差參數（Goal Tolerance Parameters）
~<name>/yaw_goal_tolerance (double, default: 0.05)
The tolerance in radians for the controller in yaw/rotation when achieving its goal差幾米停止
~<name>/xy_goal_tolerance (double, default: 0.10)
The tolerance in meters for the controller in the x & y distance when achieving a goal
~<name>/latch_xy_goal_tolerance (bool, default: false)
If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. - New in navigation 1.3.1
( 2.3 ) 前向仿真參數（Forward Simulation Parameters）
~/sim_time (double, default: 1.0)
前向模擬軌跡的時間，單位爲 seconds 。
The amount of time to forward-simulate trajectories in seconds.
~<name>/sim_granularity (double, default: 0.025)
在給定軌跡上的點之間的步長，單位爲 meters 。
The step size, in meters, to take between points on a given trajectory
~<name>/angular_sim_granularity (double, default: ~<name>/sim_granularity)
給定角度軌跡的弧長，單位爲 radians 。
The step size, in radians, to take between angular samples on a given trajectory. -New in navigation 1.3.1
~<name>/vx_samples (integer, default: 3)
x方向速度的樣本數。
The number of samples to use when exploring the x velocity space .
~<name>/vtheta_samples (integer, default: 20)
角速度的樣本數。
The number of samples to use when exploring the theta velocity space
~<name>/controller_frequency (double, default: 20.0)
調用控制器的頻率，單位爲 Hz 。
Uses searchParam to read the parameter from parent namespaces if not set in the namespace of the controller. For use with move_base, this means that you only need to set its “controller_frequency” parameter and can safely leave this one unset.
( 2.4 ) 軌跡評分參數（Trajectory Scoring Parameters）
cost = pdist_scale * (distance to path from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter)
+ gdist_scale * (distance to local goal from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter)
+ occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))
該cost函數使用如上公式給每條軌跡評分
~<name>/meter_scoring (bool, default:false)
默認false情況下，用單元格作爲打分的單位，反之，則用米。
Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default.
~<name>/pdist_scale (double, default: 0.6)
The weighting for how much the controller should stay close to the path it was given.
~<name>/gdist_scale (double, default: 0.8)
The weighting for how much the controller should attempt to reach its local goal, also controls speed.
~<name>/occdist_scale (double, default: 0.01)
The weighting for how much the controller should attempt to avoid obstacles .
~<name>/heading_lookahead (double, default: 0.325)
How far to look ahead in meters when scoring different in-place-rotation trajectories .
~/heading_scoring (bool, default: false)
是否根據機器人前進路徑的距離進行評分。
Whether to score based on the robot’s heading to the path or its distance from the path.
~<name>/heading_scoring_timestep (double, default: 0.8)
How far to look ahead in time in seconds along the simulated trajectory when using heading scoring .
~/dwa (bool, default: true)
是否使用Dynamic Window Approach (DWA)，或者是否使用Trajectory Rollout。
Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout (NOTE: In our experience DWA worked as well as Trajectory Rollout and is computationally less expensive. It is possible that robots with extremely poor acceleration limits could gain from running Trajectory Rollout, but we recommend trying DWA first.)
~
/publish_cost_grid_pc (bool, default: false)
Whether or not to publish the cost grid that the planner will use when planning. When true, a sensor_msgs/PointCloud2 will be available on the ~/cost_cloud topic. Each point cloud represents the cost grid and has a field for each individual scoring function component as well as the overall cost for each cell, taking the scoring parameters into account. New in navigation 1.4.0
~/global_frame_id (string, default: odom)
設置cost_cloud工作座標系，應該與局部cost地圖的global座標系一致。New in navigation 1.4.0
( 2.5 ) Oscillation Prevention Parameters
~<name>/oscillation_reset_dist (double, default: 0.05)
機器人必須運動多少米遠後才能復位震盪標記。
( 2.6 ) Global Plan Parameters
~<name>/prune_plan (bool, default: true)
定義當機器人是否邊沿着路徑移動時，邊抹去已經走過的路徑規劃。 設置爲true時，表示當機器人移動1米後，將1米之前的global路徑點一個一個清除。（包括全局的global path和局部的global path）
Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.
TrajectoryPlanner
base_local_planner::TrajectoryPlanner實現了DWA and Trajectory Rollout算法。
如果要在ROS中想使用base_local_planner::TrajectoryPlanner, 請使用TrajectoryPlannerROS wrapper。
( 1 ) API Stability
雖然C++ API是穩定的。 但是仍然推薦使用TrajectoryPlannerROS wrapper 而不是base_local_planner::TrajectoryPlanner。
參考
http://wiki.ros.org/base_local_planner
http://blog.csdn.net/x_r_su/article/details/53380545
http://blog.sina.com.cn/s/blog_135e04d0a0102zpl1.html#cmt_3076623