Classmethod Forum用にIsaac Simのデモを作ってみた

はじめに

先日開催されたClassmethod Forum用に、NVIDIAのIsaac Simを使ってUnitreeのGO2がオフィスを巡回しながら人や障害物を避け、不審者を発見した場合はアラートを出すデモを作りました。

そのデモについて、概要と各機能の解説をする記事です。

デモの内容

デモの内容は以下のようなものです。

• オフィスを模したステージで、Go2が25個のチェックポイントを順番に巡回する
• 経路上に人や障害物を発見すると検知して迂回ルートを計算し避けて通る
• カメラに「黄色のベストを着た人 (不審者役)」が映るとアラートを出す
• 別ウィンドウのダッシュボードで、ロボットの現在位置・走行軌跡・カメラ映像・検知状態をリアルタイムで表示する

主な機能を整理しますと

• 歩行
• 経路計画
• 動的障害物回避
• 人物検知
• 不審者識別
• リアルタイム可視化

の6つです。

デモを一部分を録画したので載せておきます。
https://youtu.be/u8uKaaUQF9A?si=M8OIJT86Z_x36VdT

各機能がどう実現されているかを、次のセクションから順に説明します。

ステージ

Isaac Sim のデフォルトマップではなく、scene_objects.pyというファイルを自前で書いてカスタムステージを構築しています。

• 壁はUSDの直方体を並べて作る
• 人物や家具はNVIDIAが提供しているNucleusというアセットストアからUSDファイルを参照して配置

オフィスの形・人の配置・チェックポイントの座標はすべて自分で設計したものです。

歩行

基本的な歩行についてはIsaac Labが用意している学習済みの歩行ポリシーをそのまま使っています。こちらが自前で書いているのは速度指示を出す部分だけです。

GO2は速度指示 (前後速度・横速度・旋回速度の3つ) を受け取って、各関節をどう動かすかを自分で判断して歩いてくれます。入力として「ロボットの現在の姿勢」「脚の角度」「与えられた速度コマンド」などを受け取り、出力として「各関節の目標角度」を返します。Isaac Sim の物理エンジンがその関節角度をトルクに変換して、実際にシミュレーション上で脚が動きます。

速度指示の受け渡し方

Isaac LabにはObservationsCfgという設定クラスがあり、ここに「ロボットの速度」「脚の角度」「速度コマンド」など何を観測値として使うかを定義しておくと、実行時にIsaac Labが自動的にロボットの状態を集めてポリシーの入力に並べてくれます。
こちらで実装したTwoDimPreTrainedPolicyActionというクラスでは、後述の経路追従アルゴリズムが出した「前後速度」と「旋回速度」を、歩行ポリシーが期待する形式に変換してセットしています。横速度はここでゼロに固定しています。横速度を残すと「斜め前のゴールに対して横歩きで近づく」のが効率的な動きとして出てきてしまうため、構造的に潰しています。

経路計画

「どのルートを通るか」を計算するアルゴリズムが必要です。今回はA* (A-star) というアルゴリズムをPythonの標準ライブラリ(heapq)で自前実装しました。

A*とは

マス目状のマップ上で、スタートからゴールまで障害物を避けた最短ルートを探すアルゴリズムです。各マスに「スタートからここまでにかかったコスト+ここからゴールまでの推定コスト」を計算して、コストが小さい順に探索していきます。すべてのマスを総当たりで調べる必要がないので効率的です。

マップとチェックポイント

マップとチェックポイントはYAMLファイルに書いています。

config/walls.yamlにオフィスの壁の座標を書いておきます。

config/waypoints.yamlに巡回したいチェックポイントの座標を25個並べて書いておきます。

waypoints:
  - [3.0, 2.0]   # 0番: spawn地点
  - [3.0, 7.5]   # 1番: Room Aの入口付近
  - [3.0, 5.5]   # 2番
  ...
  - [3.0, 2.0]   # 24番: 最初に戻る(ループ)

これはIsaac SimやIsaac Labの標準フォーマットではなく、独自に定義したただのYAMLです。yaml.safe_load() で読み込んでPythonのリストにします。

起動時に以下の処理が走ります。

1. walls.yamlを読んで、20cmのマス目のマップ (OccupancyMap)を構築する。壁はロボットの体幅分膨らませて通れる領域を計算
2. waypoints.yamlの25個のチェックポイントを読む
3. 各チェックポイント間にA*を走らせて、壁を避けたルートを計算する
4. ルートを0.7m間隔の細かい点列(169 個)に変換して持っておく

