自己位置推定を理解する

はじめに

自動運転AIチャレンジ2024決勝まで、残りわずかとなりました。

目下、自己位置推定 の改善にも取り組んでいます。

GNSS + IMU + 車輪速センサ で自己位置を推定するのですが、
GNSS遅延やIMU精度不足…など、実車特有の課題があります。

どのような原理でAutowareの自己位置推定が動いているのか、個人的に整理したく、まとめました。完全なメモですが、なにかの参考になれば…と思い公開します。

原理

今回は、拡張カルマンフィルタを用いた自己位置推定を行っています。
原理に関しては、ティアフォー様の以下技術ブログがとてもわかり易いです。

個人的にこちらもスキ。

2024大会での自己位置推定

デフォルト設定で、どのように自己位置が推定されているか整理します。

システムアーキテクチャ

GNSS + IMU + 車輪速センサ で自己位置を推定しています(without LiDAR / Camera)。

処理フローとしては、1. 個別センサ値の前処理 → 2. センサ値の統合 → 3. 拡張カルマンフィルタでの自己位置推定 の流れでしょうか。

それぞれ個別ノードの詳細を見てみます。

1. 個別センサ値の前処理

imu_corrector

IMUのオフセット補正や標準偏差情報付与を行うノード。
IMUが信頼できない場合はこの標準偏差(=不確実性)を高く設定することで、後段の処理でこの観測値は更新に使われなくなる。

vehicle_velocity_converter

車速情報を特定トピックに変換するノード。

共分散情報も付与。
車輪速センサが信頼できない場合はこの共分散(=不確実性)を高く設定することで、後段の処理でこの観測値は更新に使われなくなる。

2. センサ値の統合

imu_gnss_poser

IMUとGNSSを使って、poseを推定するノード。

GNSSから受け取ったデータに基づいて、姿勢情報が無効な場合はIMUのデータを使って補完。
共分散(=不確実性)を調整。

gyro_odometer

IMUと車速を使って、twistを推定するノード。

データ量に応じて、共分散(=不確実性)を調整。

3. 拡張カルマンフィルタでの自己位置推定

ekf_localizer

拡張カルマンフィルタによる自己位置推定。

• 予測モデル：運動学モデル
• 観測値：imu_gnss_poser が推定したpose、gyro_odometer が推定したtwist

How to tune EKF parametersに書いてあるEKF調整方法は以下と理解しました。

• 0.準備
  • poseとtwistのヘッダータイムスタンプが正しく設定できていることを確認せよ
    • 遅延がある場合は、twist_additional_delay / pose_additional_delayを設定せよ
  • poseとtwistの関係が適切か確認せよ
    • 具体的にはposeの微分がtwistと一致するか確認せよ
• 1.センサーパラメータ調整
  • 各センサに標準偏差を設定せよ
  • pose_measure_uncertainty_time, pose_smoothing_steps, twist_smoothing_stepsを調整せよ
• 2.プロセスモデルパラメータ調整
  • proc_stddev_vx_c, proc_stddev_wz_c, proc_stddev_yaw_c, proc_stddev_yaw_bias_cを調整せよ

センサ特性

今大会で採用されているセンサは以下になります。

	型番
GNSS	u-blox ZED-F9R
IMU	ICM-20948

GNSSは0.5秒ほど遅延あり、IMUは方位角がほぼ使えないことがわかっています。
つまり、ある程度信頼できる0.5秒前のGNSS情報と、IMU / 車輪速情報を合わせて、現在の自己位置を推定することが必要と理解しています。

実際のデータを見てみる

bagファイル(.db3)を、CSVにして可視化してみます。
今回は plotjuggler で bagファイルをすべて読み込んで CSV Exporter で出力してます。

plotjugglerを起動します。

ros2 run plotjuggler plotjuggler 

File - Data を押下、metadata.yaml を選択。

CSV出力したいmessageを選択。

CSV Exporterのチェックボックスをクリック、Start time: firstをクリック、End time: lastをクリック、Export Data Range: Fileをクリック、出力CSV名を入力してSaveします。

(参考) bag2csvの他方法

https://qiita.com/nonanonno/items/8f7bce03953709fd5af9

https://zenn.dev/xen_nippon/articles/0ae35719f6cc44

https://gitlab.com/ternaris/rosbags

https://github.com/alesof/rosbag2_editor?tab=readme-ov-file

https://github.com/AtsushiSakai/rosbag_to_csv

https://github.com/fishros/ros2bag_convert

運営様から共有いただいたデモ走行データを、1周分だけ可視化してみます。

