nav-autonomy-deploy: LiDARベース自律ナビゲーションのためのオープンソース本番ランタイム

LiDARベースの自律移動ロボットのためのオープンソース本番デプロイシステム nav-autonomy-deploy に関するエンジニアリングノート。CMUのAutonomous Exploration Development Environmentの上に構築されている。

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

CMUの開発環境は、システム開発と実機展開の双方を明示的に意図して設計されている。Go2向けの自律スタックの派生版には、SLAM、経路計画、衝突回避をUnitree Go2のオンボード計算機上で走らせる完全な実機ローンチパスが含まれている（Zhang, autonomy_stack_go2）。しかし、研究用のデプロイワークフローと、異なるロボットプラットフォームをまたいで再現性をもって展開でき、本番環境で管理され、リモートから操作できるシステムとのあいだには、無視できない隔たりがある。クロスハードウェアの抽象化、コンテナ化された環境分離、マップのライフサイクル管理、ランタイム監視、単一コマンドでの起動。この運用レイヤは学術リポジトリには存在せず、しかも決して薄いレイヤではない。

nav-autonomy-deploy は、その運用レイヤに対する1つの試みである。CMUのナビゲーションスタックを、コンテナ化されハードウェア抽象化された運用可能な形にパッケージするオープンソースのデプロイシステムである。単一の docker compose up -d で、対応する任意のロボットプラットフォーム上に自律ナビゲーションが立ち上がる。本ノートでは、そのアーキテクチャと、背後にある主要なエンジニアリング判断を順を追って述べていく。

ナビゲーションスタック

nav-autonomy-deploy の中核を成すナビゲーションパイプラインは、5つのコンポーネントを単一のDockerコンテナ内で動かしている。それぞれが何をしており、どこから来ているのかを理解しておくことは、以降のデプロイエンジニアリングを読むうえで重要な前提となる。

FAST-LIO2。 自己位置推定とマッピングを担う。特徴量ベースのSLAM手法とは異なり、FAST-LIO2は密結合の反復カルマンフィルタを用いて、生のLiDAR点を直接マップにレジストレーションする。これにより、LiDARハードウェアを切り替えた際に多くのSLAMシステムを脆弱にしている手作りの特徴抽出ステップを排している。マップはインクリメンタルなk-d木（ikd-Tree）として保持され、点群のリアルタイム挿入・削除が可能である。その結果、IntelおよびARMプロセッサ上で最大 100 Hz で動作し、急峻な運動や雑然とした環境を扱え、スピン式とソリッドステートのLiDAR双方で再設定なしに動作するSLAMシステムが得られる（Xu et al., IEEE TRO 2022）。本スタックでは、CMUのGo2自律スタックのデフォルトであるPoint-LIOをFAST-LIO2に差し替えている。

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

ローカルプランナー は、CMUのbase autonomyシステム由来であり、リアルタイムの衝突回避を担う。事前にモーションプリミティブのライブラリを生成しておき、実行時に障害物によってオクルージョンを受けるものを除外して、衝突のない経路を選択する。入力としては、地形解析 モジュールが3D点群から局所地形を走行可能領域と走行不可領域に分類した走行可能性マップを利用する。ウェイポイントフォロワ がグローバル層とローカル層を接続し、FAR Plannerのウェイポイントを閉ループの制御の中でローカルプランナーに供給する。

生のLiDAR入力から速度指令に至るパイプライン全体は、ROS 2ワークスペース内の単一の実行スレッド上で動作する。CMUの報告によれば、自律スタック全体のCPU使用率は 10%未満 であり、ローカル経路計画は1サイクルあたり 1 ms未満、CPU使用率にして 1%未満 に収まっている（cmu-exploration.com）。これは重要な数字である。すなわち、ナビゲーションアルゴリズムは計算上のボトルネックではない。後述するように、ボトルネックはそれ以外の部分にある。

コンテナ化されたデプロイ

ROS 2のナビゲーションスタックをコンテナ化することは、Webアプリケーションをコンテナ化することとは似て非なる作業である。Dockerが暗黙のうちに前提とする事項（独立したネットワーク、ステートレスなプロセス、揮発的なファイルシステム）は、物理ハードウェアにアクセスし、マルチキャストでピアを発見し、起動をまたいでマップを永続化する必要があるリアルタイムロボティクスシステムのほとんどあらゆる要件と衝突する。本システムのDocker構成における各設計判断は、こうした衝突のいずれかを解消するために存在している。

