4
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

はじめに

自動運転AIチャレンジ2025の決勝大会において、私たち「とっとこちゅら太郎」は、一般クラス1位になりました!!
決勝当日はあいにくの雨天となり、非常に難しい路面コンディションでしたが、そのような状況下でも各チームが素晴らしい走行を披露されていて、白熱した戦いが繰り広げられていました。
このようなエキサイティングな大会を実現してくださった、参加者の皆様、そして準備から当日まで手厚くサポートしてくださった運営の皆様、本当に、本当にありがとうございました!

自動運転AIチャレンジ2025とは

公益社団法人 自動車技術会が主催する自動運転技術の大会です。
7月から始まり約3ヶ月とわりと長丁場の大会となっています。予選はシミュレーター(AWSIM)上でラップタイムを競い、一般クラス上位8チームが決勝に進みました。決勝では実車のEVレーシングカートに開発したプログラムを搭載して走らせ、1周分の最速タイムを競います。
image.png

開催概要はこちらです。

オープンソースの自動運転ソフトウェア「Autoware」を用いた、運営により大会用にカスタマイズされたコードをベースに、パラメータを調整したり、独自実装を追加するなどが主な競争ポイントかと思います。

本記事では、決勝大会での取り組み内容について振り返ります。

決勝大会に向けた開発アプローチ

まず、開発を進めていく上で、前回2024年大会優勝者Roborovskyさん、2025年予選1位通過Paper Driversさんの記事を多々参考にさせていただきました。大変勉強になりました!感謝です!

私たちのチームの取り組みとしては、主に3つのアプローチに分けて紹介します。

  1. 練習会での実車走行データ分析
    決勝大会前に実施された練習会での実車走行データ(rosbag)から、実環境の特性を分析して課題を抽出し、対策を考えました。
  2. 経路追従制御ロジックの改善
    Dual Preview Points法などを参考に、デフォルトのPure Pursuitを改造しました。
  3. 便利ツールの作成
    Trajectory Editer や、CLIによるパラメータ調整ツールなどを作成しました。

開発過程ではClaude Code(AIエージェント型コーディングツール)を活用し、ツール開発やデータ分析スクリプトの作成、制御ロジックの実装などコーディング全般で効率化しています。家事をしている間にゴリゴリ実装してくれるのでほんとに助かる!!!

1.練習会での実車走行データ分析

走行軌跡データ

経路追従制御ロジックは、デフォルト実装のPure Pursuitを利用して、低速からスタートし、速度を徐々に上げていき各速度域での走行データを記録しました。10km/hからスタートし2km/hずつ速度を上げて、固定された経路に対する基本的な軌道追従性能の確認を行いました。
こちらが走行軌跡、カラーマップは車速(km/h)を表しており、車速が上がるたびに、経路に対して追従結果がオーバーステアやアンダーステアが増加しています。
trajectory.png

課題1:アンダーステア (外に膨らむ)

コーナーによっては大きく膨らんでしまう箇所もあり、コースアウトしてしまうこともありました。
路面の状況や車両の特性もあるかと思いますが、カーブ手前での減速が必要になってくると考えました。

課題2:オーバーステア (内に切り込む)

設置のブロックを擦ったりして、なかなか危うい走行。
Pure Pursuitという制御ロジックの特性として、前方注視点距離が遠くなるとカーブ出口付近のTrajectory Pointを見に行き過ぎて経路が内側へ切り込むようになります。目標経路に対するオーバーシュートが原因ではないかと考えました。
デフォルトの実装では、速度によって前方注視点距離が調整されるようになっていて、速度が上がると前方注視点距離を遠く設定しています。

double lookahead_distance = lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_;

課題3:決勝用AWSIM(シミュレーション)と実環境の違い

決勝用に配布されたAWSIMは、実環境を模擬するためのノイズやモデル化が施されていましたが、実環境と比較して過剰であったため、かえって実環境と乖離していました。
主には3つのノイズ特性の違いがあったかと思います。

  • ステアリング応答遅延
  • GNSSノイズ
  • 速度バイアス

ステアリング応答遅延

実環境では、ほとんど遅延はない状況でした。

シミュレーション
image.png

実環境
steering_delay_debug.png

GNSSノイズ

シミュレーションの方がばらつきが大きく、実車両では想定以上に安定していました。
自己位置推定に関する心配はほとんど必要ない状況でした。

シミュレーション
image.png

実環境
image.png,  width="400"

速度バイアス