※ 以下で確認した限り当日の衛星数は問題なし、極端なGNSS精度劣化はないようです。

GNSS

/sensing/gnss/pose_with_covariance/pose/pose/position/x,y,z

/sensing/gnss/pose_with_covariance/pose/pose/orientation/x,y,z

Orientation Zの共分散値が大きめでした。
FIX解, FLOAT解問わず固定の共分散値かもしれません。

IMU

/sensing/imu/imu_raw/linear_acceleration/x,y,z

/sensing/imu/imu_raw/angular_velocity/x,y,z

/sensing/imu/imu_raw/orientation/pitch,roll,yaw

やはりYawは使用不可なようです。

使用したスクリプトはこちらにあります。

(余談) Colab、コード補完があってGoodです。

実車センサデータを使った自己位置推定シミュレーション

センサデータは確認できたのですが、/localization/imu_gnss_poserや/localization/twist_estimator、/localization/kinematic_stateなどの
自己位置推定の結果がありませんでした。

そこで、実際のセンサデータを入力して自己位置推定のシミュレーションを行ってみます。
使用するreference.launch.xmlとコマンドは以下です。

reference.launch.xml

reference.launch.xml

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <!-- Essential parameters -->
  <arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
  <arg name="simulation" description="used for sensor kit param"/>
  <arg name="use_sim_time"/>
  <arg name="map_path" default="$(find-pkg-share aichallenge_submit_launch)/map/demo"/>
  <arg name="vehicle_model" default="racing_kart"  description="vehicle model name"/>
  <arg name="sensor_model" default="racing_kart_sensor_kit" description="sensor model name"/>
  <!-- Optional parameters -->
  <!-- Map -->
  <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>

  <!-- Vehicle -->
  <arg name="launch_vehicle_interface" default="false"/>
  <log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>

  <!-- Global parameters -->
  <group scoped="false">
    <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
      <arg name="use_sim_time" value="$(var use_sim_time)"/>
      <arg name="vehicle_model" value="$(var vehicle_model)"/>
    </include>
  </group>

  <group>
    <arg name="model_file" default="$(find-pkg-share tier4_vehicle_launch)/urdf/vehicle.xacro" description="path to the file of model settings (*.xacro)"/>
    <arg name="config_dir" default="$(find-pkg-share racing_kart_sensor_kit_description)/config"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
      <param name="robot_description" value="$(command 'xacro $(var model_file) vehicle_model:=$(var vehicle_model) sensor_model:=$(var sensor_model) config_dir:=$(var config_dir) simulation:=$(var simulation)' 'warn')"/>
    </node>
  </group>

  <!-- Map -->
  <group>
    <push-ros-namespace namespace="map"/>

    <!-- map_container -->
    <node_container pkg="rclcpp_components" exec="component_container" name="map_container" namespace="">

      <!-- map_loader::Lanelet2MapLoaderNode -->
      <composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader" namespace="">
        <remap from="output/lanelet2_map" to="vector_map" />
        <param name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
        <param from="$(find-pkg-share aichallenge_submit_launch)/config/map/lanelet2_map_loader.param.yaml" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

      <!-- map_loader::Lanelet2MapVisualizationNode -->
      <composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="lanelet2_map_visualization" namespace="">
        <remap from="input/lanelet2_map" to="vector_map" />
        <remap from="output/lanelet2_map_marker" to="vector_map_marker" />
        <param name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
        <param from="$(find-pkg-share aichallenge_submit_launch)/config/map/lanelet2_map_loader.param.yaml" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

      <!-- map_tf_generator::VectorMapTFGeneratorNode -->
      <composable_node pkg="map_tf_generator" plugin="VectorMapTFGeneratorNode" name="vector_map_tf_generator" namespace="">
        <param name="map_frame" value="map" />
        <param name="viewer_frame" value="viewer" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

    </node_container>

  </group> <!-- map -->

  <!-- Sensing -->
  <group>
    <push-ros-namespace namespace="sensing"/>
    <!-- Vehicle Velocity Converter  -->
    <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
      <arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
      <arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
      <arg name="config_file" value="$(find-pkg-share aichallenge_submit_launch)/config/vehicle_velocity_converter.param.yaml"/>
    </include>
    <group>
      <push-ros-namespace namespace="imu"/>
      <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
      <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
        <arg name="input_topic" value="imu_raw"/>
        <arg name="output_topic" value="imu_data"/>
        <arg name="param_file" value="$(var imu_corrector_param_file)"/>
      </include>
    </group>
  </group>

    <!-- Localization -->
  <let name="pose_additional_delay_var" value="0.5" />
  <group>
    <push-ros-namespace namespace="localization"/>
    <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
      <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
      <arg name="input_imu_topic" value="/sensing/imu/imu_data"/>
      <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
      <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
    </include>

    <node pkg="imu_gnss_poser" exec="imu_gnss_poser_node" name="imu_gnss_poser" output="screen"/>

    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
      <arg name="enable_yaw_bias_estimation" value="false"/>
      <arg name="tf_rate" value="50.0"/>
      <arg name="twist_smoothing_steps" value="1"/>
      <arg name="pose_smoothing_steps" value="1"/>
      <arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
      <arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/pose_with_covariance"/>
      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
      <arg name="output_odom_name" value="kinematic_state"/>
      <arg name="output_pose_name" value="pose"/>
      <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
      <arg name="output_biased_pose_name" value="biased_pose"/>
      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
      <arg name="output_twist_name" value="twist"/>
      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
      <arg name="proc_stddev_vx_c" value="10.0"/>
      <arg name="proc_stddev_wz_c" value="5.0"/>
      <arg name="pose_additional_delay" value="$(var pose_additional_delay_var)"/>
      <arg name="extend_state_step" value="100"/>
    </include>

    <!-- twist2accel -->
    <group>
      <node pkg="twist2accel" exec="twist2accel" name="twist2accel" output="screen">
        <param name="accel_lowpass_gain" value="0.9"/>
        <param name="use_odom" value="true"/>
        <remap from="input/odom" to="/localization/kinematic_state"/>
        <remap from="input/twist" to="/localization/twist_estimator/twist_with_covariance"/>
        <remap from="output/accel" to="/localization/acceleration"/>
      </node>
    </group>

    <!-- localization_error_monitor -->
    <group>
        <push-ros-namespace namespace="error_monitor"/>
        <include file="$(find-pkg-share localization_error_monitor)/launch/localization_error_monitor.launch.xml">
          <arg name="input/odom" value="/localization/kinematic_state"/>
          <arg name="param_file" value="$(find-pkg-share autoware_launch)/config/localization/localization_error_monitor.param.yaml"/>
        </include>
      </group>

  </group>

  <!-- vehicle -->
  <group if="$(var launch_vehicle_interface)">
    <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
      <arg name="converter_param_path" value="$(find-pkg-share aichallenge_submit_launch)/config/converter.param.yaml"/>
      <arg name="csv_path_accel_map" value="$(find-pkg-share aichallenge_submit_launch)/data/accel_map.csv"/>
      <arg name="csv_path_brake_map" value="$(find-pkg-share aichallenge_submit_launch)/data/brake_map.csv"/>

      <arg name="max_throttle" value="1.0"/>
      <arg name="max_brake" value="1.0"/>
      <arg name="convert_accel_cmd" value="true"/>
      <arg name="convert_brake_cmd" value="true"/>
      <arg name="convert_steer_cmd" value="false"/>

      <arg name="input_control_cmd" value="/control/command/control_cmd"/>
      <arg name="input_odometry" value="/localization/kinematic_state"/>
      <arg name="input_steering" value="/vehicle/status/steering_status"/>
      <arg name="output_actuation_cmd" value="/control/command/actuation_cmd"/>
    </include>
  </group>

  <!-- API -->
  <group>
    <!-- default_ad_api -->
    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py" />

    <!-- ad_api_adaptors -->
    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" />
  </group>