この169個の点列をPythonのリストとして保持して、以降ロボットがその順番に追いかけていきます。

経路追従

A*で作った点列を実際に追いかけるのがPure Pursuitという制御手法です。これもIsaac Labにライブラリはないので自前実装しました。

Pure Pursuitとは

「経路上の一定距離先の点を常に追いかける」古典的な制御手法です。

仕組みは単純で、「現在地から経路上の（今回の場合）1m先の点がどの方向にあるか」と「今ロボットがどちらを向いているか」のズレを毎ステップ計算して、そのズレを埋めるように旋回量と速度を出します。ズレが小さい (正面向き) ほど速く前進、ズレが大きい (横向き) ほど旋回を優先してゆっくり前進、という調整をします。追いかける点が経路に沿って動くので、ロボットも経路に沿ってなめらかに走ります。

実装の流れ

普通のPythonのwhileループで、毎ステップ以下のフローを繰り返しています。

while シミュレーションが動いている:
    get_robot_xy()         # Isaac LabのAPIで現在位置を取得
    get_robot_yaw()        # 現在の向きを取得
    advance_wp_idx()       # 通り過ぎたら次のチェックポイントへ
    lookahead_target()     # 経路上の1m先の点を計算
    pure_pursuit_command() # 速度と旋回量を計算
    env.step(actions)      # 速度指示を歩行ポリシーに渡して1ステップ進める

「今何番目のチェックポイントに向かっているか」はcurrent_wp_idxという整数で管理しています。「現在のチェックポイント → 次のチェックポイント」のベクトルと「現在のチェックポイント → ロボット位置」のベクトルの内積を計算して、半分以上進んでいたら通り過ぎたと判定して番号を1進めます。

このようにして、169個の点列を順番に追いかけ続けることで巡回を実現しています。

動的障害物回避

事前に決めた経路の上に人や物体がいる場合、それを検知して避ける必要があります。今回はLidar(レーザー距離センサー)を使って検知し、A*で迂回ルートを再計算する仕組みにしました。

Lidarの設定

Isaac Labのisaaclab.sensorsモジュールに含まれる MultiMeshRayCasterCfgというクラスを使ってカスタムのLidarを作っています。SDKではなくPythonクラスとして環境設定に追加する形です。

GO2の実機についているLidarとまったく同じものはIsaac Labに用意されていないので、前方に32本のレーザーを飛ばして各方向の距離を計測する擬似Lidarとして実装しました。

障害物の判定

障害物検知の仕組みは、事前にもっている静的マップとの差分を見る方式です。

1. walls.yamlから作った静的マップを使って、「壁だけある場合に各レーザーが何メートルで当たるか」の理論値を事前計算しておく
2. 毎ステップLidarの実測値を取得する
3. 理論値より明らかに近くで当たっているレーザーがあれば、「地図にないもの=障害物がいる」と判定する

マップがあることが前提の仕組みです。実機でこのやり方をするには、歩きながら自分の位置と地図を同時に作る技術 (SLAM) が必要になります。

マップなしで障害物を検知する方法もあり、「前のフレームとのLidarの差分を見る」「深度カメラで一定距離以下に何かが入ったら止まる」「強化学習で障害物回避を端から端まで学習させる (end-to-end)」など、いろいろなアプローチがあります。

迂回ルートの計算

障害物を検知したら以下の流れで経路を引き直します。

1. 検知した障害物を中心に、半径1.1mのマスを「通行不可」にした一時マップを作る
2. 「現在位置」から「6〜10 個先のチェックポイント」をゴール候補として、この一時マップ上でA*を走らせて迂回ルートを計算する
3. 元の点列の前半 + 迂回ルート + 元の点列の後半をつなげた新しい点列を作る
4. 元の点列はbackup_sub_wpsという変数に保存しておく
5. 新しい点列に差し替えてPure Pursuitで追従させる
6. 迂回完了(ゴールに到達)したら、保存しておいた元の点列に戻す

この計算もすべてPythonで書いたA*のアルゴリズムが行っています。

不審者検知

カメラ映像から人を検出し、特定の見た目(今回は黄色のベストを着用している人) の人を不審者と判定します。

YOLO による人物検出

