nav-autonomy-deploy：LiDAR ベース自律ナビゲーションのための本番展開システムのオープンソース化

Contextual Agentic Vision Platform に関する取り組みの補足エンジニアリングノートである。Platform は知覚層と、その知覚層が動作するハードウェアの双方にまたがる。ハードウェア展開は事後的な作業ではなく、Platform が現場へ到達するうえでの戦略的な軸である。本稿はそのハードウェア側の一断面を扱う。すなわち nav-autonomy-deploy、Platform のオープンソース移動ロボットランタイムであり、固定センサで対象を捉えるのではなく、移動プラットフォームを介して検査対象へ到達する必要がある場合に用いる。

自律移動ロボット内部のナビゲーションアルゴリズムは、合理的な基準のほとんどにおいて、既に解かれている。香港大学発の密結合 LiDAR 慣性オドメトリ枠組みである FAST-LIO2 は、100 Hz のオドメトリとマッピングを達成し、1,000°/s の回転下でも安定して動作する（Xu et al., IEEE TRO 2022）。カーネギーメロン大学発の動的可視性グラフベース経路プランナーである FAR Planner は、既知環境と未知環境の双方をリアルタイムでナビゲーションできることが評価され、IROS 2022 Best Student Paper Award を受賞した（Yang et al., IROS 2022）。これら二つのシステムを、地形走行可能性解析、衝突回避、ウェイポイント追従の各モジュールと組み合わせたものが、CMU の Autonomous Exploration Development Environment の中核を構成している。これは DARPA Subterranean Challenge において CMU-OSU チームに 「Most Sectors Explored Award」 をもたらしたナビゲーションスタックでもある（Cao et al., ICRA 2022）。これらのコードはすべてオープンソースである。

CMU の開発環境は、システム開発と実機展開の両方を明示的に想定して設計されている。Go2 向け autonomy stack の派生版には、SLAM、経路計画、衝突回避を Unitree Go2 のオンボード計算機上で動作させる完全な実機立ち上げ経路が含まれている（Zhang, autonomy_stack_go2）。とはいえ、研究用の展開ワークフローと、実運用に耐えるシステムとの間には、無視できない隔たりが存在する。後者は、異なるロボットプラットフォーム間で再現性高く展開でき、本番運用で管理可能で、遠隔から操作できる必要がある。クロスハードウェア抽象化、コンテナによる環境隔離、マップライフサイクル管理、ランタイムモニタリング、ワンコマンドでの起動、こうした運用層は学術リポジトリには存在せず、しかも小さな層ではない。

この空白を埋めるために構築したのが nav-autonomy-deploy である。CMU のナビゲーションスタックを、コンテナ化され、ハードウェア抽象化され、運用に耐える形態にパッケージングしたオープンソース展開システムだ。docker compose up -d 一発で、対応するロボットプラットフォーム上に自律ナビゲーションが立ち上がる。本稿では、そのアーキテクチャと、背後にある主要な工学的判断を順に解説する。

ナビゲーションスタック

nav-autonomy-deploy 内部の中核となるナビゲーションパイプラインは、単一の Docker コンテナの中で五つのコンポーネントを稼働させる。それぞれが何を担い、どこから来たものかを把握しておくことが、続く展開エンジニアリングの議論の前提となる。

FAST-LIO2 は SLAM を担う。特徴点ベースの SLAM 手法とは異なり、FAST-LIO2 は密結合の反復カルマンフィルタを用いて生の LiDAR 点群を直接マップに位置合わせし、LiDAR ハードウェアの切替に対して多くの SLAM システムを脆弱にしている、手作業の特徴抽出工程を排している。マップは増分 k-d ツリー（ikd-Tree）上で保持され、リアルタイムでの点群挿入と削除に対応する。結果として、Intel と ARM の双方のプロセッサ上で 100 Hz で動作し、急峻な運動と雑然とした環境を扱え、回転式とソリッドステートの LiDAR を再構成なしで切り替えて利用できる SLAM システムが得られている（Xu et al., IEEE TRO 2022）。本スタックでは、CMU の Go2 autonomy stack の既定実装である Point-LIO を、FAST-LIO2 で置き換えている。

