最近在琢磨别的事情，Gazebo探索上面进展不大，但也有一些收获，秉承慢慢写的佛系态度记录一下：pigot四足项目的步态改进，前行换成了摆线步态，加入了斜向步态

Gazebo-IMU(惯性测量单元)插件的用法

利用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，一般只需修改的值为想要连接IMU的连杆名称即可。

true

true

100

true

__default_topic__

imu

body_link

100.0

0.0

0 0 0

0 0 0

imu_link

0 0 0 0 0 0

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

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

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

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

#!/usr/bin/env python

# license removed for brevity

import rospy

from sensor_msgs.msg import Imu

import math

def imu_cb(imu_data):

# Read the quaternion of the robot IMU

x = imu_data.orientation.x

y = imu_data.orientation.y

z = imu_data.orientation.z

w = imu_data.orientation.w

# Read the angular velocity of the robot IMU

w_x = imu_data.angular_velocity.x

w_y = imu_data.angular_velocity.y

w_z = imu_data.angular_velocity.z

# Read the linear acceleration of the robot IMU

a_x = imu_data.linear_acceleration.x

a_y = imu_data.linear_acceleration.y

a_z = imu_data.linear_acceleration.z

# Convert Quaternions to Euler-Angles

rpy_angle = [0, 0, 0]

rpy_angle[0] = math.atan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))

rpy_angle[1] = math.asin(2 * (w * y - z * x))

rpy_angle[2] = math.atan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))

return

if __name__ == '__main__':

rospy.init_node('imu_node', anonymous=True)

rospy.Subscriber("/pigot/imu", Imu, imu_cb)

rospy.spin()

需要注意的地方有：

消息类型Imu

from sensor_msgs.msg import Imu

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

geometry_msgs/Quaternion orientation

float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity

float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration

float64[9] linear_acceleration_covariance # Row major x, y z

文档描述了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_accelerationrqt_plot 官方文档​wiki.ros.org

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

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

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

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

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

以上现象也可以直接从动作中观察到：ROS四足机器人步态仿真对比https://www.zhihu.com/video/1101602829063634944

项目地址pigot​github.com

参考文献