最近傍 waypoint の argmin が 1.5 m で逆向きの区間に飛ぶ: 折り返しコースの自己位置の罠

Last updated at 2026-09-12Posted at 2026-09-12

Team Hayes（Ajayaditya Lokchandra, Nithisha Venkatesh）です。自動運転AIチャレンジ2026 のスターターキットに入っている MPC（multi_purpose_mpc_ros）を読んでいて見つけた、自己位置の割り当ての問題を短くまとめます。

結論: kashiwanoha プリセットでは、経路から横に 1.5 m ずれるだけで、車が「逆向きに走る別の区間」にいると判定されます。1.5 m は実車の GNSS 誤差や追従誤差として普通にありうる大きさです。

何が起きているか

core/spatial_bicycle_models.py の get_closest_waypoint() は、全 waypoint までの距離を計算して、その argmin を返します。

distances = np.sqrt((np.array([wp.x for wp in self.reference_path.waypoints]) - x)**2 +
                    (np.array([wp.y for wp in self.reference_path.waypoints]) - y)**2)
closest_wp_id = np.argmin(distances)

前回どこにいたかは使っていません。この関数を update_states() が毎制御周期呼び、結果で s（経路上の位置）を上書きします。s から参照速度と MPC の予測区間が決まるので、wp_id が飛ぶと制御の前提がまとめて別の区間のものになります。

10 行で再現する

ROS は要りません。multi_purpose_mpc_ros/ で実行します（aichallenge-racingkart dev ac3f929）。

import numpy as np
a=np.genfromtxt('env/kashiwanoha/traj_mincurv.csv',delimiter=',',names=True)
xy=np.c_[a['x_m'],a['y_m']]; s=a['s_m']; psi=a['psi_rad']
i=np.argmin(abs(s-152.1)); n=np.array([-np.sin(psi[i]),np.cos(psi[i])])
for d in (1.0,1.4,1.5,1.6):
    for side in (1,-1):
        j=np.argmin(np.hypot(*(xy-(xy[i]+side*d*n)).T))   # get_closest_waypoint と同じ
        print(d, side, round(s[j],1))

1.0 1 152.1
1.0 -1 152.1
1.4 1 152.1
1.4 -1 152.1
1.5 1 152.1
1.5 -1 291.2
1.6 1 152.1
1.6 -1 291.2

s=152.1 m にいる車が片側に 1.5 m ずれると、s=291.2 m と判定されます。139 m 先の、逆向きの区間です。

なぜ kashiwanoha だけで起きるのか

kashiwanoha のラインは折り返していて、s≈146-167 m と s≈277-298 m の 2 区間が方位差 180° で並走し、最も近いところで 2.96 m しか離れていません。中間点（約 1.5 m）を越えた瞬間に、反対側の区間の waypoint のほうが近くなります。全 waypoint・両側で横ずれを振ると、2.0 m 以内のずれで飛ぶ点は 754 中 29 でした。

既定の citycircuit（final_ver3）は、隣り合わない区間どうしが最短でも 8.52 m 離れているため、6 m ずれても飛びません。スターターキットの既定コースで問題が出ないのはこのためです。

直し方

前回の一致点の近くだけを探すようにしました。

前回の一致点から [-5, +30] の範囲だけを探索する（周回コースでは wrap）
窓の中の最短距離が 5 m を超えたときだけ、全経路の argmin に戻る（初回、リセット、テレポート）
update_reference_path() で記憶を消す（古い経路のインデックスは意味を持たないため）

探索点数は 1 回あたり約 350-380 点から 36 点に減ります。kashiwanoha と final_ver3 の実際の地図と経路を使ったテストでは、修正前は jumped from wp 152 to wp 287 で失敗し、修正後は test/ 全体の 18 件が通りました。final_ver3 では、従来の全経路探索と同じ位置を返すことも確認しています。

先に気づいていた人

同じ現象は、tamasy さんの Zenn 記事「自動運転AI challenge 2026 活動軌跡〜MPC制御をやってみた」§5 で先に報告されています（「コースが折り返している場所では別ループの waypoint が最近傍になる」）。私たちはそれをスターターキットのコードで再現し、数値にして Issue と PR にしました。
https://zenn.dev/tamasy/articles/4aa96d2e665713

作り方について

調査とパッチ作成には AI コーディングエージェントを使いました。数値はすべて、上の再現スクリプトと、修正前に失敗し修正後に通るテストで確認しています。

English

Team Hayes (Ajayaditya Lokchandra, Nithisha Venkatesh). A short write-up of a self-localisation problem we found while reading the MPC in the JSAE AI Challenge 2026 starter kit (multi_purpose_mpc_ros).

In one line: on the kashiwanoha preset, a 1.5 m lateral error is enough for the car to be placed on a different leg of the course that runs the opposite way. 1.5 m is an ordinary GNSS or tracking error on a real kart.

What happens

get_closest_waypoint() in core/spatial_bicycle_models.py computes the distance to every waypoint and returns the argmin (code above). It ignores where the car was on the previous cycle. update_states() calls it every control cycle and overwrites s with the result. The reference velocity and the MPC horizon both come from s, so when wp_id jumps, the controller's whole picture of the road jumps with it.

Reproduce it in ten lines

No ROS needed; run the script above in multi_purpose_mpc_ros/ (aichallenge-racingkart dev ac3f929). A car at s = 152.1 m, offset 1.5 m to one side, is matched to s = 291.2 m: 139 m further on, on a leg running the other way (figure 1).

Why only kashiwanoha

The kashiwanoha line folds back on itself. The legs at s ≈ 146-167 m and s ≈ 277-298 m run antiparallel and come within 2.96 m of each other, so past the midpoint (about 1.5 m) the other leg's waypoints are closer. Sweeping every waypoint and both sides, 29 of 754 cases jump at 2.0 m or less (figure 2). On the default citycircuit line (final_ver3) the closest non-adjacent legs are 8.52 m apart, and nothing jumps up to 6 m. That is why the default course never shows it.

The fix

Search only near the previous match:

look at [-5, +30] waypoints around the previous match, wrapping on circular paths;
fall back to the whole-path argmin only when nothing in that window is within 5 m (first call, reset, teleport);
clear the memory in update_reference_path(), because indices into the old path mean nothing.

Each call now checks 36 points instead of about 350-380. Using the real kashiwanoha and final_ver3 maps and paths, the test fails before the fix with jumped from wp 152 to wp 287, and all 18 tests in test/ pass after it. On final_ver3 the windowed search returns the same location as the whole-path search.

Credit

tamasy reported the same failure mode first, in section 5 of their Zenn article (https://zenn.dev/tamasy/articles/4aa96d2e665713). We reproduced it on the starter-kit code, measured it, and turned it into an issue and a PR.

How this was made

We used AI coding agents for the investigation and the patch. Every number here was checked with the repro script above and with a test that fails before the fix and passes after it.

Team Hayes の自動運転AIチャレンジ2026 シリーズ / series

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up