前回はG1の歩行のカタカタを改善しました。今回は、その歩行にVisual SLAMを接続しました。棚と積み荷の間を歩き、カメラ画像から色付きの地図を作る過程を紹介します。先行する別の短時間試験では、保存地図と別走行の画像を照合する再定位も検証しています。

棚の間を約70秒歩き、地図の形を見比べる

70.28秒・25 fps。シミュレーション時間に対する等倍速で、途中のカットや静止画への差替えはありません。左上が歩行、左下がカメラ画像。右上は実画像の色を載せた表面点群、右下は高さで色分けした上面図です。白点が推定位置、緑線が軌跡。冒頭約5秒は初期化、最後約3秒は停止確認です。

長く歩くだけでは、何が地図になったのか分からなかった

先に棚の手前を約88秒歩かせたところ、点群は増えましたが、主に床と壁が映る経路では物の形を読み取りにくいままでした。そこで開始位置を棚の間へ移し、棚沿いに進んで奥を曲がり、隣の通路を戻る経路に変更しました。

高さだけの色分けも見直しました。左右画像の視差から距離を求めた点へ、実際の左カメラ画像の色を載せています。オレンジ色の棚の梁、茶色い箱、奥の白い壁を映像と地図で見比べられます。上段の表示範囲は観測点群に合わせて調整します。シミュレータの正解形状や正解位置を地図生成に使ってはいません。

今回の走行は7目標すべてに到達し、追跡欠落0 / 1,757画像。表面点群は681,630点、水平位置RMSEは6.29 cm、最後の停止位置誤差は18.76 cmでした。位置誤差はシミュレータの真値との比較であり、棚や箱の形状精度を測った値ではありません。

70.30秒のシミュレーションに実処理994.64秒かかっています。実時間性能の検証ではありません。経路はあらかじめ指定しており、地図からの自動経路生成や障害物回避も未実装です。今回の地図での再定位は追加試験していません。以下の再定位結果は、先行する約20秒の別試験です。

約31秒。前半はIsaac Simで歩行と地図生成を同時に実行した映像。後半は、別走行の連続映像に、2秒ごと・5地点の再定位結果を重ねています。右上が立体図、右下が上面図。前半は白点が推定位置、緑線が軌跡。後半はオレンジが記録済みの視覚推定、緑の丸が保存地図との照合位置です。

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

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

左右カメラは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日の実装・試験結果です。