ネットワークモード。 ROS 2のデフォルトミドルウェアであるFast DDSは、ノード発見にマルチキャストUDPを用いる。Dockerのブリッジネットワークは、ホストとコンテナのネットワーク間でマルチキャストを転送しない。本システムでは network_mode: host を採用し、コンテナにホストのネットワークインターフェースへの直接アクセスを与える。これは意図的なトレードオフであり、ネットワークの分離を犠牲にする代わりに、実際に機能するDDS発見を得ている。複数台ロボット構成においては、これに替えて ROS_DOMAIN_ID（デフォルト: 42）でROS 2の通信ドメインを分割する方針を取る。

共有メモリ。 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: メカナム駆動、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の2つのディストリビューションサポート。 本システムはROS 2 Jazzy（デフォルト、Ubuntu 24.04）とHumble（Ubuntu 22.04）の双方をサポートする。これは実用上重要であり、デプロイ済みの多くのロボット（とくにJetsonベースのシステム）は、ベースOSを容易にアップグレードできないためである。フォールバックスクリプト run.sh は --humble または --jazzy フラグを受け付け、対応するイメージタグをプルする。これにより、各チームは既存のインフラに合わせる形で、再ビルドを伴わずに本システムを取り入れることができる。

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

SLAM研究の関心は、精度の高いマップを構築することに集中する。しかし本番運用において、マップの構築は問題全体のごく一部にすぎない。より難しい問いはこうである。マップはいつ再構築すべきか。複数回の走行を経てロボットが複数のマップを構築している場合、どれを使うべきか。ロボットが再起動した際、既存マップに対してどのように再ローカライゼーションを行うか。そして、いったん自己位置が確立したとき、どのようにして再現可能なルートを実行するのか。

これらの問いには、独立したコンテナ iserve_map が応えている。これは主たるナビゲーションコンテナと並行して動作し、4つの協調するコンポーネントによってマップのライフサイクル全体を管理する。

マップビルダ。 map_builder ノードは、オドメトリストリームから設定可能な解像度でボクセル化された3D点群マップを構築する（デフォルト: ボクセルあたり 0.05 m、最大 500万点）。現在のマップを 10秒 ごとに永続ストレージへと自動保存する。さらに、オドメトリ入力に対する空間的な重複排除を実装しており、これがマップ品質にとって本質的な部分である。入力ポーズを 0.5 m の空間グリッドと 25° の方位ビンに離散化し、新しいグリッドセルに該当するポーズのみをマップ更新に用いる。これにより、ロボットが長く滞在した領域（たとえばウェイポイントでの待機地点）にマップが支配されるのを防ぎ、空間的によりバランスのとれたカバレッジを得ることができる。

マップオプティマイザ。 異なる走行、異なる時間帯、異なる環境条件から複数のマップが得られている場合、map_optimizer は3つの基準にわたる重み付きスコア関数によってそれらを評価しランク付けする。すなわち、運用領域のうちマップが捉えている範囲を測る カバレッジ（重み: 0.4）、現在の環境状態を反映するマップを優先する 新しさ（重み: 0.3）、そしてマップの密度と完全性の代理指標としての点数（重み: 0.3）である。オプティマイザは、指定されたディレクトリ内のすべてのマップを採点し、ランク付けされた推奨を返すROS 2サービス（/map_optimizer/evaluate_maps）を提供する。

マップスイッチャ。 map_switcher は、オプティマイザの推奨に基づいて動作する。SLAM手法間の自動切替をサポートし、新しいマップをロードする際にICPベースの再ローカライゼーションをトリガする。再ローカライゼーションのサービス（/localizer/relocalize）はPCDのパスと初期姿勢の推定値を受け取り、iterative closest pointマッチングを用いてロボットの現在のスキャンを対象マップに位置合わせする。これは、ロボットが再起動後にマニュアル介入なしで運用を再開することを可能にするコンポーネントである。推奨マップをロードし、再ローカライゼーションを行い、制御をナビゲーションパイプラインへと引き渡す。

ポーズリピータ。 ロボットの自己位置が確立したのちは、pose_repeater が再現可能な自律ルートの実行を担う。ナビゲーション中（オペレータ指令あるいは自律探索のいずれによっても）にゴールポーズを記録し、要求に応じて再生する。再生はループ回数を設定可能であり、repeat_count が0であれば無限ループとして、24時間体制の巡回パターンを実現できる。記録のために /goal_pose を購読し、再生時には同じトピックに発行する。ウェイポイント到達の確認のために /far_reach_goal_status を監視し、走行後の解析のためにオドメトリを 20 Hz でログに残す。結果として、オペレータが巡回ルートを一度だけ手動で走行させれば、あとはロボットがそれを無期限に繰り返す、というシステムが得られる。

