環境

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

項目	値
CPU	Core i5-8250U
Ubuntu	16.04
ROS	Kinetic
Gazebo	7.14.0

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

概要

ロボットにおいて自分の位置を取得するというのは大事なタスクです。一発で自分の位置を正しく取得することができれば苦は無いのですが、大掛かりなセンサーが必要であったりするので容易ではありません。そこで不確かな多数のセンサーを統合するという手法を取られます。今回はrobot_localizationというパッケージを使って

シナリオ1：wheel odometry + IMU
シナリオ2：wheel odometry + 位置ノイズのあるtrackingセンサー
シナリオ3：wheel odometry + レートが低いtrackingセンサー

という3つのシナリオで位置情報の統合を行ってみます。

カルマンフィルタ

複数のセンサー情報を統合する時にはフィルタ処理をする必要があります。例えば位置を取得するセンサーが2つあった場合はその2つの出力の平均を採用するというような操作です。しかし位置と速度のように次元の違うセンサー情報を統合する場合は単純でないフィルタ処理が必要です。
また正確な位置が取れるのですがレートが遅いセンサーと、レートが早い速度が取れるセンサーを統合してレートが早い位置情報を生成するのにもフィルタ処理が必要です。
このような処理するフィルタが多数ありますが、今回はekf:拡張カルマンフィルタを使います。理論的には難しいのですが簡単に言うと、センサーの持っている誤差を考慮しながらいい感じに複数の情報を合わせて精度を上げれくれるものです。

今回統合する情報

以下の3つの情報を統合します。全部が〇のセンサーがあればよいのですがありません。そこでこれらをフィルタ処理で補い合ってよい位置情報を取得します。

	tracking	wheel odometry	imu
変位（位置）	△誤差あり	×積分誤差がつく	×
速度（位置）	×	◎	×
変位（角度）	△誤差あり	×積分誤差がつく	〇誤差あり
速度（角度）	×	◎	〇
レート	△低め	◎高い	◎高い

robot_localization

ROS navigation packageの中にはrobot_pose_ekfというパッケージがあるのですが、これは使いにいためにこっちを使います。以下のコマンドでインストールします。

sudo apt-get install -y ros-kinetic-robot-localization

ソースコード

launch

一番最後にlocalization関連の記述が足されています。

nav_lecture/launch/fusion.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find nav_lecture)/xacro/dtw_robot.xacro" />
  <arg name="rvizconfig" default="$(find nav_lecture)/rviz/fusion.rviz" />  
  <arg name="mode" default="none"/>

  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- dtw_robot -->
  <include file="$(find nav_lecture)/launch/dtw_spawn.launch">
    <arg name="robot_name" value="dtw_robot1"/>
  </include>

  <group ns="dtw_robot1">
    <!-- ground truth -->
    <node name="gazebo_odom_truth" pkg="nav_lecture" type="nav_gazebo_odom"  output="screen">
      <param name="model_name"  value="dtw_robot1" />
      <param name="odom_frame" value="dtw_robot1/odom" />
      <param name="base_frame"  value="dtw_robot1/ground_truth" />
      <param name="publish_rate"  value="20" />
      <param name="noise"  value="0.0" />
      <param name="tf_enable" value="true" />
      <remap from="odom" to="tracking/groundtruth" />
    </node>

    <!-- traking with noise -->
    <node name="gazebo_odom_noise" pkg="nav_lecture" type="nav_gazebo_odom"  output="screen">
      <param name="model_name"  value="dtw_robot1" />
      <param name="odom_frame" value="dtw_robot1/odom" />
      <param name="base_frame"  value="dtw_robot1/base_link" />
      <param name="publish_rate"  value="20" />
      <param name="noise"  value="0.05" />
      <remap from="odom" to="tracking/noise" />
    </node>

    <!-- tracking choppy -->
    <node name="gazebo_odom_choppy" pkg="nav_lecture" type="nav_gazebo_odom"  output="screen">
      <param name="model_name"  value="dtw_robot1" />
      <param name="odom_frame" value="dtw_robot1/odom" />
      <param name="base_frame"  value="dtw_robot1/base_link" />
      <param name="publish_rate"  value="2" />
      <param name="noise"  value="0.0" />
      <remap from="odom" to="tracking/choppy" />
    </node>

    <group if="$(eval mode=='mode1')">
      <!-- to increase the wheel odometry error -->
      <param name="/dtw_robot1/diff_drive_controller/wheel_radius" value="0.055" />
      <!-- robot localization -->
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true" output="screen">
        <rosparam command="load" file="$(find nav_lecture)/config/ekf_localization/mode1.yaml" />
        <param name="tf_prefix" value="dtw_robot1"/>
        <remap from="/odometry/filtered" to="fusion/odom" />
        <remap from="odom0" to="diff_drive_controller/odom" />
        <remap from="imu0"  to="imu/data" />
      </node>
    </group>
    <group if="$(eval mode=='mode2')">
      <!-- to increase the wheel odometry error -->
      <param name="/dtw_robot1/diff_drive_controller/wheel_radius" value="0.055" />
      <!-- robot localization -->
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true" output="screen">
        <rosparam command="load" file="$(find nav_lecture)/config/ekf_localization/mode2.yaml" />
        <param name="tf_prefix" value="dtw_robot1"/>
        <remap from="/odometry/filtered" to="/dtw_robot1/fusion/odom" />
        <remap from="odom0" to="diff_drive_controller/odom" />
        <remap from="odom1" to="tracking/noise" />
      </node>
    </group>
    <group if="$(eval mode=='mode3')">
      <!-- to increase the wheel odometry error -->
      <param name="/dtw_robot1/diff_drive_controller/wheel_radius" value="0.055" />
      <!-- robot localization -->
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true" output="screen">
        <rosparam command="load" file="$(find nav_lecture)/config/ekf_localization/mode2.yaml" />
        <param name="tf_prefix" value="dtw_robot1"/>
        <remap from="/odometry/filtered" to="/dtw_robot1/fusion/odom" />
        <remap from="odom0" to="diff_drive_controller/odom" />
        <remap from="odom1" to="tracking/choppy" />
      </node>
    </group>
  </group>

  <!-- rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