Paper Driversさんの記事でも述べられている通り、既知の情報として、決勝用AWSIMでは、速度バイアスにより自己位置推定結果(EKF)が実際の車両位置(GNSSを正として)に対してずれてしまう問題がありました。図のように、EKFの青線がGNSS観測点群の密集中央を少し外しています。
実環境ではバイアスはなく、EKFとGNSSはぴったり重なっており、GNSSが正しければ自己位置推定精度はとても安定してそうです。

シミュレーション
image.png

実環境
image.png

予選版AWSIMの採用

課題3について、理想的には決勝用AWSIMを実環境に近づけるようなチューニングをしたかったのですが、難しくて断念・・・
代わりに 予選時の最初に配布されたAWSIM(ノイズが少ない版) を開発環境として使用しました。結果として、Trajectoryの調整や制御ロジックの動作確認をスムーズに実施できました。

予選版の特徴:

  • GNSSノイズが小さく、実車両に近い精度
  • ステアリング応答が速く、実車両に近い特性
  • 速度バイアスがない

2.経路追従制御ロジックの改善

練習会の走行データ分析より、課題1のアンダーステア、課題2のオーバーステアに対して制御ロジックの改善を試みました。

課題1アンダーステアへの対策:曲率ベースの速度制御

速度を上げていくと、そのままカーブに進入した場合、タイヤのグリップ限界を超えてアンダーステアによるコース逸脱や、スリップが発生する可能性が高まります。
対策については、昨年度唯一の高校生チーム「EIPIII NEXUS」さんの記事を参考にさせていただきました。

記事にもある通り、デフォルトの実装では、Trajectory Pointに制限速度情報を付与し、それを参照するハードコーディングな手法となっていました。しかし、実環境のコンディションに合わせた調整を、決勝レース中にその場で行うのは困難であると考えました。
そこで記事を参考に、前方の経路曲率を先読みして、カーブ手前で速度を調整するように改造しました。

どれだけ先の経路を見たらよいのか について、EIPIII NEXUSさんが、Pure Pursuit制御で使われる前方注視距離を活用されていたのはナイスアイディアだと思います!Pure Pursuitの前方注視距離は速度で調整されるよう実装されているので、減速に必要な距離を速度に合わせていい感じに調整できます。

どれくらい減速したらよいか については、横加速度制限を設定して、曲率から制限速度を算出しました。この横加速度制限のパラメータを変更することで、どれくらいカーブ手前で減速するかを制御できます。

横加速度に基づくカーブでの安全速度の算出

車両がカーブを曲がる際に発生する横加速度 $a_{lat}$ は、車両にかかる遠心力を表し、走行速度 $v$ とカーブの曲率 $\kappa$によって決まります。

$$
a_{lat} = v^2 \cdot \kappa \quad \left( \kappa = \frac{1}{R} \right)
$$

過剰なアンダーステアやスリップをしないためには、この発生する横加速度 $a_{lat}$ が、路面やタイヤのコンディションで決まる最大許容横加速度 $a_{lat_max}$ を超えてはいけません。
許容範囲 $a_{lat} \le a_{lat_max}$ を満たす最大の速度 $v_{limit}$ を求めることで、そのカーブでの安全な制限速度が導出されます。

$$
v_{limit}^2 \cdot \kappa = a_{lat_max}
$$

これを $v_{limit}$ について解くと、以下の算出式が得られます。

$$
v_{limit} = \sqrt{\frac{a_{lat_max}}{\kappa}}
$$

実装例
double SimplePurePursuit::calculateCurvatureBasedTargetSpeed(
    size_t lookahead_idx) const
{
  // ステップ1: 前方注視点から曲率を計算
  double curvature = calculateCurvatureFromLookahead(*trajectory_, lookahead_idx);
    
  // ステップ2: 曲率から目標速度を計算
  double curv_target_vel = calculateTargetSpeedFromCurvature(curvature);
    
  return curv_target_vel;
}
  
double SimplePurePursuit::calculateCurvatureFromLookahead(
  const Trajectory& trajectory,
  size_t lookahead_idx) const
{
  const size_t traj_size = trajectory.points.size();

  size_t p1_idx = lookahead_idx;
  size_t p2_idx = (lookahead_idx + 1) % traj_size;
  size_t p3_idx = (lookahead_idx + 2) % traj_size;

  return calculateCurvatureFrom3Points(
    trajectory.points[p1_idx],
    trajectory.points[p2_idx],
    trajectory.points[p3_idx]);
}

