最近在琢磨別的事情，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

參考文獻

創作挑戰賽新人創作獎勵來咯，堅持創作打卡瓜分現金大獎

總結

以上是生活随笔為你收集整理的gazebo 直接获取传感器数据_【ROS-Gazebo】IMU插件使用与数据采集——以四足机器人pigot为例...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。