https://arxiv.org/pdf/2403.16967

 

日本語訳

---

Visual Whole-Body Control for Legged Loco-Manipulation

概要: 本研究では、腕を搭載した脚式ロボットを用いたモバイルマニピュレーション、すなわち「レッグド・ロコマニピュレーション」を扱う。ロボットの脚は通常、移動のために使われるが、全身制御で腕を協調させることで、マニピュレーション能力を強化する可能性がある。本研究では、視覚観測を用いて全身制御を自律的に行えるフレームワークを提案する。提案手法「Visual Whole-Body Control (VBC)」は、エンドエフェクタ位置に加え、ロボット本体の速度指令を追従する全自由度の低レベル制御ポリシーと、視覚入力に基づいてこれらの指令値（本体速度やエンドエフェクタ位置）を決定する高レベルのタスクプランニング・ポリシーから構成される。すべてのポリシーはシミュレーションで学習し、Sim2Real（シミュレーションから実機への転移）によって実ロボットに適用している。多様なオブジェクトや環境での大規模な実験により、様々な高さ・位置・方向のオブジェクトを把持するタスクにおいて、提案手法が他の手法に比べ優位性を持つことが示された。

キーワード: ロボット学習、全身制御、レッグド・ロコマニピュレーション

---

1 はじめに

モバイルマニピュレーションの研究は、より優れたマニピュレーション制御やナビゲーションシステムの進歩とともに大きく進展してきた。脚ではなく車輪を付けたマニピュレータを用いれば、多くの家庭内タスクを解決できることが知られている [1, 2] が、こうしたロボットを荒れた地形で屋外に適用するのは非常に困難である。例えば、キャンプ場でロボットがゴミや薪を拾ってくれたら非常に便利だろう。こうした多様性や屋外での応用を目指し、本研究では「脚式ロボットに腕を搭載したシステムによるモバイルマニピュレーション」すなわち、レッグド・ロコマニピュレーションを取り上げる。特に、ロボットがエゴセントリックカメラからの視覚情報のみを頼りに、自律的にタスクを実行できるようにすることを目的としている。

レッグド・ロコマニピュレーションでは、ロボットが有する全自由度（例：本研究では19自由度）を活用した全身制御が可能である点が重要である。脚は本来、移動のために使われるが、脚と腕を同時に制御することで、マニピュレーション能力や作業空間を大幅に拡張できる可能性がある。我々人間も同様に、棚の上の本を取るために背伸びをしたり、靴紐を結ぶために身体を曲げるなど、脚と腕を協調させている。図1が示すように、ロボットがこのような全身制御を用いて道路脇でゴミを拾ったり、草の上に落ちたものを拾ったりといった動作が可能になる。もし脚の協調がなければ、アーム単体の稼働範囲では地面に落ちたものを拾うことは困難となる。

しかし、このような全身制御は非常に難しい課題である。足先や物体との接触を利用しつつ、外乱に対して安定性と堅牢性を同時に確保する必要があるほか、モバイルプラットフォームとして精度の高いマニピュレーションを行う必要がある。脚式ロボットの場合、車輪付きロボットに比べて本体を安定させることがはるかに難しく、さらに、これらすべてを多様な環境下で視覚入力だけに基づいて実現する必要がある。最近の視覚学習ベースの手法は、脚式ロボットによる障害物回避、階段昇降、段差ジャンプといったタスクで有望な成果を示している [3, 4, 5, 6] が、ロコマニピュレーションはさらに高い精度の制御が必要であり、複雑性も増すため、エンドツーエンドの学習では難易度が高くなる。

本研究では、視覚入力から得られる情報を基に、二階層のフレームワークを構築する。上位レベルのポリシーがエンドエフェクタの位置やロボット本体の速度指令を生成し、下位レベルの制御ポリシーがその指令を追従する。下位レベルのコントローラは汎用的に設計し、上位レベルのポリシーは特定のタスクに合わせるという設計思想である。本手法を「Visual Whole-Body Control (VBC)」と名付けた。具体的には、VBCでは以下の3段階の学習を行う。

