安全な速度制御の仕組み

自律計画と手動操作という2つの速度指令ソースを、単一の出力へ安全に集約し、さらに障害物への衝突を軽減する。この記事ではnewbotの速度制御パイプラインの設計を実装レベルで記す。

cmd_vel裁定パイプライン: 優先度とロックの二層制御

速度指令の集約には、性質の異なる2つの制御軸を使う。

Nav2 controller/behavior --(remap)--> /cmd_vel_nav
   --velocity_smoother--> /cmd_vel_smoothed
   --collision_monitor--> /cmd_vel_nav_safe ─┐
keyboard/procon teleop ------------> /cmd_vel_key ──┼─ twist_mux ─(cmd_vel_out=/cmd_vel)→ esp32_bridge
                                             │
                              /estop, /manual_override_active

優先度: keyboard(50) > navigation(10)。数値が大きいほど優先され、手動入力がある間は自律計画からの指令を無視する。 ロック: estop(255) > manual_override(100)。ロックは優先度とは別次元の仕組みで、発動している間は該当ソース以外の指令を完全に遮断する。

重要なのは、生の/cmd_vel_navを直接twist_muxへ接続しないことである。Nav2の出力はvelocity_smoother(加減速の平滑化)とcollision_monitor(直近障害物への衝突判定)を経由してから初めてtwist_muxに渡す。実際に、この経路の設定を誤り、collision_monitorの安全チェックを丸ごとバイパスする配線になっていたバグを実機投入前に発見・修正した経緯がある。

また、ROS2のある版でメッセージ型の既定値が変わったことに気づかず、twist_muxとNav2/駆動系との間でメッセージ型が食い違ったまま接続が一切成立していなかった、という罠も踏んでいる。ROS2はpub/subの型が一致しないと接続自体が成立しないため、指令は生成されていても実際には一度もモーターへ届いていなかった。手動運転は別経路(ESP32が直接コントローラー入力を処理)のため影響を受けず、一見動いているように見えていたのが発見を遅らせた要因である。

障害物ガード: 前進成分だけを止める非対称クランプ

Nav2のcollision_monitorは、自律計画(cmd_vel)経路にしか効かない。手動操作中はESP32がコントローラーの入力を直接モーター駆動へ変換するため、この安全機構は原理的に手動運転を制限できない(これは手動経路を上位コンピュータに依存させない設計思想の裏返しであり、欠陥ではない)。

そこで、自律・手動どちらの経路にも等しく効く、モード非依存の別チャンネルを新設した。「前進速度上限」という単一の値を、制御モードに関係なく常時ESP32へ送り続ける。

LiDAR(生の点群) ─┐
                 ├→ obstacle_guard ─/forward_speed_limit→ esp32_bridge ─"L,<mm_s>"→ ESP32
カメラ(障害物距離推定)─┘        (2つのセンサーの推定値のうち最小値を採用)

ESP32ファームウェア内部では、受信した左輪速度 $l$ ・右輪速度 $r$ の指令を、前進成分 $v$ と旋回成分 $w$ に分解する。

v = \frac{l + r}{2}, \qquad w = \frac{r - l}{2}

これは前節で扱った差動駆動の運動学(並進・旋回速度と左右輪速度の変換)そのものであり、上限値によるクランプは前進成分 $v$ にだけ適用してから、左右輪速度を再合成する。旋回・後退は一切制限しない。 障害物の手前で身動きが取れなくなる方が、前進速度が多少残ることより危険だと判断したためで、常に離脱の余地を残す設計である。

段階的なしきい値制御は以下の通り。

状態	距離しきい値	前進上限	旋回・後退
CLEAR	1.0m 超	無制限	可
SLOW	1.0m 以下	0.12 m/s	可
BLOCK	0.5m 以下	0(前進禁止)	可
CRITICAL	0.3m 以下	0(即制動)	可
SENSOR_LOST	全センサー途絶	既定でSLOW相当	可

状態可視化ダッシュボードで障害物ガードの検知距離(0.97m)とSLAM俯瞰図を表示している様子

実際の走行中の状態可視化ダッシュボード。障害物ガードが検知距離0.97mでCLEAR判定を出しており、下部には俯瞰図(既知の空き領域・障害物・走行軌跡)がリアルタイムで更新されている。

しきい値の発動と解除には、意図的に非対称なヒステリシス(既定0.1m)を設けている。発動は距離を検出した瞬間に即座に行い(安全側に倒す)、解除はしきい値からさらに0.1m余分に離れるまで待つ。対称なヒステリシスだと、境界線上をロボットがうろつくたびに上限のON/OFFが切り替わる(チャタリング)ため、これを防ぐ非対称設計にした。上限は目標速度と実際に適用される速度の両方に掛けており、至近距離で「加速度上限のせいで止まりきらない」という抜け道を塞いでいる。