double SimplePurePursuit::calculateCurvatureFrom3Points(
  const TrajectoryPoint& p1,
  const TrajectoryPoint& p2,
  const TrajectoryPoint& p3) const
{
  // 3点から円の曲率を計算(Menger curvature)
  double ax = p2.pose.position.x - p1.pose.position.x;
  double ay = p2.pose.position.y - p1.pose.position.y;
  double bx = p3.pose.position.x - p2.pose.position.x;
  double by = p3.pose.position.y - p2.pose.position.y;

  double area = std::abs(ax * by - ay * bx) / 2.0;

  double a = std::hypot(ax, ay);
  double b = std::hypot(bx, by);
  double c = std::hypot(
    p3.pose.position.x - p1.pose.position.x,
    p3.pose.position.y - p1.pose.position.y);

  if (a < 1e-6 || b < 1e-6 || c < 1e-6) return 0.0;

  double curvature = 4.0 * area / (a * b * c);
  return curvature;
}

double SimplePurePursuit::calculateTargetSpeedFromCurvature(double curvature) const
{
  // v = sqrt(a_lat_max / curvature)
  double target_speed = std::sqrt(max_lateral_accel_mps2_ / curvature);

  return target_speed;
}
  

課題2オーバーステアへの対策:二重前方注視点と曲率ベース重み付け

Pure Pursuit法はシンプルで実装が容易ですが、ステアリング指令が前方注視距離という唯一のパラメータに依存しています。Pure Pursuitは、指定した距離先の点へ向かうという単純な幾何で動作するため、カーブ入口~中盤と出口では、同じ前方注視距離でもまったく異なる挙動になります。

前方注視距離 カーブ入口~中盤の挙動 カーブ出口の挙動 特徴
遠距離 - 注視点が出口側になる
- 経路をショートカットしやすい
オーバーシュート(内側切れ込み)
- 遠くの直線方向を見る
- 必要な操舵量が不足しにくい
アンダーシュートが起きにくい
入口に不利・出口に有利
近距離 - 手前を見て過剰な切れ込みを抑える
オーバーシュート抑制
- 手前を見るため旋回が遅れる
アンダーシュート(出口で膨らむ)
入口に有利・出口に不利

制御ロジック特性によるオーバーシュートの結果オーバーステア的な挙動となっていると考えました。
そこで、Dual Preview Points法(DPP法) で提案される、近距離注視点・遠距離注視点という2つの前方注視点の導入を参考にしました。
DPP法の詳細については以下の記事を参照ください。

私たちのチームでは、この2つの前方注視点から算出されるステアリング角に対して、先の経路曲率によって重み付けをすることで、カーブ形状(曲率)に応じて制御特性を変化させました。
曲率が大きくなるカーブ入口~中盤では近距離注視点を重視しオーバーシュートを抑制し、カーブ出口付近では遠距離注視点を重視し立ち上がりをスムーズにするよう設計しました。

重み付け実装例
std::pair<double, double> SimplePurePursuit::getDPPWeights(
  size_t far_lookahead_idx) const
{

  // DPP のための曲率適応型ウェイト計算
  // 遠方の lookahead 点からさらに先の 3 点を使って曲率を計算し、
  // これにより「これから先のカーブ形状」に応じた予測的な重み調整を行う
  const size_t traj_size = trajectory_->points.size();

  // 遠方 lookahead 点から始まる連続 3 点を取得
  // (周回コースを想定し、インデックスは循環させる)
  size_t idx1 = far_lookahead_idx % traj_size;
  size_t idx2 = (far_lookahead_idx + 1) % traj_size;
  size_t idx3 = (far_lookahead_idx + 2) % traj_size;

  // 上記 3 点から曲率を計算
  double forward_curvature = calculateCurvatureFrom3Points(
    trajectory_->points[idx1],
    trajectory_->points[idx2],
    trajectory_->points[idx3]
  );

  // ウェイト計算のための線形補間
  // κ = 0   (直線)        → w_near = 0.3   (遠方lookahead重視)
  // κ = 0.15(最大カーブ)→ w_near = 0.7   (近距離lookahead重視)
  const double max_curvature = 0.15;  // 最大曲率の閾値
  double curvature_clamped = std::clamp(forward_curvature, 0.0, max_curvature);

  // 線形補間式: w_near = 0.3 + (0.4 / 0.15) * κ
  double w_near = 0.3 + (0.4 / max_curvature) * curvature_clamped;
  double w_far = 1.0 - w_near;

  return {w_near, w_far};
}

