前回は、G1が歩きながら色付きの地図を作るところまで進めました。今回は、その先の「地図を使って目的地へ歩く」を実装しました。途中で止まる原因を調べ、追加観測で地図を補った結果、Nav2が作った経路で棚を回り込み、目標付近で停止できました。

50.08秒・25 fps。シミュレーション時間の等倍速、途中のカットなし。左がG1とカメラの特徴点、右の黄色がNav2の経路、青が画像から推定した現在位置、紫が目標です。白は観測済みの空き領域、黒は障害物、灰色は未確認の領域。冒頭約4秒は再定位待ち、最後約3秒は停止確認です。

形が見える点群を、そのまま通行地図にはできなかった

ステレオ画像の視差から作った点群では、棚や壁の形は見えても、通れる床が十分につながりませんでした。点がない場所を空いていると扱うと、まだ見ていない場所まで通行可能にしてしまいます。

そこで今回は、自己位置はステレオcuVSLAM、床・障害物の地図はシミュレータの理想的な深度カメラという構成にしました。深度をステレオ画像から推定した成果と混同しないように分けています。倉庫モデルの正解形状や正解位置を、地図生成・走行制御へ渡してはいません。

目標到達だけで合格にすると、未知領域を横切っていた

最初の回り込みは目標に到達しました。しかし事後評価で、軌跡の一部が地図の未知領域を横切っていると分かりました。この走行は不合格とし、経路生成だけでなく追従側でも未知領域を禁止しました。次の試験では、今度はNav2の前方衝突予測で目標手前に停止しました。

地図を調べると、通路の一部で床と視線の観測が足りていませんでした。禁止領域を消したり、空き判定を緩めたりせず、追加走行で得た深度を統合。同じ保存地図への画像照合に成功した記録だけを使い、合計769フレームから地図を作り直しました。

地図は10cm格子で、観測済みの空き領域は約188.83m²。床と視線をそれぞれ複数フレームで確認し、障害物の観測がないセルを空きとしています。高さの基準も各フレームの床から求め、姿勢の揺れで床を見落とす問題を抑えました。

地図作成時より2m先の位置から開始し、近傍の初期位置予測を与えて保存地図へ画像照合しました。成功するまで移動を待ち、その後は画像からの位置推定を使ってNav2へ現在位置を渡します。経路生成はNavfn、追従はRegulated Pure Pursuitです。

当初と同じ目標への最終走行で、棚を回り込み、到達と停止を確認しました。到達判定の瞬間と、減速して止まった後の誤差は分けて記録しています。

正解位置は事後評価だけに使い、地図作成時の初期カメラ姿勢で座標を合わせています。軌跡全体へのフィッティングはありません。機体の衝突形状を測り、その全身包絡が観測済みの空き領域内に収まることも確認しました。動画は1,252フレームを全復号し、映像の途中停止も検査しています。

確認できたのは、静的な倉庫の指定開始領域から指定目標までの走行です。倉庫全域の探索が終わったわけではなく、灰色の未観測領域は残っています。近い初期位置予測を使っているため、任意の場所からの大域再定位でもありません。

実機、動く障害物への経路再計画、複数試行の成功率、実時間性能は未検証です。今回は理想化した深度を使っており、実機カメラの欠測やノイズへの対応は別途必要です。まず「地図が見える」と「機体が通れる」を分けて評価することで、次に確かめる条件を明確にできました。

環境：Isaac Sim 5.1.0 / Isaac Lab 2.3.0 / ROS 2 Humble / cuVSLAM 17 / Unitree G1の対応29関節モデルと公開歩行ポリシー。数値はチームゼットの2026年9月6日の実装・試験記録です。担当した実装と成果の概要はこちら。