【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例

1050
0
2020年5月7日 17时40分

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

1、pigot四足项目的步态改进,前行换成了摆线步态,加入了斜向步态

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

3、利用rqt_plot绘制数据验证步态改进的效果

步态改进

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

 

【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例插图

摆线

 

由于我们已经有了四足机器人足端逆解方法,因此规划摆线轨迹没有什么困难。轨迹代码见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' -->
  <gazebo reference="body_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>100</update_rate>
      <visualize>true</visualize>
      <topic>__default_topic__</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <topicName>imu</topicName>
        <bodyName>body_link</bodyName>
        <updateRateHZ>100.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_link</frameName>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

 

对于大型工程,建议单独建立一个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_acceleration

 

【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例插图(1)

 

rqt_plot 官方文档​wiki.ros.org

 

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

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

 

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

 

【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例插图(2)

线性插值步态的加速度

 

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

 

【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例插图(3)

摆线步态的加速度

 

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

 

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

 

ROS四足机器人步态仿真对比

项目地址

pigot​github.com

参考文献

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

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

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

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

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

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

[7] rqt_plot 官方文档

发表评论

后才能评论