FAR Planner はグローバル経路計画を担う。簡約された可視性グラフを動的に構築・維持し、障害物を多角形としてモデル化し、エッジ点を抽出し、得られたグラフ上で衝突回避経路を計算する。重要な性質は、既知環境と未知環境の双方で動作する点だ。事前マップが与えられれば最適経路を計画し、未マップ領域では複数経路を試行しながらナビゲーション中に環境構造を学習する（Yang et al., IROS 2022）。本プランナーは低い頻度で再計画を行い、長距離ウェイポイントを供給する。それを実行に移すのが、次に述べるローカルプランナーである。

CMU の base autonomy system に由来する local planner が、実時間の衝突回避を担う。モーションプリミティブのライブラリを事前生成し、ランタイムでは障害物に遮蔽されるものを除外して、衝突回避経路を選択する。入力となる地形走行可能性マップは terrain analysis モジュールが供給し、3D 点群を用いてロボット周辺の局所地形を走行可能・走行不可の領域に分類する。waypoint follower がグローバル層とローカル層を結び、FAR Planner のウェイポイントを閉ループでローカルプランナーに供給する。

生の LiDAR 入力から速度指令までを含むパイプライン全体が、ROS 2 ワークスペース内の単一実行スレッド上で動作する。CMU は autonomy stack 全体で CPU 使用率 10% 未満、ローカル経路計画は周期あたり 1 ms 未満かつ CPU 1% 未満 で動作すると報告している（cmu-exploration.com）。これは見過ごせない数値だ。ナビゲーションアルゴリズムは計算上のボトルネックではない、ということを意味する。ボトルネックは、後述する通り、それ以外のすべての部分にある。

コンテナ化された展開

ROS 2 ナビゲーションスタックのコンテナ化は、Web アプリケーションのコンテナ化とは性質が異なる。Docker の標準的な前提（ネットワーク隔離、ステートレスなプロセス、揮発的なファイルシステム）は、物理ハードウェアへのアクセス、マルチキャストによるピア発見、起動間でのマップ永続化を必須とする実時間ロボティクスシステムの要件と、ほぼ全面的に衝突する。Docker 構成における各設計判断は、これらの衝突のいずれかを解消するために存在している。

ネットワークモード。ROS 2 の既定ミドルウェアである Fast DDS は、ノード発見にマルチキャスト UDP を用いる。Docker のブリッジネットワーキングは、ホスト側とコンテナ側のネットワーク間でマルチキャストトラフィックを転送しない。本稿では network_mode: host を採用し、コンテナにホストのネットワークインターフェースへ直接アクセスさせている。これは意図的なトレードオフだ。ネットワーク隔離を失う代わりに、現実に動作する DDS 発見を得る。マルチロボット構成では、ROS 2 通信ドメインの分割を ROS_DOMAIN_ID（既定: 42）に委ねている。

共有メモリ。Fast DDS は、同一ホスト内通信の既定トランスポートとして共有メモリを使用する。Docker の既定共有メモリ割り当てである 64 MB は、ROS 2 ノード間を流れる点群および占有グリッドデータの量に対して不足する。本稿では 8 GB（shm_size: '8gb'）を割り当てており、これによって極めて診断困難なメッセージのサイレントドロップを防いでいる。サイレントドロップが起きると、システムは一見正常に動作しているように見えるが、ナビゲーションの判断が古い、あるいは不完全なデータに基づいてしまう。

ハードウェアアクセス。コンテナは LiDAR（Ethernet 経由）、モータコントローラ（シリアルデバイス経由、典型的には /dev/ttyACM0）、ジョイスティック入力デバイス（/dev/input）、GPU（/dev/dri）への直接アクセスを必要とする。privileged モードで動作させ、デバイスノードを明示的に渡している。NET_ADMIN と SYS_ADMIN のケーパビリティによって、コンテナは LiDAR サブネットをランタイムで構成可能になる。LiDAR ネットワークインターフェース側で計算機の IP を設定し、ゲートウェイを構成し、ポイントツーポイント接続を確立する作業を、起動前にホストネットワーキングを手動構成することなく行えるわけである。

