AIChallenge入門ガイド(3)

この記事の対象者

この記事のシリーズはAI Challengeで環境構築は終わったけど次にどこから取り組んだらいいのかわからないという方に向けた記事です。AIChallengeそのものの概要からよくわからない方は以下の記事をご参考ください。
https://zenn.dev/ttanaka3/articles/a66b6bfacf81a1

この記事ではAIChallengeに登録が完了し、環境構築を終えて入門ガイド(1)、(2)を完了した方向けに記載しています。また、競技という観点でいうと入門ガイド(1) >>> (2) > (3)の順で走行スコアに影響がでてきますので、順に進めることをおすすめします。
https://zenn.dev/ttanaka3/articles/a66b6bfacf81a1
https://zenn.dev/ttanaka3/articles/3dc507df539985
https://zenn.dev/ttanaka3/articles/4c1b6dea256ea5

この記事の対象範囲

この記事では主にplanningについてざっくりと説明していきます。今回は速度平滑化に関して記載していく予定です。

速度平滑化

「Motion Velocity Smoother」は、Autoware Universeにおける自動運転車両の速度計画を行うモジュールです。このモジュールの主な目的は、目標軌道上の各点で望ましい車速を計画し、出力することであり、速度の最大化と乗り心地の良さを両立させるために設計されています。

主要な機能とプロセスは以下の通りです：

軌道の抽出: 自車の後輪軸中心位置に最も近い参照経路上の点を基に、特定の範囲内の参照経路を抜き出します。

外部速度制限の適用: モジュール外部から指定された速度制限を適用します。

停止接近速度の適用: 停止点に近づく際の速度を設定します。

横加速度制限の適用: 経路の曲率に基づいて、指定された最大横加速度を超えない速度を制限速度として設定します。

軌道のリサンプル: 指定された時間間隔で経路の点を再サンプリングします。

初期状態の計算: 速度計画のための初期値を計算します。

速度の滑らかさ: 車速の計画を行い、JerkFiltered、L2、Linfの3種類のアルゴリズムの中から選択して最適化を行います。最適化のためにOSQPソルバを使用します。

後処理: 計画された軌道の後処理を行い、次のノードに渡す前にリサンプリングを行います。

入力と出力は以下の通りです：

入力: 参照軌道、外部速度制限、現在のオドメトリー、TF、TF static。
出力: 修正された軌道、現在の外部速度制限、計画された速度、加速度、ジャーク（デバッグ用）、各種デバッグデータ。
パラメータ設定には、制限速度、加速度、躍度、横加速度、曲線に関するパラメータ、再計画に関するパラメータ、停止速度に関するパラメータ、抽出に関するパラメータ、再サンプリングに関するパラメータ、最適化のための重みなどが含まれます。

このモジュールは、参照経路上の点に正しく設定された制限速度（停止点）を前提としており、指定された減速度やジャークで達成できない速度制限の場合、可能な範囲で速度、加速度、ジャークの逸脱量を抑えつつ減速を行います。

docker/aichallenge/aichallenge_ws/src/aichallenge_submit/aichallenge_submit_launch/launch/autoware_micro_awsim.launch.xml
のL228行目あたりに平滑化処理と必要なパラメータを入れていきます。

追記
また、今回新たなノードが加わるためtopicを/planning/trajectoryにremapしました。pure pursuitの入力のinput trajectoryを/planning/trajectoryに変えてください。

      <!-- velocity planning with max velocity, acceleration, jerk, stop point constraint -->
      <group>
        <!-- external velocity limit selector -->
        <group>
          <include file="$(find-pkg-share external_velocity_limit_selector)/launch/external_velocity_limit_selector.launch.xml"/>
        </group>

        <!-- motion velocity smoother -->
        <group>
          <!-- common param -->
          <!-- debug flags -->
          <arg name="smoother_type" default="JerkFiltered"/>
          <!-- Analytical, JerkFiltered, L2, or Linf -->
          <node pkg="motion_velocity_smoother" exec="motion_velocity_smoother" name="motion_velocity_smoother" output="screen">
            <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/common.param.yaml"/>
            <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/nearest_search.param.yaml"/>
            <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml"/>
            <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/motion_velocity_smoother/$(var smoother_type).param.yaml"/>
            <param name="publish_debug_trajs" value="false"/>
            <param name="algorithm_type" value="$(var smoother_type)"/>
            <remap from="~/input/trajectory" to="/planning/scenario_planning/trajectory"/>
            <remap from="~/output/trajectory" to="/planning/trajectory"/>
            <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity"/>
            <!-- Topic for setting maximum speed from the outside (input topic) -->
            <remap from="~/output/current_velocity_limit_mps" to="/planning/scenario_planning/current_max_velocity"/>
            <!-- Topic for displaying the set maximum speed (output topic) -->
          </node>
        </group>
      </group>

走行開始が遅いため
docker/aichallenge/aichallenge_ws/src/aichallenge_submit/autoware_launch/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yamlのengage velocity だけ以下の値に変えました。

    engage_velocity: 2.0              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
    engage_acceleration: 2.0           # engage acceleration [m/ss] (use this acceleration when engagement)

上限速度300km/hに対してなめらかな加速経路を実現しています。

カーブ時に減速できています。

実際にやってみよう

common paramもレーシング仕様にガンガン変えていきましょう。

docker/aichallenge/aichallenge_ws/src/aichallenge_submit/autoware_launch/autoware_launch/config/planning/scenario_planning/common/common.param.yaml

/**:
  ros__parameters:
    # constraints param for normal driving
    normal:
      min_acc: -1.0         # min deceleration [m/ss]
      max_acc: 1.0          # max acceleration [m/ss]
      min_jerk: -1.0        # min jerk [m/sss]
      max_jerk: 1.0         # max jerk [m/sss]

    # constraints to be observed
    limit:
      min_acc: -2.5         # min deceleration limit [m/ss]
      max_acc: 1.0          # max acceleration limit [m/ss]
      min_jerk: -1.5        # min jerk limit [m/sss]
      max_jerk: 1.5         # max jerk limit [m/sss]

#　終わりに
様々なパラメータで試して見てください。
VectorMap職人VS平滑化職人の図になると盛り上がってきますね。