motion_velocity_smootherをインテグレーションしてみた

# はじめに

自動運転AIチャレンジ2024決勝 @ 11月初旬 に向けて開発を進めています。

https://www.jsae.or.jp/jaaic/2024ver/summary/


横滑りやセンサ遅延を考慮した走行計画・車両制御など、
予選＠シミュレーションとは違った課題に取り組んでいます。

>決勝では、コースで競技車両であるEVレーシングカートを使用した大会を行います。シミュレーションで得た知見を実車両に活かしつつ、AWSIMでは再現できない実車ならではの課題にもチャレンジします。
例えば参加者には実車両に適用するため、パラメータ調整にも挑戦してもらいます。また、シミュレーションでは再現できないノイズ処理、遅延対策のアルゴリズム開発も行います。


特に、予選表彰式の前夜祭にて運営様から 
**「とにかく低速で走れ」** という戦略のヒントをいただきましたので、
**速度計画** について検討を進めています。

# motion_velocity_smoother

**「とにかく低速で走る」** とはいえなるべくラップを短縮したいので、
横滑りやセンサ遅延がクリティカルに効いてきそうな **カーブ付近のみ減速** すればよいのでは？と思いました。

この戦略は、以下の `motion_velocity_smoother` ノードで実現できそうです。

https://github.com/AutomotiveAIChallenge/autoware.universe/tree/awsim-stable/planning/motion_velocity_smoother

以下記事に沿って、launchファイルに追加してみます。

https://zenn.dev/ttanaka3/articles/bff7086ef1d956

具体的には、201行目-225行目を以下のように変更します。

 - `motion_velocity_smoother`ノードの追記
 - `simple_pure_pursuit`ノードのパラメータ変更（input/trajectory, use_external_target_vel）

https://github.com/AutomotiveAIChallenge/aichallenge-2024/blob/main/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml#L201-L225

```reference.launch.xml
...
      <!-- Customizable -->
      <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
        <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
        <remap from="output" to="/planning/scenario_planning/trajectory"/>
      </node>
  
      <!-- 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>

    </group>
  </group>


  <!-- Pure Pursuit -->
  <let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)"/>
  <let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)"/>

  <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
    <param name="use_external_target_vel" value="false"/>  <!-- false へ変更 -->
    <param name="external_target_vel" value="4.16667"/>
    <param name="lookahead_gain" value="0.5"/>
    <param name="lookahead_min_distance" value="3.5"/>
    <param name="speed_proportional_gain" value="1.0"/>
    <param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>

    <remap from="input/kinematics" to="/localization/kinematic_state"/>
    <remap from="input/trajectory" to="/planning/trajectory"/>  <!-- /planning/trajectory へ変更 -->
    <remap from="output/control_cmd" to="/control/command/control_cmd"/>
  </node>
...
```

動きました！　RViz2上に`max_velocity`の値が表示されます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/ff2f7590-77e6-30aa-6899-3a2b47cbf316.png)

`motion_velocity_smoother`のパラメータには以下があるようで、`min_curve_velocity`, `decel_distance_before_curve`, `decel_distance_after_curve`などでより細かく速度計画を調整できそうです。

https://github.com/AutomotiveAIChallenge/autoware.universe/blob/awsim-stable/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml

# おわりに

速度計画について、戦略とその実現方法（`motion_velocity_smoother`）をご紹介しました。

ただこちらは、あくまで１つの案です。
カーブで減速せず、横滑りやセンサ遅延を考慮/予測した車両制御ができれば、
より速く走れると思います。

決勝当日は、皆さんの「俺の考えた最強の走行計画・車両制御」が見られること、
とても楽しみしております！

（横滑りをうまく利用して、**ドリフト**走行するようなチームが出てきたらとってもかっこいいな…　でもちょっと危ないかも…　と思ったりしています）


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