Regulated Pure Pursuit Lab — 曲率と終点距離で速度を変える

まず速度規制の有無を比較する

上のブラウザ実験で「急カーブ・規制あり」を実行し、結果をAに保存します。次に「適応距離のみ」を実行してください。同じ経路、初期姿勢、車両、規制前の目標速度で、曲率と終点接近の減速だけを外した比較になります。横偏差だけでなく、実速度と終点通過時間を見てください。遅く走った結果を誤差だけで優劣判定しないことが重要です。

注視距離の設定と速度規制は別のグループです。「固定距離＋速度規制」は適応距離だけを切ります。「曲率規制なし」「終点減速なし」で各規則を切り分け、「規制速度の下限を上げる」で減速候補が下限によって上げられる例を調べます。プリセットは全条件を戻すため、A/Bの条件差も確認してください。

注視距離は操舵目標、規制は速度指令

適応ONでは Ld=clamp(vT,Lmin,Lmax)、OFFでは固定Ld。vは現在のモデル速度です。目標点と曲率κはPP/APPと共通で、車両座標の目標(xb,yb)から κ=2yb/(xb²+yb²)、δ=atan(Lκ)。円と経路の交点がなければ投影点から経路長Ldだけ進んで終点で制限し、実際の目標距離を分母に使います。目標が後方または一致する場合は理由を表示して終了します。

速度指令は次の順序で計算します。

1. 曲率規制ONなら、R=1/|κ| として目標速度に min(1,R/Rmin) を掛ける。κ=0では減速しない。
2. 曲率候補を「曲率規制後の速度下限」以上にする。この下限は安全限界ではなく、減速を弱めることがある。
3. 残り経路長が接近距離Dより小さいとき、終点への直線距離/Dで減速する。接近速度下限を適用し、接近前の値よりは上げない。
4. 最後に0〜規制前の目標速度へ制限する。共通の速度制御 a=2(指令−実速度) と加減速度上限が実速度を決める。

例えば目標4m/s、κ=0.25/m、Rmin=8mなら曲率候補は2m/s。下限0.25m/sではそのままです。残り経路1.5m、終点距離1.5m、接近距離3mなら接近後は1m/s（接近下限0.1m/s）。残り経路がちょうど3mなら接近減速はまだ始まりません。

再生で速度の決まり方を見る

再生・一時停止・コマ送り・時刻スライダーで各瞬間を確認できます。表示は「目標 → 曲率候補 → 下限適用 → 接近減速後の指令」と実速度を区別します。速度図の灰点線は目標、緑は規制後の指令、青/橙は実速度。Aは破線、Bは実線です。経路・姿勢・注視円・目標点と、横偏差・操舵角・注視距離の時間変化も表示します。数値表とJSONは計算データから作成します。

3つの作用時間は別々にdtを加算するため、重なる場合があります。「曲率候補が目標速度未満の時間」は速度下限で打ち消された場合も含み、実速度が下がった時間そのものではありません。

参照版とモデルの違い

速度規則はNav2 Jazzyの固定コミット f4108e5b1c2bce804a1aa0c7be6673a8eb4a1501 を参照しています。スカラー計算の順序を照合していますが、Nav2の実行結果ではありません。本教材は共通の後輪軸基準モデルと全経路の最近傍投影を使い、残り経路長=全長−投影位置です。Nav2が受け取る変換・切り出し済みローカル経路と入力が一致するとは限りません。曲率規制には制約適用前の追従曲率を使います。

刻み0.05秒、最大30秒。速度上限8m/s、加減速度±2m/s²、操舵±0.6rad、操舵変化率0.8rad/s。後輪軸の正確な一定曲率移動を区間ごとに積分します。実速度は指令へ瞬時には変わりません。慣性、タイヤの滑り、障害物、コストマップ、衝突予測、逆走、旋回して方位合わせ、別の曲率注視点は含みません。安全性や実機性能の評価ではありません。

残り経路長と終点距離が共に0.25m以下、最終接線との方位差0.15rad以下で「終点通過」です。停止を保証せず、接近下限が正でも駐車を意味しません。10m超の経路距離で逸脱、30秒で時間上限。未完了の通過時間は「—」。横偏差・方位誤差RMSは二乗サンプルの台形時間積分を走行時間で割った平方根です。途中終了や走行時間が異なる結果を誤差だけで比べないでください。

パラメータ名は版に依存する

接近減速のON/OFFは教材用の比較スイッチで、JazzyのROSパラメータ名ではありません。Rollingのdynamic-window関連機能も実装していません。教材の初期値はNav2の設定推奨値ではありません。

同じコードから生成した例

初期値の急カーブ（半径5m）、初速度と目標速度4m/s、横偏差1m、ホイールベース2.5mでネイティブPythonから生成しました。

参考資料と関連Lab

• Pinned Jazzy speed functions
• Jazzy controller and constraint order
• Jazzy parameter guide
• Rolling parameter guide
• APP Lab / PP Lab

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

• Stanley Lab