二重のフェイルセーフ

Pi5は40ms周期で前進上限の指令をESP32へ送り続けるが、この指令が1秒間途絶すると、ESP32は自律的に固定の低速上限へフォールバックする。無制限にも戻さず、完全に0にもしない。上位コンピュータがクラッシュしても手動操作でロボットを退避させられる、という手動経路の独立性を壊さないための折衷点である。

実際に起きた接触事故と、その原因調査の過程

障害物ガードは机上の設計だけで完成したわけではない。2026年8月中旬、実機テスト中に前方・側面・後方の接触事故を複数回経験し、その都度原因を調べては設計を見直してきた。ここではその調査の過程を、当初の誤った仮説も含めて記す。

最初の仮説: LiDARの垂直画角による幾何学的な死角

最初に接触が起きたとき、原因はLiDAR(Livox Mid-360)の垂直画角(-7°〜+52°)が生む幾何学的な死角だと考えた。搭載高から逆算すると、距離0.3mでは高さ18.9cm以上の物体しか検知できず、距離が近いほど検知に必要な最低高さが上がる計算になる。この理論だけを見ると、前方の検知限界は計算上0.858m前後になるはずだった。

実測して分かった食い違い

ところが実際に前方・側方・後方それぞれで検知限界を測定すると、幾何学モデルの予測とはっきり食い違う結果が出た。前方は理論通り0.858m付近だった一方、側方(60〜90°)は0.389m、後方は0.36mと、いずれも垂直画角の計算だけでは説明できないほど短かった。もし原因が純粋な垂直画角の幾何学であれば、方位によらずほぼ一定の関係になるはずである。この食い違いから、根本原因は幾何学的なFOVの問題ではなく、LiDARの取り付け方向や機体自身による遮蔽(オクルージョン)の可能性が高いという、より正確な診断に至った。原因が完全に特定できていない状態でも、まず「何が確認された事実か」を一覧化し、そこから確度の高い対処に絞り込むという進め方をとった。

確認された事実の一覧化

原因が完全には特定できない中でも手を止めないため、確認できた事実だけを番号付きで並べ、そこから対処の優先順位を決める進め方をとった。主なものを挙げる。

前方0.858m以内、側方(60〜90°)0.389m以内、後方0.36m以内でLiDARの点群がゼロになる(未確認/推測ではなく実測)
旋回時のクランプ(rotationLimitMmSという変数)は宣言だけされており、実際にはコード上どこからも参照されていなかった。つまり「旋回を制限している」という設計ドキュメントの記述自体が実装と食い違っていた
後方方向は速度クランプの対象外で、ファームウェアのapplyForwardLimitはv<0(後退)の場合はクランプを一切通さない実装になっていた。これが実際に、Nav2の自動復旧動作(BackUp)による後方衝突を引き起こした直接原因である
地図(既に構築済みのSLAM地図)から逆算した距離推定は、0.020mまで有効に機能することを確認した。生のLiDAR点群だけに頼らない代替の距離情報源として使える
カメラを有効にするとCPU使用率が約75%まで跳ね上がり、自己位置推定(FAST-LIO)の更新頻度が10Hzから0.5Hzまで低下する。センサーを増やせば安全になるとは限らず、処理負荷とのトレードオフがある
全方位が同時にブロックされ、身動きが取れない状態(デッドロック)が45秒以上続いたケースが1回確認されている
一連の接触事故は速度0.12 m/sで発生しており、物理的な破損は一度も発生していない

設計思想の転換: 「素人発想・玄人実行」

これらの事実を踏まえ、安全設計そのものの方針を切り替えた。それまでの実装は、タイムラッチ・方位ごとの除外ゾーン・地図情報の記憶・複数センサーの最小値統合など、個別には合理的に見える工夫を積み重ねたものだったが、結果として「賢くしようとしすぎた」ことが不具合の温床になっていたと総括している。これを踏まえ、発想自体は素朴でよいが、実装は妥協しないという方針(「素人発想・玄人実行」)に転換した。

この方針の下で定めた実行上の原則は次の5つである。

フェイルセーフを既定値にする — 判断に迷ったら止まる側を既定動作にする
常に脱出の余地(creep)を残す — 完全な停止ではなく、低速ながら動ける方向を必ず残す
「発行された」ではなく「実際に届いて反映された」ことまで確認する — トピックにpublishしたことと、それが実際にモーターの挙動を変えたことは別の確認が要る
速度上限は実測に基づいて決める — 理論値ではなく、実際に接触事故が起きた速度域から逆算する
実際に起きた事故の数値を使った回帰テストを持つ(safety_math.py) — 一度直した不具合を再発させないための固定

