ROS导航之参数配置和自适应蒙特卡罗定位

我们的机器人使用两种导航算法在地图中移动：全局导航（global）和局部导航（local)。这些导航算法通过代价地图来处理地图中的各种信息，导航stack使用两种costmaps http://www.cnblogs.com/zjiaxing/p/5543386.html存储环境的障碍信息。一个costmap进行全局规划，在整个环境做长期的规划，而另一个是用于局部规划和避障。

全局导航：

在已建立的地图上做一个远距离目标的路径规划。

局部导航：

在正在建立的地图上做近距离目标同时要实时避开障碍物的路径规划。

代价地图的参数用于配置算法的计算行为。下面进行分析：

基本参数的的配置：

在你的launch（launch文件编写http://www.cnblogs.com/zjiaxing/p/5542614.html）里，首先创建一个costmap_common_params.yaml文件，内容：

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
#robot_radius: ir_of_robot
inflation_radius: 0.55
 
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}

这个文件主要用于配置基本的参数，这些参数会被用于local_costmap和global_costmap.

obstacle_range 和raytrace_range：表示传感器的最大探测距离，并在代价地图中引入探测障碍物信息。obstacle_range用于障碍物探测，例如机器人检测到一个距离小于2.5m的障碍物，就会将这个障碍物引入到代价地图中 raytrace_range用于机器人运动过程中，实时清除代价地图中的障碍物，例如该机器人将清除前面距离3m（传感器获取的数据）的障碍物。并更新可移动的自由空间数据。其实我们用激光传感器是无法感知物体的形状和大小的，但是，这个测量结果足够定位的了。

footprint：将机器人的几何参数告诉导航功能包集。这样，机器人和障碍物之间保持一个合理的距离，也就是说，例如前方有个门，要提前探知机器人是否能穿过这个门。

inflation_radius：给定机器人与障碍物之间必须要保持的最小距离。按照机器人的内切半径对障碍物进行膨胀处理。

observation_sources:设定导航包所使用的传感器。

laser_scan_sensor和point_cloud_sensor:对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。

全局代价地图的配置：

创建一个global_costmap_params.yaml文件，内容：

global_costmap:
global_frame: /map
robot_base_frame: base_link
update_frequency: 5.0
static_map: true

global_costmap和robot_base_frame：定义机器人和地图之间的坐标变换，建立全局代价地图必须使用这个变换。

update_frequency:地图更新的频率。

static_map: 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

局部代价地图的配置：

创建一个local_costmap_params.yaml 文件，内容：

local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.05

publish_frequency:发布信息的频率，也就是costmap可视化信息发布的频率。

rolling_window:在机器人运动过程中，代价地图始终以机器人为中心，这个在源码里是个if函数。

width、height、resolution:代价地图的的尺寸和分辨率，单位都是m.一般情况下resolution的数值与建的static map的一致。

基本局部规划器配置：

创建base_local_planner_params.yaml 文件，内容：

TrajectoryPlannerROS:
max_vel_x: 0.45
min_vel_x: 0.1
max_vel_theta: 1.0
min_in_place_vel_theta: 0.4
 
acc_lim_theta: 3.2
acc_lim_x: 2.5
acc_lim_y: 2.5
 
holonomic_robot: false

这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的限值。

holonomic_robot: 如果你的机器人是全向移动机器人那么此值为true，笔者的为差分型的。

为了运行导航功能包集，还需要创建个启动文件 move_base.launch ，内容：

<launch>
<master auto="start"/>
 
<!-- Run the map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find your_map_package)/your_map.pgm your_map_resolution.yaml"/>
 
<!--- Run AMCL -->
<include file="$(find amcl)/examples/amcl_diff.launch.launch" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find your_nav_package)/launch/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find your_nav_package)/launch/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find your_nav_package)/launch/local_costmap_params.yaml" command="load" />
<rosparam file="$(find your_nav_package)/launch/global_costmap_params.yaml" command="load" />
<rosparam file="$(find your_nav_package)/launch/base_local_planner_params.yaml" command="load" />
</node>
</launch>

利用自适应蒙特卡罗定位（AMCL）算法完成机器人定位，此方法是一种在2D环境下移动机器人的概率统计方法，这种方法在ROS系统中的具体实现是通过在已知地图的基础上使用粒子滤波算法跟踪机器人的位姿。如果你在rviz里不通过2D Pose Estimate 按钮设定初始位姿，则AMCL会假定你的机器人从坐标原点开始运行。这个包默认激光雷达数据，你也可以修改源码，适应如双目之类的传感器。amcl_diff.launch.launch是专用于支持差分驱动机器人平台的 https://github.com/ros-planning/navigation/tree/indigo-devel/amcl 内容如下：

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of  Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value=""/>
<param name="min_particles" value=""/>
<param name="max_particles" value=""/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value=""/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

min_particles和max_particles：设定算法运行所允许的粒子的最小和最大数量，如果粒子数多，就算会更加精确，当然也后悔更加消耗cpu资源。

laser_model_type：配置激光雷达类型。也可以设置beam光束雷达。

laser_likelihood_max_dist：设置地图中障碍物膨胀的最大距离。