<param name="/dtw_robot1/diff_drive_controller/wheel_radius" value="0.055" />でタイヤの半径を実際の1割増しにしています。これは統合の結果をわかりやすくするためです。

robot_localizationのコンフィグ

robot_localizationのコンフィグは多数に及ぶので別ファイルにしてlaunchで読み込んでいます。項目は多数あるので参考にあるpackageの解説ページを見てみてください。

シナリオ1(wheel odometry + IMU)用

nav_lecture/config/ekf_localization/mode1.yaml

# for wheel and imu fusion

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
publish_tf: true
publish_acceleration: false

map_frame: map
odom_frame: odom
base_link_frame: base_link

#odom0: wheel odometry
odom0: odom0
odom0_config: [false, false, false,
               false, false, false,
               true , false, false,
               false, false, true,
               false, false, false]

#imu0
imu0: imu0
imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, true,
              false, false, false]

シナリオ2、3(wheel odometry + tracking)用

nav_lecture/config/ekf_localization/mode2.yaml

# for wheel and imu fusion

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
publish_tf: true
publish_acceleration: false

map_frame: map
odom_frame: odom
base_link_frame: base_link

#odom0: wheel odometry
odom0: odom0
odom0_config: [false, false, false,
               false, false, false,
               true , false, false,
               false, false, true,
               false, false, false]

#imu0
imu0: imu0
imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, true,
              false, false, false]

odom0: XXXのように入力の名前を付けるとその受け口が活性化します。odom0_configですが1行目からxyz位置、rpy角度、xyz速度、rpy角速度、xyz加速度を表します。その入力が有効なところのみtrueにしてください。また入力のtopicのcoverienceの部分をrobot_localizationはまじめに読みます。シッカリ設定しておきましょう。

実行

シナリオ1：wheel odometryとIMUの統合

以下のコマンドで実行します。

シミュレーションを起動

roslaunch nav_lecture fusion.launch mode:=mode1

表示	内容
ロボットの位置	統合後位置
赤線	統合後位置の軌跡
青矢印	trackerの位置（以後のシナリオで使用）
緑矢印	wheel odometry
黄矢印	真の位置
黄線	真の位置の軌跡

wheel odometryは次第にずれて、特に並進誤差よりも回転誤差が大きく効きます。IMUがあるとこの回転誤差を補完してくれるのでだいぶましになります。

シナリオ2：wheel odometryとノイズの多いtrackingデータの統合

以下のコマンドで実行します。

シミュレーションを起動

roslaunch nav_lecture fusion.launch mode:=mode2

今回は正しい位置から標準偏差で5cmずれるトラッキングデータをwheel odometryと合わせて統合します。だいぶ滑らかになります。

これは別の設定でのデータですが、トラッキングデータ（薄青線）は無茶苦茶ですが、統合後（赤線）は真値（青線）にだいぶ近づいています。

シナリオ3：wheel odometryと間隔の短いトラッキングデータの統合

以下のコマンドで実行します。

シミュレーションを起動

roslaunch nav_lecture fusion.launch mode:=mode3

さすがにトラッキングデータが来た時にガタつきますが、だいぶましになっています。

（追記）途中で切れる場合

途中でgazebo_odomを落としても、残りのセンサーを使ってそこそこに動くことができます。また再起動をするとそのデータを使い始めます。

参考

robot_localization

目次ページへのリンク

ROS講座の目次へのリンク