経路追従と速度指令¶

【シム／ホスト検証】＋【実機確認】（周回で使用）

対象: purepursuit.py、mpc.py、speedctl.py。確認した版: 上位 19291eb。 MPC 追従（ctrl_mode = 1）は 2026-10-13 の実機走行の解析にも登場します（開発記録: MPC の修正で舵指令の毎周期の反転が 47〜68 % → 20 %）。 Pure Pursuit との実機での優劣の比較記録は確認できていません。

経路（レーシングライン）と速度プロファイルが決まったら、それを追う側は舵と速度の 2 つに分かれます。舵は Pi が計算し、速度は STM32 の車速 PI が追うので、その遅れを Pi 側が見込んで先回りする、という分担です。

Pure Pursuit¶

後輪軸中心を原点とした自転車モデルで、目標点への方位 \(\eta\) と前方注視距離 \(L_d\) から舵角を決めます。

\[ \delta = \arctan\!\left(\frac{2L\sin\eta}{L_d}\right),\qquad L_d = k_v\,v + L_{\min} \]

\(L\): ホイールベース 0.23 m。基準点を後輪軸に置いているのは、Pure Pursuit の標準式が後輪基準で書かれているからです。
\(L_d\) は速度に比例させます。固定にすると、低速では大回りし、高速では蛇行が発散します。

遅延補償¶

ステアリングは 50〜200 ms 遅れ、UART の往復と Python の処理でさらに 30〜50 ms 乗る、という見積もりがコードにあります（実測では操舵そのものはほとんど遅れない: システム構成）。合計 150 ms は 3 m/s で 45 cm 先です。現在位置で目標点を選ぶと、その 45 cm ぶん常に手遅れの舵を切ることになります。

そこで、判断する場所を t_delay 秒後の予測位置へ進めます。 予測は、今の速度と実舵角を使った自転車モデルです（[dynamics] が既定値を与える）。

速度はすぐ先のプロファイルから読む¶

舵の注視点（遅延予測＋ \(k v + L_{\min}\)）の速度をそのまま指令すると、2 m/s で約 1.5 m・0.75 秒先の速度を今出すことになり、 コーナーの手前で早く減速しすぎ、立ち上がりでは早く踏みすぎます。 速度は、遅延予測の位置から v · speed_preview_s だけ先の値にします。

曲率フィードフォワード¶

Pure Pursuit は「今の位置から注視点へ届く円弧」の曲率を出すので、経路の上にぴったり乗っていても、注視区間（3 m/s で約 1.6 m）で平均した曲率で曲がります。曲率が変わり続ける S 字では、これが内側を切る定常的な偏りになりました（3 m/s で ±15 cm、横 G を上げると衝突）。「車が経路の点 \(i\) に経路の向きでいたら Pure Pursuit が出す曲率」\(\kappa_{\mathrm{ideal}}\) を求め、その点の経路の曲率 \(\kappa_{\mathrm{path}}\) との差を足します。

\[ \kappa = \kappa_{pp} + g\,(\kappa_{\mathrm{path}} - \kappa_{\mathrm{ideal}}) \]

経路上にいれば \(\kappa = \kappa_{\mathrm{path}}\)（偏りなし）で、ずれの修正は Pure Pursuit のまま残ります。

横偏差は「評価」であって「入力」ではない

cross_track は Pure Pursuit の計算に入りません（目標点への方位だけで決まる）。走りの良し悪しを人間が読むための数字として返しています。これが増え続けるなら、前方注視か遅延補償が合っていません。

MPC による追従（代替）¶

Pure Pursuit の代わりに舵だけを決めます（速度は経路の速度プロファイルをそのまま使う）。follow() と同じ引数・同じ Pursuit を返すので、planner が差し替えるだけで比較できます。

経路に対する横偏差 \(e_y\) と向きの偏差 \(e_\psi\) の線形化自転車モデル

\[ \dot e_y = v\sin e_\psi,\qquad \dot e_\psi = \frac{v\tan\delta}{L} - \frac{\kappa_{\mathrm{ref}}\, v}{1-\kappa_{\mathrm{ref}}\,e_y} \]