1. 低レベルポリシー
   あらゆるゴールを追従できる汎用的な低レベルポリシーを強化学習（RL）によって学習し、全身制御行動を獲得する。
2. 特権（privileged）付き教師ポリシー
   上記で学習した低レベルポリシーを用いながら、物体の形状や姿勢情報にアクセス可能な「特権付き」の状態ベースのポリシーをRLで学習する。これにより、タスク固有の即時的なゴールを指示する。
3. 視覚ベース生徒ポリシーへの蒸留
   上記の特権付きポリシーを模倣（オンラインイミテーションラーニング）しながら、深度画像ベースの視覚モータポリシーを学習する。
   （教師ポリシーが行う状態ベースの高度な制御を、画像ベースの生徒ポリシーが学習し、最終的に実機で動作可能な形にする。）

すべての学習はシミュレーション内で行い、その後、シミュレーションから実機への転移（Sim2Real）により、実機のロボット上にポリシーを展開する。本研究で使用するハードウェアプラットフォームは、Unitree B1 四足歩行ロボットに Unitree Z1 ロボットアームを搭載したものである。シミュレーションでは、多様な高さ・オブジェクト・環境を対象に提案手法の有効性を検証した。実機実験では、14種類の異なる物体（一般的な形状、日用品、不規則な形状のものなど）を高さ3パターン（地面上、箱の上、テーブルの上）で把持するタスクを行い、高成功率を達成した。失敗時には再試行する挙動も自然に獲得しており、汎用性の高いシステムであることが示されている。

---

2 関連研究

脚式ロボットによるロコマニピュレーションの分野では、さまざまな研究が行われている。Bellicosoら [7] はオンラインのモーションプランニングフレームワークを設計し、階層的最適化アルゴリズムに基づく全身制御によってロコマニピュレーションを実現している。Fuら [8] は、全身制御ポリシーをRLでエンドツーエンドに学習する有効な手法を提案した。Maら [9] は、モデルベースのアーム制御と学習ベースの歩行制御を組み合わせているが、外乱に対する耐性（押しに対する応答）のみを示し、タスク遂行自体は行っていない。

注目すべき点として、上記の研究はいずれも、人間によるある程度の介入やテレオペレーションを必要としており、自律的な動作には至っていない。一方、Zhangら [10]、Yokoyamaら [11]、Arcariら [12] は、四足ロボットプラットフォームを用いて自律的なモバイルマニピュレーションを行っているが、ロボットのAPIで提供されるモデルベースコントローラ（静的に高さを一定に保つ）を使い、主にアームのマニピュレーション部分に焦点を当てている。これらの手法では、物体の高さに応じてロボットの全身姿勢を柔軟に変化させるような挙動は扱っていない。

Ferrolhoら [13] は、ロバスト性指標を組み込んだ軌道最適化フレームワークによって複雑なロコマニピュレーションタスクを解く手法を提案しているが、タスクを設定する際にモーションキャプチャによる対象物体の位置情報を利用しており、適用範囲が限定的である。Sleimanら [14] は、領域特化型ルールや軌道最適化、グラフ探索、サンプリングベースのプランニングを組み合わせた二段階探索戦略を提案し、複雑なタスクを実現しているが、ユーザによるシーン記述の提供など、相当量の事前知識が必要となっている。Zimmermannら [15] も軌道最適化を用いた手法を提案し、四足ロボットによる移動・把持動作を実証しているが、単純な環境・オブジェクトでの例のみが示されている。

上記のようにアーム搭載型の脚式ロボットを対象にした研究がある一方、Wolfslagら [16] は四足ロボットの脚を使って押しや箱の持ち上げなどを行う手法を提案しているが、追加のサポート用脚（prongs）を用いて剛性やマニピュレーション性能を高めている。Jeonら [17] は、本研究と同様に階層的なフレームワークを用いているが、視覚入力を用いておらず、かつアームではなく、四足ロボットによる物体押しのみを行う。

本研究のシステムは、上記研究とは以下の点が大きく異なる：

• 自律性と視覚ベースであること
  多くの既存研究では、ある程度のユーザ介入や外部センサー（例：モーションキャプチャ）を必要とするのに対し、本研究のシステムは、ロボットに搭載したカメラから得られる深度画像のみで物体を把持する。
