G1が歩きながら地図を作る。Isaac SimでVisual SLAMと再定位を検証

前回はG1の歩行のカタカタを改善しました。今回は、その歩行にVisual SLAMを接続し、歩きながら床と壁の地図を作りました。保存した地図を使い、別の走行画像から位置を特定するところまで検証しています。

約88秒歩いて、地図が広がる過程を見る

棚の手前の通路で、直進・左折・戻りを含む12目標を指定しました。2,188画像を処理し、追跡欠落0で全目標を通過。地図は557,610点まで増えました。今回の長時間走行の水平位置RMSEは10.52 cm、最後の停止位置誤差は10.69 cmです。87.54秒のシミュレーションに実処理702.11秒かかっています。

この経路はあらかじめ指定しています。地図からの自動経路生成や障害物回避ではありません。以下は、これに先立つ約20秒の走行と、別走行の記録による再定位の検証です。地図の点数や誤差は試験ごとに分けて記載しています。

初回の地図生成と、別走行での再定位

軌跡だけでは、地図を作った様子が伝わらなかった

最初の動画には、画像上の特徴点と移動軌跡を表示していました。しかし、それだけでは棚や壁がどこにあるのか分かりません。自己位置推定に使う特徴の地図と、人が空間の形を読み取れる地図を分けて考え、後者を追加しました。

左右のカメラ画像のずれから奥行きを計算し、cuVSLAMが推定した位置と姿勢で点を重ねています。今回見えている主な構造は床と右側の壁です。シミュレータの正解地形や正解位置を地図生成へ渡したものではありません。

ステレオ画像から、約12.6万点を蓄積

左右カメラは848×480画素、基線50 mm、25 Hz。OpenCVのStereoSGBMで視差を求め、5 Hzで表面地図を更新しました。左右の対応が合わない点を除き、0.4〜10 mの範囲を採用しています。

点は5 cmの格子に分け、3回以上の更新で観測されたものを残しました。最終的に125,935点を保存できました。ただし、5 cmは格子の幅であり、地形の精度が5 cmという意味ではありません。復元した表面と正解形状の距離評価はまだです。

位置誤差は初期カメラ姿勢だけで座標を合わせ、シミュレータの真値と比較しました。軌跡全体へのフィッティングや倍率補正はしていません。20.74秒のシミュレーションに138.85秒かかっており、動画はシミュレーション時間に対する等倍速です。実時間で処理できたという結果ではありません。

保存地図で、別走行の位置を特定する

地図を作った走行とは別の記録を、別プロセスで先頭から追跡しました。その画像から計算した位置を初期予測に使い、8・10・12・14・16秒の5地点で保存地図と照合しました。

5地点すべてで照合に成功し、水平位置RMSEは5.09 cm。初期予測をさらに横方向へ30 cmずらした条件でも5地点すべて成功し、RMSEは5.08 cmでした。真値は照合が終わった後の評価にのみ使用しています。

照合に使うのはcuVSLAMが保存した視覚特徴の地図です。動画で見える密な点群そのものを照合しているわけではありません。また、同じ開始領域からの走行で、先行画像の処理と近い位置予測がある条件です。5地点は同じ走行内のサンプルであり、5回の独立走行試験ではありません。

できたことと、まだできていないこと

歩行中に地図を生成し、地図上で現在位置を表示し、保存地図を別走行の画像で再利用できました。動画の全フレーム再生、点群ファイルの読み戻し、既知の視差から距離を復元するテストも確認しています。

一方、再定位は記録再生での検証です。保存地図を読み込んで新しい走行を制御するところや、通行可能な領域の判定、障害物回避は未実装です。今回の目標点と通路は既知で、工場内の任意の場所へ自律移動する完成版ではありません。実機での検証もこれからです。

今回分かったのは、「自己位置が出る」「空間の形が見える」「保存地図を使える」はそれぞれ確かめる必要がある、ということです。次はこの地図から通れる領域を扱い、地図を使った移動へつなげていきます。

実行環境・参照資料

Isaac Sim 5.1.0、IsaacLab 2.3.0、cuVSLAM v17。歩行は前回と同じUnitreeの対応モデルと公開ポリシーを使用しています。本記事の数値・動画は、チームゼットによる2026年9月6日の実装・試験結果です。

NVIDIA公式：PyCuVSLAM API / OpenCV公式：StereoSGBM / Unitree公式：使用したモデル・ポリシーの版