ロボットによるドア開け

腕のついた台車ロボットでドアを開けるというタスクには、様々な困難が存在しています。

• ドアと自分との位置関係の推定
• ドアノブの認識
• ドアや壁に当たらないような軌道の生成
• 台車と腕の協調制御
• 多様なドアの重量、形状に対する適応的制御
• etc.

本事例ではSLAM技術とモデル予測制御、および機械学習を組み合わせた手法によって、これらの問題を解決しドア開けを成功させました。

前準備と認識

まずはドア開け動作の前準備として、所定の位置に移動することから始まります。ARマーカーとLidarを用いて自己位置を推定し、古典的なPID制御によってドアに対してあらかじめ決めておいた位置まで制御します。次に、ARマーカーと簡単な画像/点群処理によってドアノブを認識します。

目標軌道の生成と逆運動学の計算

台車の位置姿勢を $\mathbf{q}_c$、アーム関節角を $\mathbf{q}_a$、台車に対する相対的なアーム手先の位置姿勢を $\mathbf{q}_r$ とします。

最初に、ドアの幅に応じて動的に台車位置姿勢の参照軌道 $\mathbf{q}_c^{\mathrm{ref}}$ とアーム手先の位置姿勢の参照軌道 $\mathbf{y}^{\mathrm{ref}}$ を計画します。

Nyokkeyの腕には関節自由度が2つあるため、冗長自由度が1つありリンク同士の干渉を避けるような動きが可能となります。そのような動きを与える人工ポテンシャルを $H(\mathbf{q}_a)$ とし、アームの逆運動学に関するヤコビアンを $J(\mathbf{q}_a)$、ステップサイズを $k_H$ とすると、このポテンシャルの下での目標軌道の逆運動学は次式となります。

$$\mathbf{q}_a^{\mathrm{ref}} = J(\mathbf{q}_a^{\mathrm{ref}})^{\dagger} \mathbf{y}_r^{\mathrm{ref}} - k_H \left( I - J(\mathbf{q}_a^{\mathrm{ref}})^{\dagger} J(\mathbf{q}_a^{\mathrm{ref}}) \right) \nabla H(\mathbf{q}_a^{\mathrm{ref}})$$

Smart MPC®による制御

Smart MPC®は機械学習とモデル予測制御を組み合わせた制御アルゴリズムです。

ここでは、状態変数を $\boldsymbol{x} = (q_c, q_a, \dot{q}_c, \dot{q}_a)$、台車制御入力とアーム関節トルクを $\boldsymbol{u}$、ドアの質量などの環境パラメータを $\boldsymbol{\theta}$ とすると、ロボットの動力学（運動方程式）を次のように定義します。

$$\dot{\boldsymbol{x}} = f_{\boldsymbol{\theta}}(\boldsymbol{x}, \boldsymbol{u})$$

機械学習は動力学モデル $f_{\boldsymbol{\theta}}$ を獲得するのに使われます。MPCでは以下の最適化問題を解きます。

$$\begin{aligned} & \min_{\{ \boldsymbol{u}(t) \}_{t \in [0,T]}} \frac{1}{2} \int_0^T \left[ (\boldsymbol{x} - \boldsymbol{x}^{\mathrm{ref}})^\top Q (\boldsymbol{x} - \boldsymbol{x}^{\mathrm{ref}}) + \boldsymbol{u}^\top R \boldsymbol{u} \right] dt \\ & \text{subject to} \quad \dot{\boldsymbol{x}} = f(\boldsymbol{x}, \boldsymbol{u}), \quad \boldsymbol{x}(0) = \boldsymbol{x}_0 \end{aligned}$$

この最適化問題は拡張ラグランジュ法によって解くことができ、実時間での制御を可能にします。

ドア開けの一連の動作は「レバーをつかんで回す」「ノック」「ドアの押し開けと前進」の3ステップからなります。レバーの硬さやドアの重さが変動しうるため、ベイズフィルタを用いて $\boldsymbol{\theta}$ の推定を行います。

Smart MPCとベイズ推定を組み合わせることで、より多様な条件下でのドア開け動作が可能となりました。