• 全身制御の活用
  ロボット本体の高さを動的に変化させたり（身体を曲げたり、伸ばしたり）、高い精度の歩行とアーム制御を同時に学習し、いろいろな高さのオブジェクトを把持可能とする。
• 多様な環境での動作
  提案手法では、室内だけでなく屋外でも深度画像のみで動作できる。多くの既存研究は限られた環境下での評価に留まっている。

図2は、脚の高さを静的に固定するアプローチ [10, 11, 12] と比較して、VBCが異なる高さの物体（地面のように低い位置の物体など）を扱う際に、身体を大きく曲げたり、伸ばしたりする柔軟な動作が可能であることを示している。

---

3 Visual Whole-Body Control

提案するVBCフレームワーク（図3参照）は、低レベルのゴール追従ポリシーと、高レベルのタスクプランニングポリシーから構成される。低レベルポリシーは、ロボット本体の速度コマンドとエンドエフェクタの目標姿勢を入力として受け取り、それを達成するよう制御する。一方、高レベルポリシーは、深度画像やプロプリオセプション情報をもとに低レベルポリシーへコマンドを与える。これにより、ロコマニピュレーションを自律的に実現する。本章では提案フレームワークの詳細を述べる。

3.1 全身ゴール追従のための低レベルポリシー

図3の青枠部分に対応する低レベル制御ポリシー $\pi_{\text{low}}$ は、あらゆるエンドエフェクタ姿勢および本体速度指令を追従可能にするため、強化学習で学習される。多様な地形上でもロバストに動作できるよう訓練し、全身制御行動を実現する。

• コマンド
  低レベルポリシーへのコマンド $\mathbf{b}_t$ は次のベクトルとして定義する：

  $$\mathbf{b}_t = [\,\mathbf{p}^{\text{cmd}}_t, \, \mathbf{o}^{\text{cmd}}_t, \, v^{\text{cmd}}_{\text{yaw}}\,],$$ここで $\mathbf{p}^{\text{cmd}} \in \mathbb{R}^3,\ \mathbf{o}^{\text{cmd}} \in \mathbb{R}^3$ はエンドエフェクタの位置と姿勢（オイラー角）コマンドを表す（詳細は付録C.1）。さらに、ロボットベース座標系での前進速度（$v^{\text{cmd}}_{\text{lin}}$）とヨー角速度（$\omega^{\text{cmd}}_{\text{yaw}}$）もコマンドとして与えられる。低レベルポリシー訓練時には、前進速度を $[-0.6, 0.6]\,[\text{m/s}]$の範囲、ヨー角速度を $[-1.0, 1.0]\,[\text{rad/s}]$ の範囲から一様サンプリングする。実際のタスクでは、高レベルポリシーが出力する指令値を適宜クリップして用いる。

• 観測
  低レベルポリシーの観測 $\mathbf{o}_t$ は90次元のベクトルであり、

  $$\mathbf{o}_t = [\,\mathbf{s}^{\text{base}}_t,\ \mathbf{s}^{\text{arm}}_t,\ \mathbf{s}^{\text{leg}}_t,\ \mathbf{a}_{t-1},\ \mathbf{z}_t,\ \mathbf{t}_t,\ \mathbf{b}_t\,]$$と定義される。ここで、$\mathbf{s}^{\text{base}}_t \in \mathbb{R}^5$ はロボット本体の状態（ロール、ピッチ、ヨーおよびそれらの角速度）、$\mathbf{s}^{\text{arm}}_t \in \mathbb{R}^{12}$ はアーム関節の状態（各アーム関節の位置と速度。エンドエフェクタの閉ループ制御用関節を除く）、$\mathbf{s}^{\text{leg}}_t \in \mathbb{R}^{28}$ は脚関節の状態（各脚関節の角度と速度、および足先接触パターン）を含む。また、$\mathbf{a}_{t-1} \in \mathbb{R}^{12}$ は前の時刻のポリシー出力、$\mathbf{z}_t \in \mathbb{R}^{20}$ は環境パラメータの乱数ベクトル（[8] を参考）、$\mathbf{t}_t$ は歩容（ゲイト）を安定化させるためのタイミング変数である [4]。