経路追従挙動の比較
緑色がTrajectory、青点線が走行軌跡です。
Pure Pursuit:前方注視点距離ゲイン 1.0
スクリーンショット 2025-11-20 020010.png
2 Points Pure Pursuit:遠距離注視点距離ゲイン 1.0, 近距離注視点距離ゲイン 0.5
スクリーンショット 2025-11-20 015350.png
基準の注視点距離に対して、その半分の距離の注視点を追加して、曲率により重み調整した結果。カーブ入口のオーバーシュートの抑制と、カーブ出口の立ち上がりスムーズ化を狙い通りできていそうです。カーブ中盤では少し膨らんでしまっています。

※本実装は DPP 法の理論に沿ったものではなく、あくまで独自ロジックによる参考実装です。理論に忠実な実装を行う場合は、DPP 法そのものをご確認ください。また、実車両での挙動についてはデータがないため不明です。

3. 便利ツールの作成

Trajectory Editor

カートレース競技ということもあり、コース取りなどの最適化も重要なポイントとなってきます。有志の方が作られたツールが公開されていたり、各チームこういったツールは作成されていたと思います。

私たちのチームでも個別機能が欲しかったりで独自に作成しました。曲率の計算を安定させるため、滑らかで一定間隔な経路ポイントの配置がしたく、補間やスムージング機能が欲しいなどです。大会期間中は最後の最後までずっとお世話になったツールです。

image.png

対話型のROS2パラメータ調整ツール

レース中は限られた時間内にパラメータを調整する必要があるため、タイムロスを減らすために作成しました。SSHなどによってネットワーク経由で車両PCに接続が可能なため、車両PC内で実行される想定のツールです。

  • 現状のパラメータ値の表示と更新
  • パラメータ番号と変更値のみの最小限の入力

レース終了時にモニター画面を撮影して最終調整パラメータを簡単に記録できるのもポイントです。

image.png

サンプルコード
#!/usr/bin/env python3

import subprocess
import sys

# ノード名定義
PP_NODE = "/simple_pure_pursuit_node"

# パラメータ定義
PARAMS = {
    1: {
        "name": "lookahead_gain",
        "node": PP_NODE,
        "ros_param": "lookahead_gain",
        "type": "float",
    },
    2: {
        "name": "steering_tire_angle_gain",
        "node": PP_NODE,
        "ros_param": "steering_tire_angle_gain",
        "type": "float",
    },
}


def run_command(cmd):
    """コマンドを実行して結果を返す"""
    result = subprocess.run(cmd, shell=True, capture_output=True, text=True)
    return result.returncode == 0, result.stdout.strip(), result.stderr.strip()


def get_param_value(node, param_name):
    """パラメータの現在値を取得"""
    cmd = f"ros2 param get {node} {param_name}"
    success, stdout, stderr = run_command(cmd)
    if success:
        value = stdout.split(":")[-1].strip() if ":" in stdout else stdout.strip()
        return value
    return None


def set_param_value(node, param_name, value):
    """パラメータを設定"""
    cmd = f"ros2 param set {node} {param_name} {value}"
    success, stdout, stderr = run_command(cmd)
    return success


def display_params():
    """現在のパラメータ値を表示"""
    print("\n" + "=" * 70)
    print("Current Parameters:")
    print("=" * 70)

    for param_id in sorted(PARAMS.keys()):
        param_info = PARAMS[param_id]
        value = get_param_value(param_info["node"], param_info["ros_param"])

        if value:
            print(f"{param_id}: {param_info['name']:30s} = {value}")
        else:
            print(f"{param_id}: {param_info['name']:30s} = (failed to retrieve)")

    print("=" * 70)


def validate_value(param_id, value_str):
    """入力値を検証して変換"""
    param_info = PARAMS[param_id]
    param_type = param_info["type"]

    try:
        if param_type == "string":
            # 文字列型の場合、選択肢をチェック
            if "choices" in param_info:
                if value_str not in param_info["choices"]:
                    print(f"  ✗ Invalid choice. Must be one of: {', '.join(param_info['choices'])}")
                    return None
            return value_str
        elif param_type == "float":
            return float(value_str)
        elif param_type == "int":
            return int(value_str)
    except ValueError:
        print(f"  ✗ Invalid value type. Expected {param_type}")
        return None

    return None