物体検出にはYOLO (You Only Look Once)というアルゴリズムを使っています。これは画像から物体の位置と種類を一度に推論するモデルです。

実装としてはYOLOのモデルをPythonから扱えるようにした ultralytics というライブラリが公開されているので、それを使っています。学習済みモデルもこのライブラリ経由で読み込めます。

カメラの設定

GO2のモデルにはデフォルトでカメラが付いていないので、Isaac LabのCameraCfgという汎用センサー設定クラスを使って、ロボットの前面にRGBカメラを自分で取り付けました。CameraCfgはGO2専用ではなく、任意のロボットの任意の位置に取り付けられる汎用クラスです。解像度などの仕様もこちらで設定したもので、GO2実機のカメラと厳密に同じではありません。

カメラ映像はIsaac LabのPython API(scene["front_cam"].data.output["rgb"])で取得します。これはGPU上の多次元配列として返ってくるので、YOLOに渡す前にnumpy配列に変換します。

検知の流れ

1. カメラ映像をYOLOに入力し、「人」を検出する
2. YOLOは検出した物体をbbox(bounding box、長方形の枠)で返す
3. bboxの周辺画像をOpenCVのcv2.cvtColor()でRGBからHSV(色相・彩度・明度)に変換する
4. H・S・Vそれぞれに事前定義した数値範囲(黄色)に収まるピクセルを数えて比率を計算する
5. 1.5%を超えたら「ベストを着ている」と判定してアラートを出す

なぜRGBではなくHSVを使用したか

RGBのままでも色判定はできます。ただし照明が変わると同じ色でもRGBの値が変わってしまうため、精度が落ちます。HSVは色相(どの色か)・彩度(鮮やかさ)・明度(明るさ)を独立して扱えるので、照明変化に強くなります。

ダッシュボード

別ウィンドウのリアルタイムダッシュボードはPythonのmatplotlibというライブラリで作っています。matplotlibは本来グラフや図を描くための汎用ライブラリですが、FuncAnimationという機能を使って100msごとに画面を更新するリアルタイム表示として使っています。

シミュレーションとの通信

シミュレーションとダッシュボードは別々のプロセスとして動いていて、ファイルを介して通信しています。特別なライブラリは使わずに自前で実装しています。

• シミュレーション側が毎ステップ/tmp/nav_state.jsonにロボットの位置・向き・状態・アラートフラグをJSONテキストで書き込む
• ダッシュボード側が100msごとにそのファイルを読んで画面を更新する

本格的にやるならROS2のトピックやZeroMQのようなIPCライブラリを使うところですが、今回はファイル経由で十分でした。

ダッシュボードの表示内容

• 左側: オフィスの俯瞰図。ロボットの現在位置、走行軌跡 (通常巡回が青、回避中がオレンジ)、壁・家具・人の配置
• 右上: ロボットのカメラ映像。YOLO が検出した人にオレンジの bbox 枠が表示される
• 右下: 現在の状態 (巡回中・回避中・停止)、不審者検知のアラート

まとめ

このデモは「役割を分けて組み合わせる」設計にしています。

• 歩行はIsaac Labの学習済みポリシー (NN モデル)
• 経路計画はA* (古典アルゴリズム、自前実装)
• 経路追従はPure Pursuit (古典制御、自前実装)
• 動的障害物検知はLidar + 静的マップとの差分比較 (自前実装)
• 動的回避はA* で迂回ルート再計算 (自前実装)
• 人物検知はYOLO(オープンソースのモデル)
• 不審者判定はHSV色判定 (自前実装)
• ダッシュボードはmatplotlib + ファイル経由通信(自前実装)

学習で動かす部分と古典アルゴリズムで動かす部分を意図的に分けています。歩行のような物理的に複雑な動きは学習に任せる方が筋がよく、経路計画や追従のような「決定論的に動いてほしい部分」は古典アルゴリズムの方が安定します。

Isaac Labには学習済みの歩行ポリシーや各種センサーの設定クラスなど、ロボット開発に必要な部品が一通り揃っています。そこに自分で書いた上位のロジック(経路計画・追従・障害物検知・不審者判定・UI)を組み合わせることで、こうした統合的なデモを比較的短期間で作ることができました。（ゼロベースで学習させることも試しましたが、フォーラムまでのタイムリミットがあったので断念しました。）

というわけで、フォーラムの展示の内容のご紹介でした。