これら4つのコンポーネントは合わせて閉ループを形成する。ロボットはマップを構築し、評価し、最良のものを選び、それに対して再ローカライゼーションを行い、再現可能なミッションを実行する。経験上、本番ナビゲーション展開におけるエンジニアリング工数の大半を消費するレイヤであり、学術的なナビゲーションスタックにはほとんど存在しない部分である。

本番監視とリモート操作

現場のロボット上で動くナビゲーションシステムは、ラボで動くものとは同一視できない。オペレータは別の建物、あるいは別の国にいることもある。システムは自らの健全性をレポートしなければならず、オペレータは現場に物理的にアクセスせずとも介入できなければならない。

デフォルトのオペレータインターフェースは Foxglove であり、WebSocket経由でナビゲーションコンテナのポート8765、マップ／運用コンテナのポート8766に接続する。RVizを置き換えるためにFoxgloveを選んだわけではなく（RVizは引き続き利用可能であり、ローカルデバッグのために USE_RVIZ=true で有効化できる）、本番運用では、X11フォワーディングやオペレータ側マシン上のGPUアクセスを必要とせずに任意のネットワーク接続で動作するリモートファーストのインターフェースが要求されるためである。

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

iserve_map コンテナは加えて リソースモニタ を動かしており、設定可能な間隔（デフォルト: 1秒）でDockerソケット経由にナビゲーションコンテナのCPU・メモリ使用率をサンプリングし、時系列データを永続ストレージへ書き出す。並走する ログアナライザ は、ナビゲーションコンテナの標準出力・標準エラー出力をパースし、一定間隔（デフォルト: 60秒）でサマリJSONを生成する。これらは意図的にシンプルなツールであり（完全な可観測性スタックではない）、デプロイされたシステムにおける問題診断のための最小限のテレメトリを提供する。すなわち、ナビゲーションが失敗したときにCPUが張り付いていたか、特定のROSノードがクラッシュしたか、ログ上で異常はいつ最初に現れたか、といった問いに答えるための材料である。

このスタックが示唆する方向

本システムの技術的系譜は、地下洞窟探査にまで遡る。CMUのAutonomous Exploration Development Environmentは、DARPA Subterranean ChallengeにおけるCMU-OSUチームのナビゲーションの背骨を成し、ロボットたちはGPS不可で、構造化されておらず、視覚的に劣化した環境（トンネル網、都市の地下インフラ、自然洞窟系など）をナビゲートした（Cao et al., Science Robotics 2023）。同チャレンジが要求したのは、GPSなしで、光が乏しく、地形が不均一で、地上との通信が断続的にしか確保できない条件下での頑健なナビゲーションであった。

LiDARを軸としたアーキテクチャは、こうした条件の一部において構造的な優位性を持つ。LiDARベースのシステムは、RGBカメラが完全に機能しなくなる低照度や一部の視覚的に劣化した環境においても、有用な幾何情報を保持できる。ただし、この優位性には限界がある。濃い煙、激しい粉塵、霧もまたLiDARの戻り光を劣化させ、こうした条件下での実際の展開には、煙除去アルゴリズム、マルチモーダルなセンサ融合、あるいは熱画像カメラやレーダといった相補的なモダリティが通常は必要となる。現行のシステムは、煙に頑健な知覚、耐火構成、視認性劣化下の知覚に対する評価のいずれも備えていない。しかしDARPA SubTの系譜と、コンテナ化されハードウェア抽象化されたデプロイ手法を組み合わせれば、本システムは今後の自然な発展方向として、そちらへの拡張を視野に入れることができる。制御された環境での本番展開のために行ったアーキテクチャ上の判断（マップのライフサイクル管理、リモート監視、ハードウェア抽象化）は、より極端なシナリオ（災害対応、地震後の捜索、煙が立ち込めた産業施設）に挑む前に同じく整えておく必要がある判断と一致している。

リポジトリと今後

リポジトリは github.com/iServeRobotics/nav-autonomy-deploy にある。Dockerがインストールされたマシン上で docker compose up -d を実行すれば、ナビゲーションスタックが起動する。.env をハードウェアに合わせて編集し、Foxgloveを ws://<robot-ip>:8765 に接続し、ゴールポーズを送る、というだけである。コントリビューション、イシュー、フォークを歓迎する。

屋外自律のより難しい問題（視認性劣化下の知覚、マルチモーダルなセンサ融合、構造化されていない現場での長時間運用）は、本レイヤの内側ではなく、その上に位置する。ここで記したエンジニアリングのレイヤ（コンテナ化、マップのライフサイクル、リモート操作、監視）は、こうした研究上の問いが実機のフリート上で扱える形になる前に、ほぼ前提として整えておかなければならない事項である。

参考文献

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