Pure Pursuit Lab — 固定注視距離で経路追従を比較

条件を変えて経路を追う

上のブラウザ実験で「実行」を押してください。灰色は参照経路、青は車両の後輪軸中心の軌跡です。円は注視距離、点は選択した目標、線分はホイールベースを表します。再生・一時停止・コマ送りと時刻スライダーで、曲がり始める位置を調べられます。数値表とJSONには同じ計算結果を保存します。

PP: 距離を固定する

PPは速度が変わっても設定した注視距離 Ld を保ちます。まず基準の曲線を実行し、結果をAに保存してから「短い注視距離」「長い注視距離」を比較してください。横偏差だけでなく、操舵の飽和や終点まで到達したかも確認します。

制御方式の選択でPPとAPPを切り替えられます。PPの固定距離とAPPの時間・上下限はそれぞれの方式だけに作用します。Aを保存してから方式だけを変えれば、経路・初期姿勢・速度指令・車両を維持した比較になります。プリセットは他の条件も初期化するため、条件差の一覧を必ず確認してください。

目標点と曲率

後輪軸中心から半径Ldの円と経路が交わる最初の点を、最近傍投影点より先の経路上から選びます。車両座標の目標を(xb, yb)とすると、κ = 2 yb / (xb² + yb²)、δ = atan(L κ)です。左旋回を正とします。

交点がない場合は投影点から経路長Ldだけ進め、終点で制限します。円との交点・復帰目標・終点を表示で区別し、曲率の分母には常に実際の目標距離の二乗を使います。目標が車両の後ろ、または現在位置と一致して完了条件を満たさない場合は、理由を記録して停止します。この復帰・終点方針は本教材の選択であり、Nav2の再現ではありません。

共通モデルと比較の限界

状態は後輪軸中心(x, y)[m]、反時計回りyaw[rad]、非負速度v[m/s]。dx/dt=v cos(yaw)、dy/dt=v sin(yaw)、dyaw/dt=v tan(δ)/Lです。初期横偏差は左が正、方位偏差は最初の経路接線からの角度です。直線・半径20m/5mの四分円・S字は0.1m刻みの開いた折れ線で、S字の刻みはx方向です。

刻み0.05秒、最大30秒。速度上限8m/s、加減速度上限2m/s²、操舵上限0.6rad、操舵変化率上限0.8rad/s。初期操舵は0です。各区間で操舵を制限して一定保持し、一定曲率の移動を厳密に積分します。速度はa=2(vtarget−v)で制御し、0と上限に達した後の移動も積分します。摩擦、慣性、タイヤの滑り、障害物、計測遅延はありません。

横偏差は最近傍線分の左法線方向、方位誤差は車両yaw−経路接線を[-π,π)に折り返した値です。RMSは二乗値の台形時間積分を走行時間で割って平方根を取ります。終点の前後方向の超過は横偏差に含めず、別の経路距離指標に含めます。操舵変化率RMSは初期0からの変化も含む離散指令の指標です。

残り経路長と終点距離が共に0.25m以下、最終接線との方位差0.15rad以下で「終点通過」です。停止や駐車を保証せず、標本間で通過を見逃す場合があります。経路距離10m超で逸脱、30秒で時間上限です。未完了の通過時間は「—」。途中終了した結果と完走結果を誤差だけで順位付けしないでください。速度は同じ指令でも終了時刻が異なり得ます。実機の安全性や高速性能を示す実験ではありません。

検証済みの合成例

次の値は同じコードをネイティブPythonで実行して生成したものです。初期速度・目標速度2m/s、初期横偏差1m、方位偏差0、ホイールベース2.5m、緩やかな曲線を使用します。

参考資料と関連Lab

• Coulter, Implementation of the Pure Pursuit Path Tracking Algorithm (1992): 幾何学的な追従則。
• Nav2 Iron RPP documentation: 速度比例の注視距離というAPPの用語を参照。完全なNav2実装ではありません。
• APP Lab
• MPC

ソースのダウンロードは任意です。ブラウザ実行にPythonのインストールは不要です。 Source + model notes.

• RPP Lab

• Stanley Lab