5
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

高校生がPure Pursuitを応用して実装した曲率ベース速度制御【自動運転AIチャレンジ】

5
Last updated at Posted at 2025-10-14

はじめに

こんにちは。昨年度「自動運転AI Challenge 2024」にEIPIII NEXUSというチームで参加し、リーダーをしていた林です。

私たちは、昨年、参加145チームの中から唯一の高校生チームとして決勝の舞台に立つことができました。

この記事は、自動運転AIチャレンジに参加される方々に向けた技術共有が第一の目的ですが、それと同じくらい、リベンジというか、やり残したことを消化したいという気持ちで書いています。

というのも決勝当日、自分たちのミスでマシンの挙動がおかしくなってしまい、開発してきた速度制御アルゴリズムの性能を十分に発揮できずに終わってしまったからです。(原因は、この記事のテーマとは全く関係ない、ステアリング周りのパラメータ設定でした…)

もちろん、速度制御の分野にはMPC(Model Predictive Control)や学習ベースの最適化など、より洗練された手法が多く存在します。
それでも、限られた環境の中で「自分なりに考えて形にした」試みとして、この経験をまとめておきたいと思いました。
同じようにこれから決勝に挑戦する方が、この記事を読んで何かのヒントを得てもらえたら嬉しいです。

なので、この記事では、そんな悔しい思いをした私が「うまく動作していたらもっといいタイムを出せていた(かもしれない)」と本気で思っていた速度制御アルゴリズムを、考え方から実装まで全部紹介します。

自動運転で「コースに合わせて速度を変えたい」と思っている人の参考になれば嬉しいです。

開発理由

決勝でタイムを出すために、私たちが向き合った最大の課題は「いかにメリハリのある走りをするか」でした。つまり、安全マージンを取りすぎてタイムを落とすのではなく、直線では最大加速、カーブではスリップしないギリギリの速度で曲がる、というアグレッシブな速度制御です。

これを実現するには、マシン自身に「今どこが直線で、どこからがカーブか」を判断させる必要がありました。

最初に思いついたのは、走行するmap自体に「この区間は40km/h、このカーブは15km/h」のように制限速度をハードコーディングする案です。しかし、この方法では、本番の路面状況やマシンのコンディションに合わせて現地で速度を微調整するのが非常に難しく、柔軟性に欠けると判断しました。

そこで、より賢く、そして柔軟な方法として、マシンが走行経路の形状(Point Data)からリアルタイムに"曲がり具合"=曲率を計算し、それに応じて速度を決定するアルゴリズムを発案するに至りました。

実装の概要

この速度制御の核心は、Pure Pursuit制御で使われる前方注視距離(Look-ahead Distance)の考え方を、速度制御のための曲率計算に応用したことです。

当初、曲率を計算する前方距離を固定値にすると、高速走行時に減速が間に合わなかったり、逆に低速時に早く減速しすぎたり、という問題がありました。そこで、ステアリング制御で使われている「速度が速いほど遠くを見る」というLook-ahead Distanceの仕組みを、速度制御にも流用したのが、このアルゴリズムの最も重要な点です。

具体的な処理の流れは以下の通りです。

  1. まず、現在の速度に基づき、Pure Pursuitで利用する前方注視距離(Look-ahead Distance)を動的に決定します。
  2. 次に、その前方注視距離の分だけ先の経路上で、これから通過する道の曲率(カーブの急さ)を計算します。
  3. 最後に、計算した曲率に応じて、次の目標速度を線形的に決定します(詳細はStep 3で説明)

この一連の処理をループさせることで、「速度が速いほど遠くのカーブを予測して減速を始め、速度が落ちると注視点も手前に移動し、カーブの出口で即座に加速に移る」という、まるで人間のような滑らかな運転が実現できます。

Step 1: 速度に応じた前方注視距離の決定

まずは、現在の速度 current_speed から、Pure Pursuitの肝である前方注視距離(Look-ahead Distance)を計算します。

// 現在速度に応じて前方注視距離を動的に変化させる
double calculateLookaheadDistance(double current_speed, 
                                 double min_speed, double max_speed, 
                                 double min_dist, double max_dist) {
    double ratio = (current_speed - min_speed) / (max_speed - min_speed);
    return min_dist + ratio * (max_dist - min_dist);
}

