0 写在前面

在本博客中将书接上文，主要介绍：

  • 机器人世界中的三种重要坐标系(odom、base_link、map)
  • 在已知地图上使用AMCL实现机器人定位，为之后的自主导航做准备工作
  • 在已知地图上使用move_base实现机器人自主导航
  • 实现机器人自主建图

1 三种坐标系(odom、base_link、map)

在介绍AMCL和move_base这两个ROS功能包之前，应该有必要介绍一下机器人世界中的三种坐标系，因为基于这三个坐标系的坐标变换(TF，可以先看这篇文章了解TF)，在AMCL和move_base两个功能包中会被反复监听和发布。

base_link(base_footprint)

最容易被理解的坐标系，他就是我们设计的移动机器人中的长方体底座(或是其中心点投影，即base_footprint)，他在URDF或是Xacro描述的机器人模型中都有直观的体现(即有代码编写)

odom

原理

最不容易被理解的坐标系，在状态来看，他最开始(没有发生里程计漂移时)与map重叠。在概念上，笔者的理解是，它是一种人为设计的坐标系，他与map坐标系之间的关系就像卡尔曼滤波中预测与更新，odom是预测，是根据我们设计的里程计算法设计的一种坐标系。“里程计”这个名词经常与odom相搭配出现，两者的内在关系在于，里程计转化得位姿矩阵是odom—>base_link的TF关系，里程计的设计方法多种多样，简单的有根据马达转速设计的，根据速度和角速度简单积分的；复杂的也有根据视觉的，根据多传感器的，了解SLAM的小伙伴应该知道SLAM算法本身可以被简化成为一个里程计，以下给出一个ROS官方文档给出的里程计设计算例：

//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
···
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
···
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
···

这个经典里程计的方式原理如下图：
odom

作用

odom在短时间之内很准确，可以用在短时间的状态估计，ROS中大量的功能包(包含AMCL和move_base)都需要此坐标系(更准确的说是需要一种TF坐标系间变换关系)

map

map坐标系也是一种固定坐标系，他是客观存在的，不是被我们所设计的，在map坐标系上的小车坐标可以被作为真实的坐标，如何求出在map坐标系求出我们的小车位置可以看作是位置估计的终极目标

三者关系

  • 按照正常的逻辑，odom和map应当都直接与我们的base_link存在TF变换关系，但是由于ROS中的坐标变换原则，每一个坐标系只能有一个父坐标系，因此在ROS中三者的坐标系关系为：
    map --> odom --> base_link
    世界坐标系是odom坐标系的父，odom坐标系是base_link的父。

总结

现在将所有知识串联在一起，工程的逻辑是：

  1. odom->base_link之间的TF关系可以通过自己设计的里程计算法获得
  2. AMCL等一系列的定位算法求取的map->base_link之间的TF变换关系，但是map->base_link在ROS中没有直接的变换(原因在上一节)，因此AMCL”曲线救国”，最终发布的是map->odom之间的TF变换。

补充

  1. 一个具体的例子解释三者关系，文章在这
  2. 以图直观解释三者关系，文章在这
  3. 以上大部分都是笔者自己的见解，如有错误也请大家指出，互相学习~

2 AMCL定位

概念

引用一篇文档给出的，笔者觉得很精炼，AMCL是“2D的概率定位系统，输入激光雷达数据，里程计数据，输出机器人在地图中的位姿，用的是自适应蒙特卡洛定位方法，这个方法是在已知地图中使用粒子滤波方法得到位姿的”。具体原理大家可以参考有关文献。

应用：ROS中的AMCL

介绍

对应上节笔者引用的AMCL概念，应当想到，ROS中的AMCL节点主要订阅了：

  • 激光雷达数据scan ([sensor_msgs/LaserScan][2])，数据来自于我们编写的gazebo激光传感器插件
  • 里程计数据tf ([tf/tfMessage][3])(TF信息中必须包括base_laser->odom之间的变换关系，当然还有其他坐标系之间的变换关系，这都是由TF树来维护)，数据来源于robot_state_publisher、joint_state_publisher，有如下的形象显示：
    TF树
  • 基于laser的栅格地图信息map ([nav_msgs/OccupancyGrid][5])，数据来源于我们上一篇博客中保存的地图
    发布的内容主要是：
  • 里程odom坐标系（可以通过~odom_frame_id参数重新映射）到地图map坐标系的变换

更详细内容可以参考文档

基于我们编写的移动机器人的应用

我们需要做的是在launch文件中设置地图的配置文件、利用robot_state_publisher、joint_state_publisher获取坐标系间坐标变换等信息，利用map_serve工具包运行地图服务器，在Rviz中加载设置的地图，并且利用amcl功能包实现机器人的定位，最后包含Rviz节点实现显示。

第一步仍是启动博客 xacro 与 gazebo 与 Rviz 集成launch指令：
roslaunch hw_car_gazebo.launch所在绝对路径

第二步启动AMCL、robot__state_publisher等节点的launch文件，这里取名叫amcl_run.launch，指令为:
roslaunch amcl_run.launch的绝对路径
对launch文件的具体代码有如下：

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="map01.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find myNav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find myNav)/launch/amcl_my.launch" />
    <!-- 运行rviz -->
    <!-- <node pkg="rviz" type="rviz" name="rviz"/> -->
</launch>

其中amcl_my.launch主要作用为设定AMCL功能包的各个参数，有如下：

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- Publish scans from best pose at a max of 10 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="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <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="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
  <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

  <param name="resample_interval" value="1"/>
  <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>

第三步就是启动我们的键盘控制节点：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
最终有如下效果展示：
amcl
键盘控制+amcl
红色箭头越密集代表机器人在此处的概率越大

3 总结

在此博客中主要介绍了ROS中的三种重要坐标系，以及从概念和应用角度基于我们在之前博客中编写的移动机器人实现了AMCL定位，在下一篇博客中，我们将基于AMCL实现机器人自主导航，即move_base功能实现，最后实现机器人自主建图的功能。