ros c++ 代码说明文档_【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例...

最近在琢磨别的事情，Gazebo探索上面进展不大，但也有一些收获，秉承慢慢写的佛系态度记录一下：

1. pigot四足项目的步态改进，前行换成了摆线步态，加入了斜向步态
2. Gazebo-IMU(惯性测量单元)插件的用法
3. 利用rqt_plot绘制数据验证步态改进的效果

步态改进

简单来说，就是将每一步的足端轨迹规划为一条摆线。摆线轨迹的两端方向垂直于地面，足端与地面在其它方向的冲击力更小，机器人行进更加稳定。

摆线

由于我们已经有了四足机器人足端逆解方法，因此规划摆线轨迹没有什么困难。轨迹代码见pig_control功能包的traj_data文件中的forward_gait()函数。详细的参考资料推荐一个：

基于力传感器的四足机器人多步态规划及初步维稳控制_百度学术​xueshu.baidu.com

在该文的3.3节中讲到摆线步态以及优化方法。

惯性测量单元(IMU)插件

惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置，在机器人导航中有着很重要的应用[1]。

Gazebo中提供了IMU插件libgazebo_ros_imu_sensor.so，官方教程十分简单。这里介绍基本的使用步骤：

1. 在模型文件中建立一个link用以放置IMU插件

既可以添加一个单独的link代表IMU传感器，图方便也可以直接固连在已有的link中。在pigot中便直接使用机体连杆body_link放置IMU。

2. 在xacro文件中配置IMU

添加如下代码以配置IMU，一般只需修改<bodyName>的值为想要连接IMU的连杆名称即可。

<!-- IMU plugin for 'body_link' -->
  

对于大型工程，建议单独建立一个xacro文件以配置所有的插件（传感器、控制插件等），再在主文件中引用。pigot项目中的插件配置文件为pigot.gazebo。

3.建立一个订阅者用以接收IMU数据

上述两步完成以后，运行仿真模型时，topic列表中就会出现一个imu话题，不停地有imu消息发送到这个话题上。pigot项目中的话题为/pigot/imu，前缀/pigot来源于机器人运行在单独的命名空间中，在pigot_world.launch中进行设置，设置方式见我的这篇文章。

订阅者代码如下，网络上很容易找到c++的代码，这里用python来写：

#!/usr/bin/env python

需要注意的地方有：

消息类型Imu

from sensor_msgs.msg import Imu

导入一种消息类型Imu，这是一种标准的传感器消息类型，官方文档sensor_msgs/Imu 消息类型中重要部分摘录如下：

geometry_msgs

文档描述了Imu的消息结构，其中姿态（orientation）类型为四元数（geometry_msgs/Quaternion）；角速度（angular_velocity）和线加速度（linear_acceleration）的类型为三维向量（geometry_msgs/Vector3）。

四元数（Quaternion）转欧拉角（Euler-Angles）

四元数是一种姿态的表达方式，与欧拉角相比，它规避了“万向节锁”的问题。

这篇文章讲解了四元数与欧拉角的相互转换

这个视频形象地介绍了万向节锁的问题

4. 利用rqt_plot绘制数据曲线

rqt_plot是ROS内置的一款用于绘图的可视化插件。它可以读取话题中发布的数据，将类型允许的数据绘制成为图像。在终端中运行以下节点可打开rqt_plot:

rosrun rqt_plot rqt_plot

如果Imu插件启动成功，那么此时的话题列表中就应该有相应的话题了。在Topic栏中输入相应的话题即可进行绘图。注意，根据官方文档说明，绘图需要指定出数值所在的完整的地址，例如想要绘制加速度linear_acceleration，它是imu话题中消息的一个成员，那么应当指明的地址为：

/pigot/imu/linear_acceleration

rqt_plot 官方文档​wiki.ros.org

利用rqt_plot验证步态改进的效果

在pigot项目中，改进前的前行步态只是简单指明了轨迹点后进行了线性插值，改进后采用了摆线步态。那么，步态的改进对于改善机器人前行的平稳性究竟有没有好处呢？可以通过rqt_plot采集机器人前行过程中的线性加速度予以验证。

在线性插值的步态下，采集线性加速度图像如下：

线性插值步态的加速度

改用摆线步态后，采集线性加速度图像如下：

摆线步态的加速度

以上两次实验的步幅、步频、抬腿高度和落脚点均相同，可以看出，采用摆线步态时，落步时的冲击一定程度上减小了。这也说明了摆线步态更具有平稳性。

以上现象也可以直接从动作中观察到：

ROS四足机器人步态仿真对比https://www.zhihu.com/video/1101602829063634944

项目地址

pigot​github.com

参考文献

[1] 百度百科 惯性测量单元

[2] 刘蕊. 基于力传感器的四足机器人多步态规划及初步维稳控制[D]. 南京航空航天大学.

[3] Gazebo-IMU 插件官方教程

[4] sensor_msgs/Imu 消息类型 官方文档

[5] 四元数与欧拉角的转换方法

[6] 万向节锁问题介绍视频　

[7] rqt_plot 官方文档