Autoware の MPC が「最適化した軌道」と「外に出す予測軌道」で 0.38 m ずれていた: ステア遅れと回帰テスト

Team Hayes（Ajayaditya Lokchandra, Nithisha Venkatesh）です。自動運転AIチャレンジで MPC を触っている流れで、Autoware の autoware_mpc_lateral_controller にある「予測軌道のずれ」を追いかけ、修正 PR に回帰テストをレビューとして付けました。

• issue: https://github.com/autowarefoundation/autoware_universe/issues/12398 （smk-robotics さんの報告）
• 修正 PR: https://github.com/autowarefoundation/autoware_universe/pull/12501 （mkquda さん）
• 私たちのレビュー（回帰テスト付き）: 上の PR のレビュー欄

結論: MPC が最適化に使う Frenet 座標の予測はステアの 1 次遅れを考慮していますが、外に出す world 座標の予測軌道はステア指令を即座に反映していました。既定のステア時定数 0.27 s では、5 秒先で横方向に最大 0.381 m ずれます。PR #12501 を当てると 0.051 m になります。既存のテストがこれを見逃していたのは、テストの時定数が 0.1 s で、そこではずれがほとんど出ないためでした。

何が問題か

autoware_mpc_lateral_controller は、MPC の解から 2 種類の予測軌道を作ります。

• Frenet 座標の予測: 最適化（QP）で使う線形モデルそのもの。状態は [横偏差, 方位偏差, ステア] で、ステアは時定数 steer_tau の 1 次遅れとしてモデル化され、双一次変換で離散化されています。
• world 座標の予測（/control/trajectory_follower/lateral/predicted_trajectory）: 外に出す軌道。車線逸脱チェッカーや制御・計画のバリデータが使います。

main の KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate は、world 座標の予測で dyaw = v * tan(input) / L と、ステア指令をその瞬間に反映していました。コードのコメントにも「本来はステアの 1 次遅れを考慮すべきだが、長い dt で離散化すると精度が落ちたので無視している」とあります。つまり、最適化した軌道と外に出す軌道とで、別の車を予測していることになります。issue #12398 では、これが 90° のカーブで安全チェックを誤作動させると報告されています。

修正（PR #12501）

PR は world 座標の更新にステアの 1 次遅れを入れます。

const auto dt_ratio = std::min(dt / m_steer_tau, 1.0);
auto current_steer = x0(2);
// ...
current_steer += (input - current_steer) * dt_ratio;  // first-order delay
dstate(2) = velocity * std::tan(current_steer) / m_wheelbase;

min(dt/τ, 1) で上限を付けているので、dt が τ より長くても発散しません。コメントにあった「長い dt で精度が落ちる」懸念にも対応しています。

回帰テスト

直線の参照軌道の上で、5 m/s・一定のステア指令 0.05 rad を 50 ステップ（dt 0.1 s）与え、KinematicsBicycleModel の Frenet 予測と world 予測の横位置の差の最大値を見ます。行列の積み上げは MPC::generateMPCMatrix と同じ形にしています。しきい値は 0.15 m です。

TEST_F(MPCTest, KinematicsWorldPredictionFollowsSteeringDelay)
{
  constexpr double velocity = 5.0;
  constexpr double steer_tau_default = 0.27;  // autoware default vehicle_model_steer_tau
  constexpr double dt = 0.1;
  constexpr int horizon = 50;
  constexpr double steer_cmd = 0.05;
  // ... build a_ex / b_ex / w_ex as MPC::generateMPCMatrix does, predict in both frames ...
  EXPECT_LT(max_lateral_gap, 0.15);
}

（全文は PR のレビューに貼っています。）

ghcr.io/autowarefoundation/autoware:universe-devel でビルドして実行した結果です。

# main (3c7d46e)
Expected: (max_lateral_gap) < (0.15), actual: 0.381416 vs 0.15
[  FAILED  ] MPCTest.KinematicsWorldPredictionFollowsSteeringDelay

# main + PR #12501
[       OK ] MPCTest.KinematicsWorldPredictionFollowsSteeringDelay
(max lateral gap: 0.0513 m)

なぜ既存のテストが見逃したか

既存の MPCTest のフィクスチャは steer_tau = 0.1 でした。時定数が短いと遅れの影響が小さく、修正前後でずれがほとんど変わりません。

