ロボットによるドア開け
腕のついた台車ロボットでドアを開けるというタスクには、様々な困難が存在しています。
- ドアと自分との位置関係の推定
- ドアノブの認識
- ドアや壁に当たらないような軌道の生成
- 台車と腕の協調制御
- 多様なドアの重量、形状に対する適応的制御
- etc.
本事例ではSLAM技術とモデル予測制御、および機械学習を組み合わせた手法によって、これらの問題を解決しドア開けを成功させました。
自己位置推定とドアノブの認識
まずはドア開け動作の前準備として、所定の位置に移動することから始まります。
ARマーカーとLidarを用いて自己位置を推定し、古典的なPID制御によってドアに対してあらかじめ決めておいた位置まで制御します。
次に、ARマーカーと簡単な画像/点群処理によってドアノブを認識します。
目標軌道の生成と逆運動学の計算
台車の位置姿勢を\(\bm{q}_c\)、アーム関節角を\(\bm{q}_c\)、台車に対する相対的なアーム手先の位置姿勢を\(\bm{y}\)とします。
最初に、ドアの幅に応じて動的に台車位置姿勢の参照軌道\(\bm{q}_c^{ref}\)とアーム手先の位置姿勢の参照軌道\(\bm{y}^{ref}\)を計画します。
Nyokkeyの腕には関節自由度が7つあるため、冗長自由度が1つありリンク同士の干渉を避けるような動きが可能となります。そのような動きを与える人工ポテンシャルを\(\bm{H}(\bm{q}_a)\)とし、アームの運動学に関するヤコビアンを\(\bm{J}(\bm{q}_a)\)、ステップサイズ\(k_H\)とすると、このポテンシャルの下での目標軌道の逆運動学は
\[ \dot{\bm{q}}_a^{ref}=\bm{J}(\bm{q}_a^{ref})^{\dagger}\dot{\bm{y}}^{ref} - k_{H}(\bm{I}-\bm{J}(\bm{q}_a^{ref})^{\dagger}(\bm{q}_a^{ref}))\bm{\nabla}\bm{H}(\bm{q}_a^{ref}) \]
となります。
Smart MPCによる制御
Smart MPCは機械学習とモデル予測制御を組み合わせた制御アルゴリズムです。
ここでは、状態変数を\(\bm{x}=(\bm{q}_c, \bm{q}_a, \dot{\bm{q}_c}, \dot{\bm{q}_a})\)、台車制御入力とアーム関節トルクを\(\bm{u}\)、ドアの質量などの環境パラメータを\(\bm{\theta}\)とするとロボットの動力学（運動方程式）を\(\dot{\bm{x}}=\bm{f}_{\theta}(\bm{x},\bm{u})\)とします。
機械学習は動力学モデル\(\dot{\bm{x}}=\bm{f}_{\theta}(\bm{x},\bm{u})\)を獲得するのに使われます。得られた2つのモデルに対してMPCを使って制御することで多様なロボットやドアに対して統一的な方法でドア開けを実行することが可能になります。
MPCでは以下のような最適化問題を解きます。
\[ \{\bm{u}(t)\}_{t\in [0,1]} = \argmin_{\{ \bm{u}(t)\}_{t \in [0, 1]}} \frac{ 1 }{ 2 } \int_{ 0 } ^ { T } \{(\bm{ x } -\bm{ x } ^ { ref })^{ T }\bm{ Q }(\bm{ x } -\bm{ x } ^ { ref }) + \bm{ u } ^ { T }\bm{ R }\bm{ u } \} \]
\[ s.t. \quad \dot{\bm{x}} = \bm{f}(\bm{x}, \bm{u}) , \quad \bm{x}(0) = \bm{x}_{0} \]
この最適化問題は拡張ラグランジュ法によって解くことが出来、実時間での制御を可能にします。
上記の定式化によって具体的な動作を実行します。
ドア開けの一連の動作は
- レバーをつかんで回す
- ノック
- ドアの押し開けと全身
の3ステップからなりますが、レバーの回転やドアの押し開けではレバーの硬さやドアの重さが変動しうるため、ベイズフィルタを用いて𝜃の推定をします。
Smart MPCとベイズ推定を組み合わせることで、より多様な条件下でのドア開け動作が可能となりました。