環境変数によるハードウェア抽象化。新しいロボットに合わせて変更すべき対象は、単一の .env ファイルに集約されている。主要なインターフェースは、ロボットプラットフォーム選択（ROBOT_CONFIG_PATH：mecanum 駆動、Unitree Go2、Unitree G1）、LiDAR ネットワーク構成（LIDAR_INTERFACE、LIDAR_IP、LIDAR_COMPUTER_IP、LIDAR_GATEWAY）、モータコントローラ識別（MOTOR_SERIAL_DEVICE）、Unitree 固有制御パラメータ（USE_UNITREE、UNITREE_IP、UNITREE_CONN）、GPU ランタイム選択（DOCKER_RUNTIME：runc または nvidia）、そしてシステム全体をマッピングモード（空：ロボットが新規マップを構築する）とローカリゼーションモード（PCD ファイルへのパス：既存マップに対してロボットがローカライズする）の間で切り替える要となる MAP_PATH 変数を網羅する。追加の変数群は、SLAM 解像度、モニタリング間隔、カメラ設定、ネットワークブリッジ構成を制御する。同一の Docker イメージ iserverobotics/nav_autonomy が、サポート対象のすべてのプラットフォーム上で変更なしに動作する。差分は .env のみである。

ROS 2 ディストリビューションのデュアルサポート。本システムは ROS 2 Jazzy（既定、Ubuntu 24.04）と Humble（Ubuntu 22.04）の双方をサポートする。実運用上これが効いてくるのは、現場展開済みのロボット（特に Jetson ベースのシステム）の多くが、ベース OS を容易にアップグレードできないからだ。フォールバック用の run.sh スクリプトは --humble または --jazzy フラグを受け、対応するイメージタグを取得する。既存インフラを再構築することなく、現場側の構成に合わせ込める。

マップライフサイクル管理

SLAM 研究は、正確なマップを構築することに焦点を当てる。しかし本番運用において、マップ構築は問題全体のごく一部に過ぎない。より難しい問いは次のようなものだ。マップはいつ再構築すべきか。複数回の運用で複数のマップを構築した場合、どれを使うべきか。ロボット再起動時に、既存マップへどう再ローカライズするか。ローカライズが完了したら、繰り返し可能なルートをどのように実行するか。

これらの問いに対処するのが、別コンテナの iserve_map である。主ナビゲーションコンテナと並列に動作し、四つの協調するコンポーネントによってマップライフサイクル全体を管理する。

Map builder。map_builder ノードは、オドメトリストリームから設定可能な解像度（既定：ボクセルあたり 0.05 m、最大 500 万点）でボクセル化された 3D 点群マップを構築する。永続ストレージへ 10 秒 間隔で現在マップを自動保存する。さらに重要な性質として、オドメトリ入力に対する空間的重複排除を実装している。入力ポーズは 0.5 m の空間グリッドと 25° のヘディングビンに離散化され、新規グリッドセルに該当するポーズのみがマップ更新に用いられる。これによって、ロボットが長時間滞在した領域（例：ウェイポイント待機点）にマップが支配されることを防ぎ、より空間的にバランスの取れた被覆を実現する。

Map optimizer。複数のマップが存在する場合（異なる運用回、異なる時間帯、異なる環境条件に由来する）、map_optimizer は三つの基準を重み付けしたスコア関数によってそれらを評価・順位付けする。基準は 被覆率（重み: 0.4）、運用領域のうちマップが捉えている範囲を測る、新しさ（重み: 0.3）、環境の現状を反映しているマップを優先する、点数（重み: 0.3）、マップ密度と完全性の代理指標である。本オプティマイザは ROS 2 サービス（/map_optimizer/evaluate_maps）を公開し、指定ディレクトリ内のすべてのマップを採点して順位付き推奨を返す。

Map switcher。map_switcher はオプティマイザの推奨に従って動作する。SLAM 手法間の自動切替に対応し、新規マップ読込時には ICP ベースの再ローカリゼーションを起動する。再ローカリゼーションサービス（/localizer/relocalize）は PCD パスと初期ポーズ推定値を受け取り、iterative closest point マッチングによってロボットの現在スキャンを対象マップに整合させる。これがあるからこそ、再起動後のロボットが手動介入なしに運用を再開できる。推奨されたマップを読み込み、再ローカライズし、制御をナビゲーションパイプラインへ引き渡す。