を、\(\delta_{\mathrm{ref}} = \arctan(L\kappa_{\mathrm{ref}})\)（曲率を丸ごと打ち消す舵角）の周りで線形化します。\(e=0,\ \delta=\delta_{\mathrm{ref}}\) が平衡点なので、フィードフォワードは別に要りません。むだ時間は、初期状態を delay_s 先へ進めて扱います（Pure Pursuit と同じ遅延補償）。

決定変数は各ステップの舵角、コストは \(\sum (q_y e_y^2 + q_\psi e_\psi^2) + r_u\sum(\delta-\delta_{\mathrm{ref}})^2 + r_d\sum(\Delta\delta)^2\)。
numpy だけで N×N の QP にし、箱制約（舵角限界と舵角レート）を射影ニュートン法で解きます。 FISTA は条件数が悪く 40 反復でも収束せず、舵が振動したので使いません。scipy・cvxpy は Pi の依存に無いので使いません。
制約なしの解析解を箱へ射影して初期値にするので、制約が効かない場面（ほとんど）はその場で収束します。

速度指令の前倒し（`SpeedFF`）¶

STM32 は目標車速をランプで追い（3.0 m/s²）、PI でトルクにします（駆動制御）。比例項だけで見ると、加速度 \(a\) を出すには「目標 − 車速」が \(a \times T_v\)（\(T_v = 1/(\text{車体の利得}\times K_p)\) ≒ 0.16 s）だけ必要です。 経路の速度をそのまま送ると、車速はその分だけ遅れます。 加速では遅いだけですが、減速ではコーナーへ速すぎる速度で入ります （実車のヘアピン進入で、目標 0.7 m/s に対して 1.2〜1.3 m/s だった）。

\[ v_{\mathrm{cmd}} = v(s') + T_v\cdot \mathrm{HP}\bigl[a(s')\bigr] + k_{fb}\,\bigl(v(s) - v_{\mathrm{meas}}\bigr) \]

項	意味
\(v(s'),\ a(s')\)	経路の速度プロファイルと、その傾きから出した加速度（\(a = v\,dv/ds\)）
\(T_v\,a\)	PI の比例項の逆算。等加速度で走っている間の遅れを消す
\(\mathrm{HP}[\cdot]\)	時定数 \(K_p/K_i\) のハイパス。長く続く加減速では PI の積分が比例項を引き継ぐので、足したままにすると行き過ぎる（線形の範囲では、これが PI ＋車体の厳密な逆モデルになる）
\(s'\)	減速は `lead_s` 秒先、加速は `lead_acc_s` 秒先の点（2 つの候補のうち目標が低くなる方を採る）。減速だけ早めに始め、加速は早めない（コーナー出口で早く踏むと、まだ曲がっている所で速度が出る）
\(k_{fb}\)	車速がプロファイルから外れたぶんを目標に足し戻す（同定値のずれ・TC の介入の保険）

値は Vehicle（vehicle.toml の同定値）と指令の遅れから決まり、手で合わせる数字は減速の前倒しの係数 LEAD_RATIO だけです。

ファームの時定数との突き合わせ

STM32 の加速度フィードフォワード（\(\tau = 20\) ms のローパス、Pi の指令周期そのもの）の値は、Pi 側では vehicle.toml の [dynamics] speed_ff_filter_s だけが持ち、planner とシミュレータの両方がそこを読みます（駆動制御）。ファームの値とずれていないかは tools/fwsim/tests/test_fw_constants.py が確かめます。

設計の要点¶

下位の遅れを上位が先回りして吸収する。 下位のモデル（ここでは PI ＋車体の逆モデル）を上位に持たせると、ランプや PI を変えたときにここも直す必要が出る。そのための同期箇所がコードのコメントで明示されている。
ある量を評価に使うか入力に使うかを区別する。 横偏差は評価専用。
安全の判断を速度プロファイルに混ぜない。 近づきすぎの停止は STM32 の auto_stop に任せる。