速度が速いほど遠くを“先読み”する距離が長くなります。この距離が、次のステップで「どれくらい先のカーブを見るか」の基準になります。

Step 2: 前方経路上での曲率の算出

次に、Step 1で計算した前方注視距離の先にある経路が、どれくらい曲がっているか(曲率)を計算します。

// 経路の曲率を計算する関数
double calculateCurvatureRate(const Trajectory &trajectory, size_t index, size_t range) {
    double max_angle_diff = 0.0;
    for (size_t i = index; i < index + range && i + 2 < trajectory.points.size(); ++i) {
        const auto &p1 = trajectory.points[i];
        const auto &p2 = trajectory.points[i + 1];
        const auto &p3 = trajectory.points[i + 2];

        // 3点の角度変化を計算
        double angle1 = std::atan2(p2.pose.position.y - p1.pose.position.y,
                                   p2.pose.position.x - p1.pose.position.x);
        double angle2 = std::atan2(p3.pose.position.y - p2.pose.position.y,
                                   p3.pose.position.x - p2.pose.position.x);

        double diff = angle2 - angle1;
        while (diff > M_PI) diff -= 2.0 * M_PI;
        while (diff < -M_PI) diff += 2.0 * M_PI;

        if (std::fabs(diff) > std::fabs(max_angle_diff))
            max_angle_diff = diff;
    }
    return std::fabs(max_angle_diff);
}

ここでは、経路上の3点間の角度変化の最大値を曲率として定義しています。この値が大きいほど、急なカーブであることを意味します。

Step 3: 曲率に応じた目標速度の決定

最後に、Step 2で算出した曲率 curvature を使って、次の目標速度を決定します。

// 曲率に応じて速度を調整する関数
double adjustSpeedForCurvature(double curvature, 
                              double min_curvature, double max_curvature, 
                              double min_speed, double max_speed) {
    // 線形補間(remap)
    curvature = std::clamp(curvature, min_curvature, max_curvature);
    double ratio = (curvature - min_curvature) / (max_curvature - min_curvature);
    double speed = max_speed - ratio * (max_speed - min_speed);
    return std::clamp(speed, min_speed, max_speed);
}

カーブが急(曲率が大きい)なほど速度を落とすという関係を線形補間(remap)で実現しています。

Step 4: 全体の統合

これら3つのステップを、メインループ(onTimer関数)内で以下のように統合します。

// onTimer()関数内での処理の流れ

// ... 現在位置や現在速度を取得 ...

// Step 1: 速度に応じた前方注視距離を決定
double lookahead_distance = calculateLookaheadDistance(current_longitudinal_vel);

// Step 2: 前方経路上での曲率を算出
double curvature_rate = calculateCurvatureRate(closet_traj_point_idx, lookahead_distance);

// Step 3: 曲率に応じた目標速度を決定
target_longitudinal_vel = adjustSpeedForCurvature(curvature_rate);

// (この後、同じlookahead_distanceを使ってステアリング角も計算する)

このように、ステアリング制御と速度制御で参照する見越し距離を一致させたことが、安定した走行性能に繋がりました。

調整時に意識したこと

カーブごとの速度変化が急だと姿勢制御が不安定になるため、曲率をスムージングして使用。
曲率の閾値(min_curvature / max_curvature)は実験的に調整。
実機とシミュレーターで挙動が異なるため、低速域では補正を強めに設定。

リポジトリ公開

本記事で紹介したアルゴリズムを含むコードは、以下のGitHubで公開しています。

👉 https://github.com/eipiiiii/aichallenge-2024

おわりに

本記事では、私が自動運転AIチャレンジのために開発した、独自の速度制御アルゴリズムを紹介しました。

このアルゴリズムの要点は、Pure Pursuitの「前方注視距離」の考え方を速度制御に応用したことです。

  1. 現在の速度から前方注視距離を決定する。
  2. その距離の分だけ先の曲率を計算する。
  3. 曲率から目標速度を決定する。

このループによって、ステアリングと速度の制御が連携し、滑らかな走行が実現できます。

今年は大学受験のため大会には参加できませんでしたが、この記事が大会に取り組む方々の参考になれば嬉しいです。
そして、「こうすればもっと良くなる」「こんな発想もある」といったご意見をぜひコメントでいただけると幸いです。
来年は、今回の経験を糧に、さらに進化した形で挑戦したいと思っています。

5
1
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
5
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?