環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	16.04
ROS	Noetic
Gazebo	11.9.0

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

gazeboではIMUをシミュレーションすることができます。gazeboプラグインではIMU用のプラグインとしてGazeboRosImuとGazeboRosImuSensorの2つがあります。前者はROSの中でgazeboの位置を2回微分したもので、重力の影響などを検知することはできません。後者のほうが実世界のIMUに近いもので、今回はこれを使います。

IMUについて

慣性計測装置（Inertial Measurement Unit）は内界センサーとも呼ばれ、ロボットの外部に対するセンシング（例えばlidar）無しで観測できるセンサーです。基本的に加速度センサー、角速度センサー、磁気センサーで構成されています。この3つのデータをフィルターで合成をすることでロボットの姿勢を検出することができます。
加速度センサーは加速度を検知できるので2回積分をすれば位置を出すことが理論的には可能です。しかし誤差が大きいために市販品では位置や速度を出すことは困難です（航空機に搭載されるような高精度品では可能です）。同じように単体の角速度センサーの値を積分して姿勢を出すこともノイズやドリフトにより困難です。sensor_msgs/Imu型では以下のように加速度センサー・角速度センサーの生の値と3つのセンサーの値を合成した姿勢のデータが入っています。磁気センサーは別のTopicで出すのが一般的なようです。

IMU型の中身（抜粋）

[sensor_msgs/Imu]:
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

ソースコード

SDF

IMUのシミュレーション部分を示します。

gazebo2_lecture/models/wheel_robot_sensor/model.sdf

<link name="main_imu_link">
  <!-- 省略 -->
  <gravity>true</gravity>
  <sensor name="main_imu" type="imu">
    <always_on>true</always_on>
    <update_rate>20</update_rate>
    <visualize>true</visualize>
    <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <topicName>/main_imu/data</topicName>
      <bodyName>main_imu_link</bodyName>
      <updateRateHZ>10.0</updateRateHZ>
      <gaussianNoise>0.0</gaussianNoise>
      <xyzOffset>0 0 0</xyzOffset>
      <rpyOffset>0 0 0</rpyOffset>
      <frameName>main_imu_link</frameName>
      <initialOrientationAsReference>false</initialOrientationAsReference>
    </plugin>
    <pose>0 0 0 0 0 0</pose>
  </sensor>
</link>

imuを取り付ける位置を指定します。基本的にどこに付けてもあまり変わりませんが、遠心力の影響があります。ロボットの回転中心から離れた場所だと回転時に遠心力が発生します。imuの取り付け位置を示すリンクを設定します（今回はimu_link）。
<topicName>imu</topicName>はROSトピック名です。
<updateRateHZ>10.0</updateRateHZ>はROSトピックの出力のHZです。
<frameName>imu_link</frameName>はROSトピックのheader/frame_idに入る名前です。

実行

以下のコマンドで実行します。各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

roslaunch gazebo2_lecture wheel_robot_sensor.launch

以下のコマンドでimuの値を見れます。

rostopic echo /main_imu/data

この結果以下のようになります。加速度センサーと角速度センサーのデータとロボットの姿勢のorientationを出力します。ロボットの方向はmagnetセンサーがないと本来はわからないのですが、このgazeboのシミュレーションでは絶対姿勢が出力されます。

header: 
  seq: 564
  stamp: 
    secs: 86
    nsecs: 219000000
  frame_id: "imu_link"
orientation: 
  x: -1.18510234486e-05
  y: 1.07697119932e-05
  z: 0.613166723555
  w: 0.789953523233
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 7.44302053036e-06
  y: 0.000102902444977
  z: 0.0527355349022
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0055667469922
  y: -0.010351806896
  z: 8.92025062578
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

センサーを傾けて付けるときの注意点

上の例ではセンサーの向き（orientarion）が起動時にgazeboワールドの向き（orientarion）と合うようになっていましたが、そうでない場合は注意が必要です。このプラグインは起動時点を基準としてorientationを出すので、重力や北の方向などは考慮してくれません。この場合<rpyOffset>の値で調整する必要があります。

参考

GazeboRosImuSensor

目次ページへのリンク

ROS講座の目次へのリンク