• アクション
  低レベルポリシーは、主に四足ロボット側を直接制御する。具体的には、4脚×3関節＝12自由度分の目標関節角度を出力し、それをPD制御器でトルクコマンドに変換する。
• アーム制御
  先行研究 [8] により、全身のRLポリシーでアームの各関節角度まで直接制御しようとすると、エンドエフェクタの位置と姿勢を高精度に追従するのが難しいことがわかっている。そこで本研究では、ポリシーが出力するのはエンドエフェクタの「目標姿勢」（位置+オイラー角）のみとし、逆運動学（IK）で関節角度に変換している。具体的には、擬似逆ヤコビアンを用いて

  $$\Delta \theta = J^T (J J^T)^{-1} \, e$$を解き、$\Delta \theta$ を求める。ここで $J$ はヤコビアン行列、$e$ はエンドエフェクタ姿勢の目標との差分（位置3次元+オイラー角3次元）である。

• エンドエフェクタ目標姿勢のサンプリング
  低レベルコントローラを多様な動作でロバスト化するため、学習中はエンドエフェクタのさまざまな目標をサンプリングして追従する。ロボットベース原点を基準とし、高さ方向を除いた座標系を考え、一定間隔でランダムな目標姿勢を与える。これにより全身を使った大きな動作が学習でき、作業空間が拡大する（付録C.1 図9 参照）。

その他の報酬設計、ポリシーネットワーク構造、乱数化の詳細はページ数の都合で付録C.1に示す。

---

3.2 タスクプランニングのための高レベルポリシー

実機では高周波数で動作させる際、ロボットが取得できるのは深度画像のみである。一方、視覚ベースの強化学習は計算コストや学習の難しさが課題となる [18, 19, 20, 21, 22]。そこで本研究では、特権付き状態（物体の形状や姿勢）を利用できる「教師ポリシー」をまず学習し、その後、視覚ベースの「生徒ポリシー」がこれを模倣してタスクを遂行できるようにする。

3.2.1 特権付き教師ポリシー

特権付き教師ポリシーは、固定済みの低レベルポリシーの上に構築され、タスク固有の報酬関数とRLによって学習される。物体の位置・姿勢・形状情報を直接観測に含む「特権的」な情報を扱うため、ロボットが物体把持などのタスクを高い成功率で達成する動作を獲得しやすい。

• 特権付き観測
  教師ポリシーの観測は1094次元で、

  $$\mathbf{o}_t = \bigl[\, \mathbf{z}^{\text{shape}}, \, \mathbf{s}^{\text{obj}}_t, \, \mathbf{s}^{\text{proprio}}_t, \, \mathbf{v}^{\text{base}}_{t}, \, \mathbf{a}_{t-1} \bigr],$$と表される。ここで $\mathbf{z}^{\text{shape}} \in \mathbb{R}^{1024}$ は、PointNet++ [23] によって物体の点群から抽出した形状特徴ベクトル（学習済みで固定）、$\mathbf{s}^{\text{obj}}_t \in \mathbb{R}^6$ はロボットアーム基準座標系における物体の位置・姿勢情報（局所座標系）。$\mathbf{s}^{\text{proprio}}_t \in \mathbb{R}^{53}$ はロボットの関節位置（19次元）と関節速度（18次元）、エンドエフェクタの位置・姿勢（3+3次元）、およびグリッパーの状態を含む。
  $\mathbf{v}^{\text{base}}_t \in \mathbb{R}^{3}$ はベースの平面速度（x方向の線速度とヨー角速度など）であり、$\mathbf{a}_{t-1}$ は前時刻のアクション（高レベルポリシーが出力したコマンド）である。

• 高レベルアクション
  教師ポリシー $\pi_{\text{high}}$ は、次のような9次元のアクションを出力する：

  $$\mathbf{a}_t = [\,\Delta \mathbf{p}^{\text{cmd}}_t,\ \mathbf{v}^{\text{cmd}}_t,\ p^{\text{gripper}}_t\,].$$ここで、$\Delta \mathbf{p}^{\text{cmd}}_t \in \mathbb{R}^6$ はエンドエフェクタの目標ポーズの微小変化量（位置3次元+オイラー角3次元）、$\mathbf{v}^{\text{cmd}}_t \in \mathbb{R}^2$ は四足の平面速度（前進・ヨー）、$p^{\text{gripper}}_t \in \{0,1\}$ はグリッパーの開閉状態である。

