差速小车的IMU
我们构建一个差速小车的目的就是要让它带着一个多线的激光雷达，通过Cartographer进行三维建图。但是Cartographer做三维建图的时候，必需有IMU提供机器人的姿态估计。 所以，本文我们将研究一下Gazebo中的IMU仿真，并给DiffCart提供一个IMU。IMU全称是惯性测量单元(Inertial Measurement Unit)，一般由一个陀螺仪和一个加速度计组成。
陀螺仪用于测量机器人的三轴角速度，加表测量的是三轴的加速度。 相比于里程计通过近似的方程估计的机器人角速度而言，陀螺仪测量的角速度要更准确一些，而且可以提供三维的姿态信息， 是移动机器人定位导航的主要传感器。加表理论上可以通过积分来估计机器人的位置，但是由于传感器本身的测量噪声以及安装误差，积分结果往往比较差。 所以我们会看到很多定位方案并没有用到加表，或者只是用于静态检测估计重力方向。
目前市面上广泛使用的IMU多是MEMS形式的，MEMS形式的陀螺精度有限，但是价格十分便宜。辅助一些误差补偿和修正手段， 也是可以得到比较理想的姿态估计的。得益于MEMS技术的低成本，惯导技术得以普及，使得无人机、无人车等产品能够越来越多的出现在我们的日常生活当中。
本文主要研究Gazebo下的IMU仿真方法，如何通过API获取机器的角速度和线加速度。至于如何对传感器的数据进行积分估计机器人的姿态， 有兴趣的可以参考一个四旋翼系列的文章。
1. Gazebo的IMU传感器
参考kobuki关于gazebo的xacro文件， 我们直接在DiffCart的模型描述文件中给"base"添加了如右图所示的<sensor>子标签。 标签的属性type设置为"imu"，表示这是一个IMU的传感器。
标签<sensor>有四个子标签，其中标签<always_on>为true表示Gazebo将一直按照标签<update_rate>所指定的频率更新传感器数据。 <visualize>则要求Gazebo的GUI工具渲染传感器，这个对于IMU而言似乎没什么用。
imu
├── angular_velocity 可选, 各轴角速度误差模型
├── linear_acceleration 可选, 各轴线加速度误差模型
└── noise 可选, 传感器输出数据的误差模型
标签<imu>用于详细描述IMU传感器的配置。如左侧的结构树所示，它主要有三个子标签， 其中标签<angular_velocity>和标签<linear_acceleration>可以用于详细的描述xyz三轴的角速度和线加速度的误差模型。 而标签<noise>给传感器生成的数据又加上了一些仿真噪声，似乎与另外两个标签的内容重复了，在SDF 1.6之后，这个标签就被移除了。 由于我们使用的是SDF 1.5所以还可以用它来描述噪声。
标签<noise>有三个子标签。其中，<type>描述了噪声类型，目前只有高斯噪声一种可选。标签<rate>和<accel>分别描述了角速度和线加速度的噪声特性， 分为noise和bias两个部分。
noise比较好理解就是传感器数据的高斯分布，用均值(mean)和标准差(stddev)来描述，每次更新传感器数据的时候都会根据指定的高斯分布采样一个噪声附加上。 bias则是传感器的偏差项，它也是用高斯分布来描述的，即上面右图中的bias_mean和bias_stddev。只是该扰动项只在开始仿真的时候采样一次，以后都将直接附加到传感器数据上，作为传感器的一个偏移量。
2. 订阅更新并发布ROS消息
修改了模型文件之后，我们直接通过指令$ roslaunch gazebo_demos diffcart.launch运行仿真后，通过快捷键<ctrl> + <p>就可以唤出右图所示的窗口。
这是Gazebo自带的绘图工具，在左侧"TOPICS"页面下列举了当前Gazebo系统发布的主题。其中第三个主题已经被我们展开了，"~/diffcart/base/imu/imu"就是我们刚刚添加的imu传感器发布的，
按照标签<update_rate>所指定的频率发布数据。
我们直接将z轴的角速度测量值拖进右侧的"y"栏下，就可以看到航向角角速度的实时变化曲线了。
然后通过指令$ roslaunch gazebo_demos diffcart_keyteleop.launch键盘控制DiffCart随意转动，就能得到类似右图的效果。
这样一来，我们只需要像Velodyne的仿真插件那样，在插件的代码中订阅该主题，
然后在消息的回调函数中解析它并填充到一个ROS的消息中发布出去就可以了。
所以，我们在插件类DiffCartPlugin中添加了成员变量mGazeboNode、mImuSub和mRosImuPub，分别用于记录Gazebo的通信节点、Imu数据更新消息的订阅器，以及ROS的消息发布器。 并在函数Load中完成对象的实例化。如下面的代码片段所示。
class DiffCartPlugin : public ModelPlugin {
// ...
public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
// ...
mGazeboNode = gazebo::transport::NodePtr(new gazebo::transport::Node());
mGazeboNode->Init();
mImuSub = mGazeboNode->Subscribe("~/" + mModel->GetName() + "/base/imu/imu", &DiffCartPlugin::OnImuMsg, this);
mRosImuPub = mRosNode->advertise<sensor_msgs::Imu>("/imu_data", 10);
// ...
}
// ...
private:
gazebo::transport::NodePtr mGazeboNode;
gazebo::transport::SubscriberPtr mImuSub;
ros::Publisher mRosImuPub;
// ...
};
下面是IMU数据更新消息的回调函数，其内容无非是从输入参数msg中获取传感器数据，并填充ROS的消息结构，最后通过mRosImuPub发布出去而已。
private: void OnImuMsg(ConstIMUPtr & msg) {
sensor_msgs::Imu imu_msg;
// 填充消息
imu_msg.header.frame_id = "base";
imu_msg.header.stamp.sec = msg->stamp().sec();
imu_msg.header.stamp.nsec = msg->stamp().nsec();
mRosImuPub.publish(imu_msg);
}
如果一切正常，运行Gazebo加载了DiffCart之后，我们就可以通过rostopic工具查看主题"/imu_data"的信息以及具体内容了。我们也就可以通过remap等手段在cartographer_ros中订阅该主题了。
3. 一些开源IMU插件的实现
在写本文的时候，我还参考了一些开源的IMU插件的实现方案：
- kobuki_gazebo_plugins: 一个经典底盘的Gazebo插件，直接通过gazebo的API获取传感器数据。
- gazebo_ros_imu: Gazebo官方教程中推荐的一个model级的插件，通过获取绑定的link的位置和速度信息，又实现了一个IMU数据更新方案。
- gazebo_ros_imu_sensor: Gazebo官方教程中推荐的一个sensor级的插件，使用Gazebo原生的API获取IMU数据更新信息，与kobuki的方式相同。
这几种插件的实现方案，都是在一次仿真更新前或后的事件回调函数中更新IMU数据的，通过计时的方法实现指定的更新频率。但这种方式有一个相位差的问题， 因为通过SDF文件描述，我们已经指定了传感器的更新频率，假设是100Hz，期望的ROS消息的频率也是100Hz。但是我们并不能保证计时器的超时时刻正好是传感器数据更新的时刻， 这样就必然存在一个固定的相位差，或者说是数据的延时，最坏的情况接近一个周期。如果期望的频率与更新频率之间不存在整数倍的关系，相位差不是固定的，但一定存在。 为了解决这一问题，我们通过直接订阅Gazebo的消息来获取IMU的更新数据。
但，如何通过API获取传感器对象，还是值得我们借鉴的。
sensors::ImuSensorPtr mImu = std::dynamic_pointer_cast<sensors::ImuSensor>(
sensors::get_sensor(mWorld->Name() + "::" + mModel->GetName() + "::base::imu"));