限于本人非常菜，实验室也没个人指导，只好闭门造车瞎琢磨，毕业也不会再从事相关相关工作（主要是也轮不到自己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来说是有效的。