• ドメイン乱数化
  教師ポリシー学習時には、ロボットと地形の摩擦係数、質量、重心位置などを乱数化している。これは、すでに低レベルポリシーがこうした乱数化を部分的に扱っているが、両ポリシーの動作周波数やアームのPD制御パラメータ（$K_p, K_d$）も含め、現実では微妙に異なる可能性があるためである。ピックアップタスクにおいては、テーブルの高さ、ロボットの初期位置・姿勢、物体の位置・姿勢などを乱数化している。

3.2.2 視覚モータ生徒ポリシー

特権付き教師ポリシーは、理想的な状態情報（物体形状・姿勢）を用いるため、堅牢かつ適応的な挙動を獲得しやすい。しかし、現実世界ではこれらの情報を得るのは困難である。そこで本研究では、教師ポリシーが出力する軌道を模倣学習（DAgger）する形で、深度画像ベースの生徒ポリシーを訓練し、実機に適用可能とする。

• 観測とアクション
  視覚ベースの生徒ポリシーが受け取る観測 $\mathbf{o}_t$ は

  $$\mathbf{o}_t = [\,\mathbf{o}^{\text{image}}_t,\ \mathbf{s}^{\text{proprio}}_t,\ \mathbf{a}_{t-1}\,],$$と定義される。ここで $\mathbf{o}^{\text{image}}_t$ はオブジェクトのセグメンテーションマスクと深度画像で構成される。教師ポリシーと同様に $\mathbf{s}^{\text{proprio}}_t$ や $\mathbf{a}_{t-1}$ も含む。出力アクションは教師ポリシー同様の9次元である。

• 蒸留のためのイミテーション学習
  教師の方策を直接RLで視覚入力から学習するのは難しいため、まず教師ポリシーを使って生成したサンプルで生徒ポリシーを初期化し、その後、DAggerで反復的にデータを収集して学習を進める。こうすることで、視覚ポリシーの学習を安定・効率化する。
• 乱数化とデータ拡張
  シミュレーションと実機の外観ギャップを埋めるため、カメラの位置や回転をわずかに乱数化する。また、深度画像に対してRandomErasing, GaussianBlur, GaussianNoise, RandomRotationなどの画像拡張を行う。深度画像は質の低い点群を除去するため、0.2m未満の値をクリップし、正規化する。その他、教師・生徒ポリシーの報酬設計の詳細は付録C.2を参照。

---

3.3 実機適用

• ロボットプラットフォーム
  本研究で使用するのは、Unitree B1 四足ロボットにUnitree Z1 アームを搭載し、計19自由度（四足12+アーム6+グリッパー1）を持つプラットフォームである。B1に搭載されたオンボードコンピュータで低レベルポリシーを走らせ、別のオンボードコンピュータでビジョン入力を処理しつつ高レベルポリシーを実行する（図4参照）。
• ビジョンマスク
  提案システムは、人間による煩雑な介入を最小限にする一方、背景から物体をセグメントして深度画像を得る必要がある。そこで、TrackingSAM [24] を用いて、RGB-Dカメラからの映像をリアルタイム（約10fps）で追跡およびマスク生成する（詳細は付録Jを参照）。TrackingSAM は、ビデオ物体追跡モデル [25] と対話式セグメンテーションインタフェース [26] を組み合わせたツールであり、オブジェクトの初期位置をユーザが軽く指定するだけで自動追跡が可能になる。実機実験では、リセットのたびにRGB-Dカメラ映像で対象物体を最初にアノテーションしておき、以後は自動追跡＆マスク生成を続ける。
  取得した深度画像はHole-fillingやクリッピングで前処理を行う。カメラは頭部側と手首側の2台を用意し、どちらか一方に物体が映っていればマスクを適用できるようにしている。