steer_tau	main	PR #12501
0.1 s（既存テスト）	0.055 m	0.055 m
0.27 s（Autoware 既定）	0.381 m	0.051 m

テストの条件が、既定値より「簡単な」値に寄っていたために、既定値で起きている問題が見えていませんでした。修正後に残る 0.05 m は、Frenet 側（双一次変換）と world 側（陽的オイラー）の離散化の違いによるものです。

ほかの車両モデル

KinematicsBicycleModelNoDelay はそもそもステアの遅れを持たないモデルなので、即座に反映するのが正しい挙動です。DynamicsBicycleModel は状態にステアを持っていません。影響を受けるのは KinematicsBicycleModel（既定の vehicle_model_type: kinematics）だけだと考えています（レビューで作者に確認をお願いしています）。

自分のコードでも確認するなら

MPC の予測を外に出しているなら、「最適化に使ったモデル」と「外に出す軌道を作るモデル」が同じ遅れを持っているかを確認してください。テストは、実際に使う既定値（今回なら 0.27 s）で書くのがおすすめです。

AI の利用について

テストの作成には AI コーディングエージェントを使いました。数値は、上の C++ テストを main と PR 適用後の両方で実際にビルド・実行した結果と、同じモデルを Python で再実装した計算（図）で確認しています。両者の値は一致しています。

---

English

While working on MPC for the Autonomous Driving AI Challenge, we followed a "predicted trajectory deviation" in Autoware's autoware_mpc_lateral_controller and added a regression test to the fix PR as a review.

• Issue: https://github.com/autowarefoundation/autoware_universe/issues/12398 (reported by smk-robotics)
• Fix PR: https://github.com/autowarefoundation/autoware_universe/pull/12501 (by mkquda); our review carries the test

In short: the Frenet-frame prediction that the MPC optimises models the steering as a first-order lag, but the world-frame predicted trajectory it publishes applied the steering command instantly. With the default steer_tau of 0.27 s, the two differ by up to 0.381 m laterally over a 5 s horizon; with PR #12501 the gap is 0.051 m. The existing test missed it because its fixture uses steer_tau = 0.1, where the gap is small either way (figures above).

What is wrong. The controller builds two predictions from the MPC solution. The Frenet one is the QP's linear model: state [lateral error, yaw error, steer], steering as a first-order lag with time constant steer_tau, discretised with the bilinear transform. The world one (/control/trajectory_follower/lateral/predicted_trajectory) feeds the lane departure checker and the control/planning validators. On main, calculatePredictedTrajectoryInWorldCoordinate integrates v * tan(input) / L, applying the command at once; a code comment even says the lag was ignored because discretising it with a long dt lost accuracy. So the published trajectory and the optimised one predict different cars. Issue #12398 reports safety checks triggering on 90-degree turns.

The fix. PR #12501 adds the lag to the world-frame update, current_steer += (input - current_steer) * min(dt / steer_tau, 1), seeded from the current steer. The clamp keeps it stable when dt exceeds tau, which answers the old concern about long dt.

The test. On a straight reference, a constant 0.05 rad command for 50 steps of 0.1 s at 5 m/s, stacking the matrices as MPC::generateMPCMatrix does; the maximum lateral gap between the two predictions must stay under 0.15 m. Built and run in ghcr.io/autowarefoundation/autoware:universe-devel: 0.381416 m on main (fails), 0.0513 m with the PR (passes). The remaining 0.05 m comes from the different discretisations (bilinear in the Frenet model, explicit Euler in the world update).

Why it was missed. With steer_tau = 0.1 the lag barely matters, so the gap is 0.055 m before and after the fix. The test ran at an easier value than the default, where the problem actually lives. When you test a model, test it at the defaults you ship.

Other models. KinematicsBicycleModelNoDelay has no steering lag, so applying the command at once is correct there; DynamicsBicycleModel has no steer state. As far as we can see, only KinematicsBicycleModel (the default vehicle_model_type: kinematics) is affected; we asked the author to confirm.

How this was made: we used AI coding agents to draft the test. The numbers come from building and running the C++ test on main and with the PR applied, and from a Python re-implementation of the same model (figures); the two agree.

Team Hayes (Ajayaditya Lokchandra, Nithisha Venkatesh)