Raspberry Pi で間に合わせる(C による高速化)¶
要点
- Raspberry Pi 5 では、Mac なら一瞬で終わる計算が何十倍も遅くなることがあります。走行中は 100 ms ごとに判断を出さなければならず、遅れると安全機構が車を止めてしまいます。
- そこで、SLAM・経路生成・駐車の重い内側のループだけを C で書き、Python から呼んでいます。
- C 版は Python 版とビット単位で同じ結果になるように書き、テストで確かめています。C が使えない環境では Python 版がそのまま動きます。
【シム/ホスト検証】+ Pi での計測
対象: raspi/nav/_native.c・slam2d/core/_native.c(読み込みは native.py、組み立ては slam2d/core/cbuild.py)。確認した版: 上位 2d0a44e。
数値は開発記録(2026-11-02)の、Raspberry Pi 5 での計測です。SLAM と経路生成の C 化は 2026-11-02 の実機の周回で確かめました。駐車の C 化は実機ではまだ走らせていません。
なぜ C が要ったか¶
ソフトの大部分は Python と numpy で書いています。Mac で開発している間は十分に速かったのですが、Raspberry Pi では次の 2 つが重なりました。
- Pi の numpy は、行列計算のライブラリ(BLAS)が最適化されていない参照実装です。密な行列の掛け算や連立方程式は、Mac の 100 倍以上遅いことがありました。
- scipy も numba も入っていません。 よく使われる高速化の手段が使えません。
その結果、長いコースの経路生成に 75 秒かかり、駐車では探索が時間の上限(120 ms)に負けて失敗していました。 走行の判断が 150 ms 以上止まると、指令の途絶を検知した安全機構が走行の許可を外します(安全層)。
何を C にしたか¶
全部を C にするのではなく、計測して時間を食っていた内側のループだけを C に移しました。外側の流れや判断は Python のままです。
| 区分 | C にしたもの | 元の Python で遅かった理由 |
|---|---|---|
| SLAM | 占有格子のレイの行進、点群と地図の対応付け、総当たりの位置探しの採点、地図の面の法線 | numpy は全部のレイを最後まで一時配列で進める。C は当たった所で止められる |
| 経路生成 | 速度プロファイル、曲率、車体外形の余裕、見積もりラップの評価、道路グラフの細線化 | 速度の漸化式は 1 点ずつしか進めず、numpy で並べられない |
| 経路生成 | 走行ラインの最適化(QP) | 密な行列のまま解いていた。帯行列のまま分解して \(O(N^3)\) → \(O(N)\) |
| 駐車 | Hybrid A* の子の生成と衝突判定、Reeds-Shepp 曲線、ヒューリスティックの Dijkstra、局所地図の更新、点群どうしの照合 | 1 回の展開ごとに Python のオブジェクトを大量に作っていた |
方針 — Python 版と同じ答えを出す¶
- ビット単位で一致させます。 演算の順を Python 版とそろえ、コンパイラが積和をまとめる最適化(FMA)を禁止して組みます(
-ffp-contract=off)。sinとcosを 1 つの命令にまとめられて 1 ulp ずれたことがあり、それも禁止しています。テスト(test_native.py)で、Python 版と同じ値になることを確かめます。 例外は QP の帯行列の解法だけです。同じ方程式を別の手順で解くので、丸めが違います(見積もりラップの差は 0.04 % 以内)。 - C が組めなければ Python 版で動きます。 C 版は初回の読み込み時に Pi の上でコンパイルします。失敗しても遅くなるだけで、止まりません。
- numpy に残したもの。 3 × 3 の行列積や solve は、BLAS の丸めが環境ごとに違うので C にしていません。
- 式を変えるときは、Python 版と C 版の両方を直します。
効果(Raspberry Pi 5 での計測)¶
| 処理 | C にする前 | C にした後 |
|---|---|---|
| 長いコースの経路生成、仕上げまで | 75.4 s | 3.3 s(停車中に 2 本を並列) |
| 同じコースの、まず走れる経路だけ | 15.1 s | 0.67 s |
| 周回中(RACE)の 1 回の判断、中央値 / p99 | 11.4 / 14.8 ms | 6.7 / 9.4 ms |
| 自分の位置の復元(LOCATE)、p95 / 最大 | 215 / 354 ms | 107 / 158 ms |
| 駐車のシミュレータ試験の成功 | 16 / 20 | 20 / 20 |
| 駐車中の 1 回の判断、中央値 | 29.9 ms | 19.3 ms |
周回中と LOCATE の数値は、Pi 上でシミュレータを動かして測ったものです。経路生成の結果は、C にする前と同じ経路になることを確かめています。
実機の周回での確認(2026-11-02、27 周)
C にする前の記録と比べて、周回中の 1 回の判断の中央値は 13.9 → 8.6 ms、1 秒ごとの最大の最大は 280 → 33.7 ms になりました。 自分の位置の復元(LOCATE)の 1 秒ごとの最大は 413 → 120 ms、仕上げた経路への差し替えは約 35 s → 約 1.1 s です。