Pose repeater。ロボットがローカライズされたあとは、pose_repeater が繰り返し可能な自律ルートを実現する。オペレータ指令または自律探索によってロボットがナビゲートする際にゴールポーズを記録し、要求に応じて再生する。再生は設定可能なループ回数に対応する。repeat_count を 0 に設定すれば無限ループとなり、24 時間 365 日のパトロールパターンを実現できる。記録時は /goal_pose を購読し、再生時には同トピックへ発行する。/far_reach_goal_status を監視してウェイポイント到達を確認し、運用後解析のためにオドメトリを 20 Hz で記録する。結果として、オペレータが一度だけパトロール経路を手動運転で示せば、あとはロボットが無期限に繰り返すシステムが成立する。

これら四つを合わせると一つの閉ループになる。ロボットはマップを構築し、評価し、最良のものを選択し、それに対して再ローカライズし、繰り返し可能なミッションを実行する。本層は、経験上、本番ナビゲーション展開におけるエンジニアリング工数の大半を消費するものでありながら、学術系のナビゲーションスタックにはほぼ存在しない部分である。

本番モニタリングとリモート運用

現場のロボット上で動作するナビゲーションシステムは、研究室で動作するそれとは別物である。オペレータは別の建物、ときには別の国にいるかもしれない。システムは自身の健全性を報告でき、オペレータは物理的にマシンへ触れることなく介入できなければならない。

既定のオペレータインターフェースは Foxglove である。WebSocket でナビゲーションコンテナのポート 8765、マップ／運用コンテナのポート 8766 に接続する。RViz を置き換える意図ではなく（RViz は引き続き利用可能で、ローカルデバッグ用途では USE_RVIZ=true で有効化できる）、本番運用が要求するのは、X11 転送やオペレータマシン上の GPU を前提とせず、任意のネットワーク接続で機能するリモートファーストなインターフェースだからこそ、Foxglove が選ばれている。

事前構成済みの Overwatch ダッシュボードは、特定のオペレータモデルを反映した三つの実時間ステータスインジケータを提供する。Autonomy indicator はジョイスティックのモード切替軸（/joy.axes[2]）を読み、自律モードと手動モードのいずれであるかを表示する。Goal Reached indicator は、現在の目標位置にロボットが到達したかを示す。Stop indicator は三つの状態を区別する。OK（通常動作）、Speed Stop（近傍障害物のためにロボットが減速した状態）、Full Stop（ロボットが完全停止した状態）である。これらのインジケータの下には、cmd_vel の並進成分と回転成分を 30 秒窓で表示する速度プロットがあり、ロボットが想定通りに動いているかについて、オペレータに即時のフィードバックを与える。3D パネルには点群マップ、計画経路、自由経路、ナビゲーション境界、FAR Planner が用いる可視性グラフが表示される。オペレータはマップ上をクリックすることで、新しい目標位置を直接発行できる。

iserve_map コンテナはさらに、リソースモニタ を稼働させ、Docker ソケット経由でナビゲーションコンテナの CPU・メモリ使用量を設定可能な間隔（既定：1 秒）でサンプリングし、永続ストレージへ時系列データを書き出す。並列で ログアナライザ が動作し、ナビゲーションコンテナの stdout／stderr を解析し、一定間隔（既定：60 秒）でサマリ JSON ファイルを生成する。これらは意図的に簡素なツールであって、完全なオブザーバビリティスタックを目指したものではない。それでも、現場展開システムの問題切り分けに必要な最小限のテレメトリは提供している。ナビゲーションが失敗した時刻に CPU は張り付いていたか。特定の ROS ノードがクラッシュしたか。ログ上に異常が最初に現れたのはいつか、こうした問いに答える素材を提供する。

このスタックが指し示す方向

本システムの技術的系譜は、地下洞窟探査に遡る。CMU の Autonomous Exploration Development Environment は、DARPA Subterranean Challenge における CMU-OSU チームのナビゲーション基盤であり、GPS の届かない、構造化されていない、視覚的に劣化した環境、たとえばトンネル網、都市地下インフラ、天然洞窟系などをロボットが走破した（Cao et al., Science Robotics 2023）。本チャレンジは、GPS なし、照明なし、不整地、地上との通信が断続的という条件下で頑健にナビゲーションすることを要求するものだった。