</launch>

Terminal 1

source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=true run_rviz:=true

Terminal 2

ros2 bag play /output/0920_demo_interface_only/0920_demo_interface_only_0.db3 --clock --start-offset 400

Terminal 3

ros2 bag record -a 

ekf_localizer の README.md - EKFパラメータを調整する方法を参考に、
調整代がありそうか確認してみます。

0.準備

遅延がある場合は、twist_additional_delay / pose_additional_delay を設定せよ

pose(GNSS)には0.5の遅延を設定しています。

poseの微分がtwistと一致するか確認せよ

pose(/localization/imu_gnss_poser)の微分 と twist(/localization/twist_estimator)を並べて、pose / twist の関係を確認してみます。

概ね良さそうです。

1.センサーパラメータ調整

各センサに標準偏差を設定せよ

すべて設定済み。GNSSは、RTK測位状態に応じて変更するのもありかもしれません。

pose_measure_uncertainty_time, pose_smoothing_steps, twist_smoothing_steps を調整せよ

以下3条件でシミュレーションをしてみます。

1. pose_measure_uncertainty_time: 0.01 → 0.001

2. pose_smoothing_steps: 1 → 5

3. twist_smoothing_steps: 1 → 5

reference.launch.xml を以下のように変えます。

reference.launch.xml

