限於本人非常菜，實驗室也沒個人指導，只好閉門造車瞎琢磨，畢業也不會再從事相關相關工作（主要是也輪不到自己O(∩_∩)O哈哈~）–本篇博客也只是一個從入門到入門從未精通最後到放棄的菜雞自言自語，看了浪費時間哈哈
首先是markdown編輯器的選擇，自己習慣了有道雲，可是不能直接上傳圖片不提供圖牀.github的圖牀又非常慢。琢磨了一上午【小書匠】還是沒搞懂github圖牀怎麼能快點，並且可版本控制。選擇困難了半天最後還是CSDN先把文字整理了，之後以
../images/xxx的形式上傳到github顯示圖片
ros navigation整理
此次解析的兩個主要工程： 所有內容（部分算法除外）
ros navigation官網源碼
大疆icra小車開源代碼
相關鏈接：
http://wiki.ros.org/cn/costmap_2d
http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData
http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals
http://wiki.ros.org/base_local_planner
http://wiki.ros.org/imu_filter_madgwick
http://wiki.ros.org/teb_local_planner
http://wiki.ros.org/dynamic_reconfigure
ros navigation有什麼
- map_server：地圖服務器，主要功能是保存地圖和導入地圖。
- costmap_2d：可以生產代價地圖，以及提供各種相關的函數。
- robot_pose_ekf：擴展卡爾曼濾波器，輸入信息是里程計、IMU、VO中的任意兩個或者三個都輸入。輸出是一個融合之後的pose，還是蠻好用的。之後的有robot_location
- voxel_grid：官方不支持，隨緣
- localization：這裏是兩個定位用的package。fake_localization一般是仿真用的，amcl纔是實際定位用的package。
- nav_core：這裏面只有三個文件，對應的是全局路徑規劃、局部路徑規劃、recovery_action的通用接口定義，用到的是ROS中的插件機制，具體功能實現則是在各個對應的規劃器package裏（global和local規劃）。
- move_base：這裏實現的是整個導航的流程。什麼時候調用全局路徑規劃、什麼時候調用局部路徑規劃、什麼時候調用recovery_action都是這個package管的。就是下圖中間方框裏做的事情，可以說是整個navigation stack的核心。
參數：
base_local planner_params yaml
列舉導航包下config/目錄下的 base_local_planner_params. yaml參數以及設定的值
controller_ frequency:3.0每多少秒我們需要更新一次路徑規劃?把這個值設得太高會使性能不足的CPU過載。對於一臺普通的計算機來說,設定爲3到5就可以運行得相當不錯
max_vel_x:0.3—機器人的最大線速度,單位是米/秒。對於室內機器人來說0.5就已經很快了,所以保守地選擇設爲0.3。
min vel x:0.05—機器人的最小線速度
max rotation vel:1.0—機器人的最大旋轉速度,單位是弧度/秒。不要把這個值設得太高,不然機器人會錯過它的目標方向
min_in_place_vel_theta:0.5—機器人的最小原地旋轉速度,單位是弧度/秒
escape_vel:-0.1—機器人逃離時的速度,單位是米/秒。請注意這個值必須設爲負數,這樣機器人才能反向
mx:2.5—在x方向上的最大線加速度
acc_lim_x:2.5-在x方向上的最大線加速度
acc_lim_y:0.0—在y方向上的最大線加速度。對於差速驅動(非完整驅動)機器人我們設爲0,這樣機器人就只能在x方向上線性運動(和旋轉)了
acc_lim_theta:3.2—最大角加速度。
holonomic_robot:false一除非你有一個全方向驅動機器人,否則一律設爲alse
yaw.goal_tolerane:0.1—至多距離目標方向的誤差(單位爲弧度)是多少?把這個值設得太小的話可能會導致機器人在目標附近倒
xy_goal_tolerance:0.1—至多距離目標位置的誤差(單位爲米)是多少?如果把值設得太小,你的機器人可能會不斷地在目標位置附近作調整。注意:不要把最大誤差設定得比地圖的分辨率還小(下一部分會講到),否則機器人會永不停息地在目標附近徘徊但水遠到不了目標位置。
pdist_scale:0.8—全局路徑規劃和到達目的位置之間的權重。這個參數比gist-seae大時,機器人更傾向於考慮全局路徑規劃
gdist_scale:0.4—到達目的位置和全局路徑規劃之間的權重。這個參數比 pdist_scale大時,機器人會更考慮到達目標位置,而不管這段路在全局路徑規劃中是否是必須的
occdist_seale:0.1—避開障礙物的權重
sim_time:規劃時需要考慮未來多長時間(單位爲秒)呢?這個參數和下一個參數(dwa)一起極大地影響機器人往目標移動的路徑
dwa:true在模擬未來的軌跡時是否用動態窗口方法( Dynamie Window Approach)
costmap_common_params. yaml
這個文件中只有兩個參數是需要你立刻爲你的機器人調整的
robot radius:0.165—對一個圓形的機器人來說,這是機器人的半徑,單位是米;對一個非圓形的機器人來說,你可以用接下來說到的 footprint參數。這裏用到的值是 Turtle的原始數據。
footprint:[[x0,y0],[xl,yl],x2,y2],[x3,y3],ect]—在列表中的每一個座標代表機器人的邊上的一點,機器人的中心設爲[0,0]。測量單位是米。這在機器人周長上的點要不按照順時針排列,要不按照逆時針排列。
inflation_radius:0.3—地圖上的障礙物的半徑,單位爲米。如果你的機器人不能很好地通過窄門或其他狹窄的地方,則稍微減小這個值。相反的,如果機器人不斷地撞到東西,則嘗試增大這個值。
global_costmap_params. yaml
這個文件中有一些參數,你需要根據機器人CPU的計算能力和你的工作站與機器人之間的情況來實驗得到。
global frame:/mp—對於全局代價地圖,我們用map框架來作爲 global框架
robot base frame:/ /base_ footprint一這個通常不是/he-ink就是/ ase_footprint對於Turtle Bot應設爲/
base_footprint
update_frequency:1.0—根據傳感器數據,全局地圖更新的頻率,單位爲赫茲。這個數值越大,你的計算機的CPU負擔會越重。特別對於全局地圖,通常會設定一個相對較小、在1.0到5.0之間的值。
publish_frequency:0—對於靜態的全局地圖來說,不需要不斷髮布。
statie_map:true—這個參數和下一個參數通常會設爲相反的值。全局地圖通常是靜態的,因此我們通常會把這個參數設爲true
rolling window:true當我們把這個參數設爲 false時,全局地圖不會在機器人移動的時候更新。
transform_ tolerance 指定在tf樹中框架之間的轉換的最大延時,單位爲秒。對於典型的工作站和機器人之間的無線網絡來說,與1.0秒同一級別的都可以工作得很好
local_costmap_params yaml
global_ frame:/odom對於本地代價地圖來說,我們使用 odometry框架來作爲 global框架
robot_ base frame:/ base_footprint這個通常不是/ base link就是/ base_ footprint對於TurtleBot應設爲/ base_footprint
update_frequency:3.0—根據傳感器數據,本地地圖更新的頻率,單位爲次/秒。
publish_ frequency:1.0—我們想更新已經發布出去的本地地圖,所以我們會把這個值設爲非零。一秒一次應該足夠了,除非你的機器人要移動得更快
static_map: false—這個參數和下一個參數通常會設爲相反的值。當本地地圖需要根據傳感器數據動態更新的時候,我們通常會把這個參數設爲 false
rolling-window:true—下面的幾個參數定義了本地地圖更新用的滑動窗口
width:6.0—滑動地圖的x維長度是多少米
height:6.0-----滑動地圖的y維長度是多少米
resolution:0.01—滑動地圖的分辨率,單位爲米。這個參數應該與YAML文件設置的地圖分辨率匹配
ransform tolerance:1.0—指定在tf樹框架之間的轉換,或可能會暫時中止的地圖繪製過程中兩者的最大延時,單位爲秒。在一臺速度較快、直連到機器人的計算機上,把這個值設定爲1.0就能很好地工作了。但是在通過無線網絡連接的較慢計算機上,這個延 時容忍參數需要增大。
蒙特卡羅（10/8部分，轉載內容較多，算法較爲粗糙可用Cartographer代替）
AMCL是一種概率定位系統，以2D方式對移動機器人定位。
它實現了自適應（或者KLD-採樣）蒙特卡洛定位法，使用粒子濾波跟蹤機器人在已知地圖中的位姿。
1.算法
概率機器人一書中講述了許多算法以及它們使用的參數，建議讀者去書中查看以獲得更多細節信息。 這裏我們用到書中的算法包括：sample_motion_model_odometry,beam_range_finder_model,likelihood_field_range_finder_model,Augmented_MCL, andKLD_Sampling_MCL。
基於目前現有的實現， 這個node僅能使用激光掃描和掃描地圖來工作，但是可以進行擴展以使用其它傳感器數據，用cartographer代替比較好。
2.1 amcl
amcl節點輸入激光地圖，激光掃描，和tf轉換信息，輸出位姿估計。
amcl在啓動時候依據提供的參數完成粒子濾波器初始化。但是需要注意，如果沒有指定參數值，使用默認參數的話，初始的濾波器狀態將是中心爲（0,0,0）含有中等尺度大小粒子的雲。
2.1.1 Subscribed Topics
scan (sensor_msgs/LaserScan)激光掃描
tf (tf/tfMessage)座標轉換信息
initialpose (geometry_msgs/PoseWithCovarianceStamped)用來初始化粒子濾波器的均值和協方差
map (nav_msgs/OccupancyGrid)當參數use_map_topic被設置後， AMCL訂閱map主題以獲取地圖完成基於激光的定位
2.1.2 Published Topics
amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
估計的機器人在地圖中的位姿，使用協方差表示
particlecloud (geometry_msgs/PoseArray)
粒子濾波器維護的位姿估計集合
tf (tf/tfMessage)
發佈從里程（可以使用參數~odom_frame_id進行重映射）到地圖的轉換
2.1.3 Services
global_localization (std_srvs/Empty)
初始化全局定位，所有粒子被隨機撒在地圖上的free空間
3.1.4 Services Called
static_map (nav_msgs/GetMap)
amcl調用該服務用來定位， 通過這個service啓動模塊有了地圖
3.1.5 Parameters
有3種ROS 參數用來配置amcl node: overall filter, laser model, and odometery model
1）Overall filter parameters
~min_particles (int, default: 100)
允許的最少粒子數
~max_particles (int, default: 5000)
允許的最多粒子數
~kld_err (double, default: 0.01)
真實分佈與估計分佈之間最大誤差
~kld_z (double, default: 0.99)
Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less thankld_err.
~update_min_d (double, default: 0.2 meters)
執行一次濾波器更新所需的平移距離
~update_min_a (double, default: π/6.0 radians)
執行一次濾波器更新所需的旋轉角度
~resample_interval (int, default: 2)
重採樣之前濾波器更新次數
~transform_tolerance (double, default: 0.1 seconds)
Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.
~recovery_alpha_slow (double, default: 0.0 (disabled))
Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.
~recovery_alpha_fast (double, default: 0.0 (disabled))
Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.
~initial_pose_x (double, default: 0.0 meters)
Initial pose mean (x), used to initialize filter with Gaussian distribution.
~initial_pose_y (double, default: 0.0 meters)
Initial pose mean (y), used to initialize filter with Gaussian distribution.
~initial_pose_a (double, default: 0.0 radians)
Initial pose mean (yaw), used to initialize filter with Gaussian distribution.
~initial_cov_xx (double, default: 0.5*0.5 meters)
Initial pose covariance (xx), used to initialize filter with Gaussian distribution.
~initial_cov_yy (double, default: 0.5*0.5 meters)
Initial pose covariance (yy), used to initialize filter with Gaussian distribution.
~initial_cov_aa (double, default: (π/12)*(π/12) radian)
Initial pose covariance (yawyaw), used to initialize filter with Gaussian distribution.
~gui_publish_rate (double, default: -1.0 Hz)
指定最大可用多大速率(Hz)掃描併發布用於可視化的路徑， -1.0 to disable
~save_pose_rate (double, default: 0.5 Hz)
指定存儲上次估計的位姿和協方差到參數服務器的最大速率 (Hz)，存儲變量爲 ~initial_pose_ and ~initial_cov_*。保存的位姿會在後面初始化濾波器時候使用。 -1.0 to disable
~use_map_topic (bool, default: false)
如果設置爲true, AMCL將訂閱地圖主題，不會使用service call獲取地圖。New in navigation 1.4.2
~first_map_only (bool, default: false)
如果設置爲true，AMCL將使用訂閱到的第一個地圖，不會使用每次更新獲取的新地圖。New in navigation 1.4.2
2）Laser model parameters
注意：無論使用什麼混合權重，權重加總應該等於1。 beam model使用了所有的4種權重: z_hit, z_short, z_max, and z_rand。likelihood_field model僅僅使用了2種: z_hit and z_rand。
~laser_min_range (double, default: -1.0)
最小掃描範圍; -1.0 will cause the laser’s reported minimum range to be used.
~laser_max_range (double, default: -1.0)
最大掃描範圍; -1.0 will cause the laser’s reported maximum range to be used.
~laser_max_beams (int, default: 30)
當更新濾波器時候，每次掃描有多少均勻分佈（等間隔的）beam被使用
~laser_z_hit (double, default: 0.95)
模型z_hit部分混合權重
~laser_z_short (double, default: 0.1)
模型z_short部分混合權重
~laser_z_max (double, default: 0.05)
模型z_max部分混合權重
~laser_z_rand (double, default: 0.05)
模型z_rand部分混合權重
~laser_sigma_hit (double, default: 0.2 meters)
模型z_hit部分使用的高斯模型標準差混合權重
~laser_lambda_short (double, default: 0.1)
模型z_short部分指數衰減參數
~laser_likelihood_max_dist (double, default: 2.0 meters)
Maximum distance to do obstacle inflation on map, for use in likelihood_field model.
~laser_model_type (string, default:"likelihood_field")
模型類型（beam, likelihood_field或者likelihood_field_prob） (same aslikelihood_field but incorporates the beamskip feature, if enabled).
3 ) Odometry model parameters
如果參數~odom_model_type是"diff"，那麼使用sample_motion_model_odometry算法，這種模型使用噪聲參數odom_alpha_1到odom_alpha4。
如果參數~odom_model_type 是"omni"，那麼使用定製化模型用於全向底座，使用噪聲參數odom_alpha_1到odom_alpha_5。前4個參數類似於“diff”模型，第5個參數用於捕獲機器人在垂直於前進方向的位移（沒有旋轉）趨勢。
由於舊模型有bug，解決了bug的新模型使用了新的類型"diff-corrected" 和"omni-corrected"。參數odom_alpha缺省值僅僅適合舊的模型，對於新的模型這些值要小很多，請參考
~odom_model_type (string, default: "diff")
模型類型（"diff", "omni", "diff-corrected" or"omni-corrected"）
~odom_alpha1 (double, default: 0.2)
指定里程旋轉估計（機器人運動旋轉分量）中期望的噪聲
~odom_alpha2 (double, default: 0.2)
指定里程旋轉估計（機器人運動位移分量）中期望的噪聲
~odom_alpha3 (double, default: 0.2)
指定里程位移估計（來自機器人運動位移分量）中期望的噪聲
~odom_alpha4 (double, default: 0.2)
指定里程位移估計（來自機器人運動旋轉分量）期望噪聲
~odom_alpha5 (double, default: 0.2)
位移相關噪聲參數 (only used if model is"omni").
這裏對 odom、base、global frame_id 三個說明
~odom_frame_id (string, default:"odom")
odometry使用的座標系
~base_frame_id (string, default:"base_link")
基座座標系
~global_frame_id (string, default:"map")
全局座標系
~tf_broadcast (bool, default: true)
設置爲false，amcl將不會發布全局座標系和里程座標系之間的轉換（參考下面的圖）
3.1.6 Transforms
amcl要把進來的激光掃描信息轉換到里程座標系 (~odom_frame_id)。因此，tf樹中必定會存在一條從發佈激光的座標系到里程座標系的路徑。 /scan屬於里程計座標系
實現細節是：amcl第一次收到激光掃描後，它會查看（事先定義好的靜態變換）激光座標系與基座座標系之間的轉換，並永久的記下這種轉換。因此，amcl不能用在相對於基座有相對位移的激光器。
下圖顯示了使用odomery和amcl定位的差異。在轉換過程，amcl會評估基座座標系(`base_frame_id`)相對於全局座標系(global_frame_id)的轉換，但是它僅發佈了全局座標系（`global_frame_id`)和里程座標系(odom_frame_id)之間的轉換，這種轉換過本質上是爲解決使用里程航跡推演出現的累計漂移。AMCL發佈的轉換可能帶有未來時間戳，但是對AMCL來說是有效的。
