【2024年度も継続中！カレンダー"シリーズ3"以降に誰でも自由に追記OK】自動運転AI+その他関連してそうならなんでもOKAdvent Calendar 2023

ちゅらデータ株式会社

自己位置推定状況を監視する

Last updated at 2024-11-18Posted at 2024-11-03

はじめに

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

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

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

走行の肝になりそうな自己位置ですが、ふと「現在どれくらいの自信を持って自己位置を推定しているのか視覚的に把握したい」と思いました。検討した内容をメモとしてまとめておきます。

前提

縮小構成版のAutoware-Microを前提にしています。

システムアーキテクチャは以下となっており、本記事では、ekf_localizerの出力である/localization/kinematic_stateの不確実性を可視化します。（GNSS / IMU / 車輪速センサ、個別の可視化は対象外とします。細かすぎて見られないので…）

1. Covarianceを表示

これが一番ラクです。

rviz2で Displays - Localization - EKF - Odometory - Covariance を ONにします。
これにより、/localization/kinematic_state の Pose / Orientation の不確実性を表す楕円や三角が表示されるようになります。スケールは調整ください。

2. `localization_error_monitor`の使用

代案です。
Autoware.universe の自己位置推定の不確実性を監視するノードを使ってみます。
既に2024大会の開発環境には取り込まれていますので、launch変更のみで実行可能です。

以下はREADMEより引用。

localization_error_monitor is a package for diagnosing localization errors by monitoring uncertainty of the localization results. The package monitors the following two values:

size of long radius of confidence ellipse

size of confidence ellipse along lateral direction (body-frame)

2.1. launchファイル変更

reference.launch.xmlの99行目あたりに以下を追加します。

reference.launch.xml

...
    <!-- 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>
...

またbagファイルの実センサデータを使って自己位置推定を評価したかったので、
以下のように、Map / Sensing / Localization だけの評価用構成のlaucnファイルを作成してみました。

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>

2.2. 起動

bagファイルで動作確認してみます。

Terminal 1

source install/setup.bash
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 450

ellipse marker(debug/ellipse_marker, visualization_msgs::msg::Marker)と
diagnostics outputs(diagnostics, diagnostic_msgs::msg::DiagnosticArray)が出力されているはずです。

$ ros2 topic echo /diagnostics 
---
header:
  stamp:
    sec: 1726823076
    nanosec: 764795141
  frame_id: ''
status:
- level: "\0"
  name: 'localization_error_monitor: localization_accuracy'
  message: ellipse size is within the expected range
  hardware_id: localization_error_monitor
  values:
  - key: localization_accuracy
    value: '0.160447'
- level: "\0"
  name: 'localization_error_monitor: localization_accuracy_lateral_direction'
  message: ellipse size along lateral direction is within the expected range
  hardware_id: localization_error_monitor
  values:
  - key: localization_accuracy_lateral_direction
    value: '0.0363265'
---

$ ros2 topic echo /localization/error_monitor/bug/ellipse_marker 
---
header:
  stamp:
    sec: 1726822884
    nanosec: 590531131
  frame_id: map
ns: error_ellipse
id: 0
type: 2
action: 0
pose:
  position:
    x: 89659.98754034616
    y: 43160.06626645516
    z: 7.031205865735586
  orientation:
    x: 0.0
    y: 0.0
    z: -0.7572189327219391
    w: 0.6531611500444952
scale:
  x: 0.795299488037341
  y: 0.4733960400579056
  z: 0.01
color:
  r: 0.0
  g: 0.0
  b: 1.0
  a: 0.10000000149011612
lifetime:
  sec: 0
  nanosec: 0
frame_locked: false
points: []
colors: []
texture_resource: ''
texture:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ''
  format: ''
  data: []
uv_coordinates: []
text: ''
mesh_resource: ''
mesh_file:
  filename: ''
  data: []
mesh_use_embedded_materials: false
---

rqt_runtime_monitorで確認してみます。

Terminal 3

ros2 run rqt_runtime_monitor rqt_runtime_monitor

2.3. jskプラグインでrviz2表示

以下をクローンしてビルドしておきます。BSD-3-Clauseライセンスです。

諸々設定を済ませて、
rviz2, rqt_runtime_monitor, plotjugglerで可視化してみると、こんな感じです。

※ カーブでlocalization_accuracy_lateral_directionが下がっています。

※ /diagnostics の localization_accuracy / localization_accuracy_lateral_direction を直接描画したほうが良い気がしてきました。

おわりに

現在どれくらいの不確実性を持って自己位置を推定しているのか可視化する方法でした。
完全にメモですが残しておきます。

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

余談

最近のAutowareは、ツール群が以下のように切り出されているようです。

便利なものがありそうなのですが、今大会で使用しているAutowareのバージョンが異なるため、そのままは動かせませんでした。この辺とか使ってみたいですね。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up