これらの条件は、現在 Platform 展開の動機となっている多くの産業検査シナリオと構造的に類似している。夕暮れ時の変電所巡視、Wi-Fi が断続的な倉庫通路、GPS のないトンネル・暗渠検査、照明と通信が復旧する前の災害後現場サーベイなどである。本ランタイムを管理された産業環境において運用可能にしているアーキテクチャ判断は、極限環境への展開、たとえば災害対応、地震後の捜索、煙の充満した産業施設での運用を後日実現するために必要な判断と、同じ系統にある。

nav-autonomy-deploy のような LiDAR ファーストのアーキテクチャは、こうした条件の一部において構造的な優位性を持つ。LiDAR ベースのシステムは、RGB カメラが全く機能しなくなる低照度や一部の視覚劣化環境においても、有用な幾何学的信号を保持できる。ただしこの優位性には限界がある。濃霧、濃い粉塵、霧そのものも LiDAR の戻り光を劣化させ、そうした条件下での実展開には、煙除去アルゴリズム、マルチモーダルセンサ融合、あるいは熱カメラやレーダーといった補完モダリティが通常は必要となる。現状のシステムは煙環境に頑健な知覚、防火耐性、視程劣化条件下での評価を含まない。とはいえ、DARPA SubT の系譜と、コンテナ化・ハードウェア抽象化された展開アプローチを組み合わせれば、ここからの発展は自然な方向となる。管理された環境での本番展開のために行った設計判断（マップライフサイクル管理、リモートモニタリング、ハードウェア抽象化）は、より極限のシナリオに踏み込む前提として、まさに整っているべきものだ。

Platform のハードウェアポートフォリオにおける位置付け

nav-autonomy-deploy は、Contextual Agentic Vision Platform 内部の複数のハードウェア展開経路のうちの一つである。別稿で扱うレンズレスセンサのオプションは、物理エンベロープあるいはネットワーク上の機密性要件によって従来型カメラが採用できないシナリオに対応する。本稿で扱う移動ロボットランタイムは、検査対象が現場全域に分散し、そこへ到達する必要があるシナリオに対応する。いずれも同一の Platform の下に位置する。Platform が判断し、ランタイムが移動する。Platform は何を検査するか、各所見が何を意味するか、それに対して何をするかを判断し、ハードウェア層はその判断を物理空間で可能にする。

新たな展開形態が登場するにつれて（自律ドローン、水中 ROV、エッジ推論アプライアンス）、それらは同じポートフォリオを同じ継ぎ目に沿って拡張していく。継ぎ目とは、Platform の知覚・判断面と、それを作業現場まで運ぶハードウェアとの間の、狭く明示的なインターフェースである。このインターフェースを狭く保つことが、知覚、判断、移動それぞれを独立したエンジニアリングペースで進化させる前提となる。

リポジトリは github.com/iServeRobotics/nav-autonomy-deploy にある。Docker をインストール済みのマシン上で docker compose up -d を実行すれば、ナビゲーションスタックが立ち上がる。ハードウェアに合わせて .env を編集し、ws://<robot-ip>:8765 に Foxglove を接続し、目標ポーズを送信すれば動き出す。コントリビュート、Issue、フォークを歓迎する。

References

W. Xu, Y. Cai, D. He, J. Lin, F. Zhang. "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry." IEEE Transactions on Robotics, 38(4), pp. 2053–2073, 2022.
F. Yang, C. Cao, H. Zhu, J. Oh, J. Zhang. "FAR Planner: Fast, Attemptable Route Planner using Dynamic Visibility Update." IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022. (Best Student Paper Award)
C. Cao, H. Zhu, F. Yang, Y. Xia, H. Choset, J. Oh, J. Zhang. "Autonomous Exploration Development Environment and the Planning Algorithms." IEEE Intl. Conf. on Robotics and Automation (ICRA), Philadelphia, PA, 2022.
C. Cao, H. Zhu, Z. Ren, H. Choset, J. Zhang. "Representation Granularity Enables Time-Efficient Autonomous Exploration in Large, Complex Worlds." Science Robotics, 8(80), 2023.
Ji Zhang. CMU Autonomy Stack for Unitree Go2. github.com/jizhang-cmu/autonomy_stack_go2
DARPA Subterranean Challenge. Final event, Louisville Mega Cavern, September 2021. CMU-OSU Team, "Most Sectors Explored Award."
Autonomous Exploration Development Environment. cmu-exploration.com