【ROS-Gazebo】IMU插件使用與數據採集——以四足機器人pigot為例
最近在琢磨別的事情，Gazebo探索上面進展不大，但也有一些收穫，秉承慢慢寫的佛系態度記錄一下：
- pigot四足項目的步態改進，前行換成了擺線步態，加入了斜向步態
- Gazebo-IMU(慣性測量單元)插件的用法
- 利用rqt_plot繪製數據驗證步態改進的效果
步態改進
簡單來說，就是將每一步的足端軌跡規劃為一條擺線。擺線軌跡的兩端方向垂直於地面，足端與地面在其它方向的衝擊力更小，機器人行進更加穩定。
由於我們已經有了四足機器人足端逆解方法，因此規劃擺線軌跡沒有什麼困難。軌跡代碼見pig_control功能包的traj_data文件中的forward_gait()函數。詳細的參考資料推薦一個：
基於力感測器的四足機器人多步態規劃及初步維穩控制_百度學術在該文的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
利用rqt_plot驗證步態改進的效果
在pigot項目中，改進前的前行步態只是簡單指明瞭軌跡點後進行了線性插值，改進後採用了擺線步態。那麼，步態的改進對於改善機器人前行的平穩性究竟有沒有好處呢？可以通過rqt_plot採集機器人前行過程中的線性加速度予以驗證。
在線性插值的步態下，採集線性加速度圖像如下：
改用擺線步態後，採集線性加速度圖像如下：
以上兩次實驗的步幅、步頻、抬腿高度和落腳點均相同，可以看出，採用擺線步態時，落步時的衝擊一定程度上減小了。這也說明瞭擺線步態更具有平穩性。
以上現象也可以直接從動作中觀察到：
項目地址
pigot
參考文獻
[1] 百度百科 慣性測量單元
[2] 劉蕊. 基於力感測器的四足機器人多步態規劃及初步維穩控制[D]. 南京航空航天大學.
[3] Gazebo-IMU 插件官方教程
[4] sensor_msgs/Imu 消息類型 官方文檔
[5] 四元數與歐拉角的轉換方法
[6] 萬向節鎖問題介紹視頻
[7] rqt_plot 官方文檔
推薦閱讀：