def update_param(param_id, value):
    """パラメータを更新"""
    if param_id not in PARAMS:
        print(f"  ✗ Invalid parameter ID: {param_id}")
        return False

    param_info = PARAMS[param_id]
    current = get_param_value(param_info["node"], param_info["ros_param"])

    print(f"\nUpdating: {param_info['name']}")
    print(f"  Current: {current}")
    print(f"  New:     {value}")
    print(f"  ", end="")

    # パラメータを設定
    if set_param_value(param_info["node"], param_info["ros_param"], value):
        print("✓ Parameter updated")
        return True
    else:
        print("✗ FAILED to update parameter")
        return False


def print_help():
    """ヘルプメッセージを表示"""
    print("\nCommands:")
    print("  <id> <value>  - Update parameter (e.g., '2 1.5')")
    print("  refresh, r    - Refresh parameter display")
    print("  help, h       - Show this help message")
    print("  quit, q       - Exit the program")
    print()


def main():
    """メインループ"""
    print("=" * 70)
    print("Interactive Parameter Tuner")
    print("=" * 70)

    # 初期表示
    display_params()
    print_help()

    while True:
        try:
            # ユーザー入力を取得
            user_input = input("\nEnter command: ").strip()

            if not user_input:
                continue

            # 終了コマンド
            if user_input.lower() in ["quit", "q", "exit"]:
                print("\nExiting...")
                break

            # リフレッシュコマンド
            if user_input.lower() in ["refresh", "r"]:
                display_params()
                continue

            # ヘルプコマンド
            if user_input.lower() in ["help", "h"]:
                print_help()
                continue

            # パラメータ更新コマンド (id value)
            parts = user_input.split()
            if len(parts) != 2:
                print("  ✗ Invalid command. Use: <id> <value>")
                continue

            try:
                param_id = int(parts[0])
                value_str = parts[1]
            except ValueError:
                print("  ✗ Invalid command format. ID must be a number.")
                continue

            # 値を検証
            validated_value = validate_value(param_id, value_str)
            if validated_value is None:
                continue

            # パラメータを更新
            if update_param(param_id, validated_value):
                # 更新後の値を表示
                param_info = PARAMS[param_id]
                new_value = get_param_value(param_info["node"], param_info["ros_param"])
                print(f"  Verified: {param_info['name']} = {new_value}")

        except KeyboardInterrupt:
            print("\n\nExiting...")
            break
        except Exception as e:
            print(f"  ✗ Error: {e}")

    return 0


if __name__ == "__main__":
    sys.exit(main())

決勝での戦略と調整

基本戦略

カーブでの減速目標を算出するための横加速度制限を高めに設定し、カーブ前での減速を強めて安全にカーブを走行できる状態から、少しずつ緩和してスピンしないなどの限界を探る方針。特に雨によるウェット路面であったため、カーブでスピンしやすい状況だったと思います。

決勝1日目:想定外の挙動と対応

問題1: 想定以上に内に切り込んだ

原因:実車両のステアリング感度が、シミュレーションより良かった。
対応steering_tire_angle_gain を調整。

問題2: 直線で蛇行した

原因:シミュレーションではステアリング操舵が機敏で、かなり近い前方注視点距離でも動作していたが、実車両ではステアリングを切る速度が間に合わなかった。
対応:前方注視点距離を少し遠目に設定して安定化。

問題3: コーナー出口で外に膨らみがち

原因:実車両特性によるところがあると思われ、急なコーナリングが難しそう。
対応:2日目に向けて、よりアウトから入るようなコーナリング経路に修正。

image.png

オペレーションの改善

1日目は都度ROS2コマンドでパラメータ変更していましたが、手間が多くオペレーションがスムーズでなかったため、2日目に向けて対話型のROS2パラメータ調整ツールを作成。

決勝2日目: 運も味方した?

前日調整したパラメータで良い感じの走行を確認し、順調に横加速度制限を緩和してスピンするギリギリのところまで調整できました。終了間際に車両不具合で停止してしまいましたが、なんとか車両が耐えて頑張ってくれました!ありがとう!!お疲れ様!!

比較的良好な路面コンディション

私たちのチームの走行までに雨が止んでいる時間が長かったため、走行時は水たまりも少なく、路面状況が比較的良好でした。これにより次のような効果があったのではと考えています。

  • タイヤのグリップが効いてスピンしにくかった
  • 水の抵抗が少なく加速が良かった

その後また雨が降り出してしまったので、有利なコンディションでの走行結果のままフィニッシュとなるなど、順位が良かったのは運かなと考えています。

おわりに

取り組み内容について全てというわけではありませんが、主だったものを紹介しました。至らない点が多々あるかと思います。次回大会に向けて、少しでも皆様の参考になれば幸いです。

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

4
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
4
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?