...
    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
      <arg name="enable_yaw_bias_estimation" value="false"/>
      <arg name="tf_rate" value="50.0"/>
-     <arg name="twist_smoothing_steps" value="1"/>
-     <arg name="pose_smoothing_steps" value="1"/>
+     <arg name="twist_smoothing_steps" value="5"/>
+     <arg name="pose_smoothing_steps" value="5"/>
+     <arg name="pose_measure_uncertainty_time" value="0.001"/>
      <arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
      <arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/pose_with_covariance"/>
      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
      <arg name="output_odom_name" value="kinematic_state"/>
      <arg name="output_pose_name" value="pose"/>
      <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
      <arg name="output_biased_pose_name" value="biased_pose"/>
      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
      <arg name="output_twist_name" value="twist"/>
      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
      <arg name="proc_stddev_vx_c" value="10.0"/>
      <arg name="proc_stddev_wz_c" value="5.0"/>
      <arg name="pose_additional_delay" value="$(var pose_additional_delay_var)"/>
      <arg name="extend_state_step" value="100"/>
    </include>
...

シミュレーション結果比較が以下です。
/localization/kinematic_state/pose/pose/position のxy座標のプロットです。

今回の設定値の範囲では、自己位置推定結果はほとんど変わりませんでした。

(参考) pose_smoothing_steps: 100, twist_smoothing_steps: 100だと以下になります。
スムージングを強くするため、自己位置推定が滑らかになりますが実測値への追従性が低下します。

2.プロセスパラメータ調整

proc_stddev_vx_c, proc_stddev_wz_c, proc_stddev_yaw_c, proc_stddev_yaw_bias_cを調整せよ

READMEを参考に、proc_stddev_vx_c と proc_stddev_wz_c を計測された加速度/角加速度の最大値である1.0に変えて自己位置推定結果の/localization/kinematic_stateを比較してみます。

proc_stddev_vx_c : set to maximum linear acceleration
proc_stddev_wz_c : set to maximum angular acceleration

reference.launch.xml を以下のように変えます。

reference.launch.xml

...
    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
      <arg name="enable_yaw_bias_estimation" value="false"/>
      <arg name="tf_rate" value="50.0"/>
      <arg name="twist_smoothing_steps" value="1"/>
      <arg name="pose_smoothing_steps" value="1"/>
      <arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
      <arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/pose_with_covariance"/>
      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
      <arg name="output_odom_name" value="kinematic_state"/>
      <arg name="output_pose_name" value="pose"/>
      <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
      <arg name="output_biased_pose_name" value="biased_pose"/>
      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
      <arg name="output_twist_name" value="twist"/>
      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
-     <arg name="proc_stddev_vx_c" value="10.0"/>
-     <arg name="proc_stddev_wz_c" value="5.0"/>
+     <arg name="proc_stddev_vx_c" value="1.0"/>
+     <arg name="proc_stddev_wz_c" value="1.0"/>
      <arg name="pose_additional_delay" value="$(var pose_additional_delay_var)"/>
      <arg name="extend_state_step" value="100"/>
    </include>
...

シミュレーション結果比較が以下です。
/localization/kinematic_state/pose/pose/position のxy座標のプロットです。

結果がやや変わりました。
カーブの始まりで内側 / 終わりで外側　に推定される傾向が出ています。

真値がないのでとりあえずGNSS(0.5秒の遅延補正済)を真値と仮置きして、
0.5秒前のGNSS + IMU + 車輪速でどれくらい自己位置が推定できているか見てみます。

xy座標で書いてみると以下で、外に膨らんでしまう傾向があります。

これを抑えるには、パッと思いつくところでは以下がありそうですがまだ試せていません…

• EKFのパラメータを調整する（望み薄かも？）
• EKFの予測モデルを変更する（動力学モデルや機械学習モデルなど）
• UKFに変更する

※ EKFパラメータ調整はGNSS(遅延補正済み)と推定値の差分を目的関数に以下で自動調整もできそう…と思いましたが、完全に手が追い付いておりません…😭

おわりに

拡張カルマンフィルタをつかった自己位置推定について理解が深まりました。

カーブで自己位置推定が外側に膨らんでしまう問題は、私が試した範囲のパラメータ調整では改善が見込めませんでした。根本的な予測モデルの変更や自己位置推定アルゴリズム自体の変更が必要なのかもしれません。

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。

(参考) GNSS + IMU + 車輪速で自己位置推定する例は他にもありそうでした。

Eagleyeと併用も可能？なようです、使ってみたいですね。