4層のアーキテクチャへの再整理

このアーキテクチャは以下の4層で構成される。

[感知層]  LiDARの取付角度・向き
[執行層]  ESP32内の速度クランプ(左・右・後退それぞれ独立)
[政策層]  obstacle_guard(前進上限のみを扱う。複雑な例外処理を持たせない)
[計画層]  Nav2 / coverage_mission(経路計画・カバレッジ探査)

感知層ではLiDARを本体から15〜25度下向きに傾けて取り付ける案を検討している。取り付け角度ごとに「どの距離で床面が見え始めるか」を計算した上で選定する方針で、これにより至近距離の低い障害物への感度を、電子的な処理を増やさずに底上げできる。

最終防衛ラインとしての物理バンパー

上記のソフトウェア層をすべて信頼しても、ゼロにはならないという前提のもと、ソフトウェア・ネットワーク・上位コンピュータのいずれにも依存しない独立層として、物理的なバンパースイッチの追加を構想している。ノーマルクローズ接点をESP32のGPIO割り込みに直結し、Pi5やROS2ノードが全滅していてもモーターを止められる設計を狙っている。段階的な導入計画(現状の3層ソフトウェア防御→物理バンパー追加→統合検証)としてロードマップ化している。

ガードとNav2が互いを知らないという構造的な課題

障害物ガードを導入して接触事故は減ったが、代わりに別の種類の詰みが見えてきた。ガードは前進・旋回・後退それぞれを独立にクランプするが、Nav2はそのクランプの存在を一切知らない。Nav2は自分が出した速度指令どおりに機体が動くと想定しているため、実際には動いていないのにFailed to make progress(進捗が出ていない)と自分の失敗として解釈し、回復動作を選び直す。実測では、この「進捗なし」の判定が12回発生したのに対し、その回復策として選ばれるはずの「その場旋回」は0回しか実行されていない。つまりNav2の状況認識と、実際に機体を止めている原因とが噛み合っていないまま回復動作の選択が行われていたことになる。

さらに問題なのは、ガードが結果としての上限値しか外部に公開していないことである。「前進速度上限=0」という値からは、それが「前方0.15mに壁がある」ことに起因するのか、他の理由なのかが上位からは分からない。上位が判断材料として使える形の情報になっていない、というのがこの構造の核心的な弱点である。対処として、まず全方向が同時に塞がれている状態を検知して公開するトピックを追加したが、これはあくまで人間が気づくためのものであり、Nav2の判断ロジック自体は変わらない。より本質的な対処としては、クリアランス(前方・後方・全方位の距離)そのものを公開して上位が「どちらへ逃げられるか」を判断できるようにする案、あるいはガードのクランプ状態をNav2のコストマップに反映してプランナが最初から塞がった方向を選ばないようにする案(コストマップのカスタムレイヤとして実装可能)を検討している。後者が本筋の解決策になる。

3つの保護層が、それぞれ違う前提で動いている

障害物からロボットを守る仕組みは、実は単一ではなく3層ある。エッジ機上で生の点群を直接見る障害物ガード、PC上でNav2のcollision_monitorが/scanを見る層、そしてPC上のコストマップが同じく/scanとfootprint設定を見る層である。この3層は、それぞれ別のデータソース・別に保持された幾何定義で動いており、これらが食い違っていても、それに気づく仕組みが存在しない。実際に、前節で扱った車体後端寸法の誤りは、この3箇所のうちの一部だけを見ていては発見できなかった種類の不整合だった。

加えて、障害物ガードだけがエッジ機上で動くのは意図的な設計であり(PCやネットワークが落ちても保護が生き続けるようにするため)、それ自体は正しい判断である。しかし裏を返せば、このガードを何らかの理由で無効化すると、エッジ機側の保護はゼロになる。ガードを外した実験でもPC側のcollision_monitorは頻繁に反応していたが、これはPC側だけの保護であり、Wi-Fiが切れれば同時に失われる、という限界がある。

限界: これは「軽減」であって「保証」ではない

障害物ガードは衝突の可能性を減らすが、ゼロにはできない。LiDARは真上・真下・足元の至近距離が死角になり、カメラは水平画角の外(真横・後方)が完全に死角になる。単眼カメラの距離推定は床が平面という仮定に依存し、段差や光沢のある床では誤差が出る。検出から制動までには遅延があり、速度が上がるほどこの遅延が安全マージンを侵食する。したがって、モーター電源そのものを遮断する物理的なキルスイッチは、ソフトウェアの安全機構とは独立に必須である。

参考リンク

#Nav2 #ROS2