MPC Lab — 予測地平と操舵上限の中で曲率列を解き直す

目次 — 知りたいところから読む

入力を変えて確かめる

実験パネルを開き、実行を押すとPythonを読み込みます。停止と初期値へのリセットができます。結果はこの端末内で計算します。Pythonのインストールは不要です。

下にあるローカル実行の手順は、配布ソースを再現するための任意の手順です。ブラウザ実験には不要です。

予測軌跡を見ながら走らせる

「直線・横偏差1m」を実行し、再生してください。各時刻の緑の破線が、その時刻に解いた予測軌跡です。MPCは予測地平の曲率列をまとめて決めますが、車両に適用するのは先頭の1つだけで、次の周期（0.05秒後）にまた全体を解き直します。予測軌跡と実際の軌跡がずれるのは、予測モデルが近似であることと、後の周期で計画が更新されることの両方が理由です。

結果をAに保存してから「短い予測地平（3段）」や「長い予測地平（30段×0.2s）」を実行すると、同じ重みでも先をどこまで見るかで操舵の立ち上がりが変わります。予測地平の長さは段数N×刻みΔtの秒数です。

予測モデルと最適化問題

状態は経路に対する横偏差e_y（左が正）と方位誤差e_ψです。予測にはMPC用に単純化した線形モデルを使います。

e_{y,k+1} = e_{y,k} + v_k\,\Delta t\,e_{\psi,k},\qquad e_{\psi,k+1} = e_{\psi,k} + v_k\,\Delta t\,(u_k - \kappa_k)

u_kは指令する経路曲率[1/m]、κ_kは予測した到達位置での経路曲率、v_kは車両と同じ速度サーボで予測した速度です。操舵角はδ=atan(L u)で、操舵角の上限|δ|≤δ_maxは曲率の上下限|u|≤tan(δ_max)/Lと厳密に同じです。評価関数は

J = \sum_{k} q_y e_{y,k}^2 + q_\psi e_{\psi,k}^2 + r\,(u_k-\kappa_k)^2 + r_\Delta\,(u_k-u_{k-1})^2

で、曲率の上下限だけを持つ二次計画問題になります。u_{-1}は直前に実際に適用した曲率です。rは経路曲率からの外れ、r_Δは曲率の急な変化に対する重みです。予測は小角度近似の線形モデルですが、実際に動かす車両は非線形の共通自転車モデルです。この差も実験で観察する対象です。

ソルバーと、解けなかったときの扱い

二次計画は射影ニュートン法（Bertsekas 1982）で解きます。上下限に張り付いた変数を固定し、残りの変数でニュートン方向を求め、上下限への射影を含めて直線探索します。射影勾配の残差が1e-6 [1/m]以下なら収束です。前の周期の解を初期値に使うため、通常は数回以内で収束します。

反復上限に達しても収束しない場合の扱いは3通りから選べます。「途中解を使う」は上下限を満たす途中解を適用し、未収束として記録します。「前回の指令を保持」は直前の操舵を維持し、保持した周期として記録します。「停止」は走行を失敗（controller_failed）として終了し、理由を表示します。反復上限1回の3つのプリセットで違いを確認してください。この問題は上下限だけの制約なので、実行不能（解が存在しない）にはなりません。状態への硬い制約を加えた場合の実行不能はこのLabの範囲外です。

制約・重み・速度の関係

「8 m/s・既定の重み」は完走しますが、「8 m/s・変化率の重み小（発散例）」は経路から逸脱します。車両側には操舵変化率0.8 rad/sの制限がありますが、この最適化問題はそれを制約として持っていません。変化率の重みr_Δが小さいと、予測上は急な操舵を前提にした計画になり、実際の車両が追いつけずに振動が大きくなります。モデルに含めていない制約が閉ループを壊す例です。「横偏差の重み大」でも、急な修正が操舵変化率の制限に当たる様子を確認できます。

「急カーブ（曲がり切れない例）」は、半径5mの経路に対して車両が追従しきれない例です。同じ条件ではStanley Labも経路から逸脱します。制御則を変えても、車両と制約で決まる限界は越えられません。

共通車両と比較指標

車両はPP・APP・RPP・Stanleyと共通の後輪軸基準・前進のみの運動学的自転車モデルです。0.05秒刻みで最大30秒、速度上限8 m/s、加減速度±2 m/s²、操舵変化率0.8 rad/s。速度はMPCで最適化せず、他のLabと同じ a=2(目標速度−実速度) の速度サーボで決めます。このLabのMPCは操舵だけを扱います。

横偏差・方位誤差・RMS・終点判定は他のLabと共通の定義で、後輪軸基準です。終点を通過しても停止や駐車を意味しません。走行時間や完了状態が違う結果を、RMSだけで順位付けしないでください。

計算時間について

ソルバーの計算時間は、このブラウザ内のPython（Pyodide）での壁時計時間です。端末・ブラウザ・同時に動いている処理で変わり、車載コンピュータでの計算時間や制御周期に間に合うかを示すものではありません。性能評価には、対象ハードウェアと実装（例えばOSQPのような専用ソルバー）で最悪計算時間を測る必要があります。

試す順番

基準の条件で再生し、予測軌跡と実際の軌跡の違いを観察する。
予測地平の段数と刻みを変え、操舵の立ち上がりと横偏差を比較する。
8 m/sの2つのプリセットで、変化率の重みと操舵変化率の制限の関係を確かめる。
反復上限1回の3つのプリセットで、未収束のときの扱いの違いを読む。

ブラウザ内で計算し、再生・コマ送り・A/B比較・リセット・共有・JSONダウンロードが使えます。