新入社員の中村です。前回の6自由度アームの逆運動学に続いて、CRANE-X7の重力補償を実装します。重力補償は、重力によって生じる各関節のトルクを打ち消すために必要なトルクを追加することを指します。重力補償を用いると重力に抗する力がフィードフォワード制御として与えられるため、フィードバック制御であるPID制御のゲインを小さくすることができ、柔らかい制御を実現することができます。本記事では、ラグランジュ方程式を用いたCRANE-X7の重力補償の実装方法について説明します。
問題設定
下図にCRANE-X7の関節の座標系を示します。
\(\Sigma_w\)はワールド座標系、\(\Sigma_h\)は手先座標系、\(\Sigma_1,\cdots,\Sigma_6\)は各関節の座標系、\(l_1,,\cdots,l_7\)は各リンクの長さを表します。\(\Sigma_1\)では\(z\)軸、\(\Sigma_3\)と\(\Sigma_5\)と\(\Sigma_7\)では\(x\)軸、その他の座標系では\(y\)軸が回転します。各関節の回転角度\(\theta_1,\cdots,\theta_7\)が与えられたときの各関節のトルク\(\tau_1,\cdots,\tau_7\)を求めます。
重力補償の実装と実機での検証
リンク\(k\)の重力補償トルク\(\tau_k\)は次式で求まります。
\begin{equation}
\tau_k=\frac{\partial U}{\partial \theta_k}=\sum_{i=k}^7m_ig\begin{bmatrix}0&0&1&0\end{bmatrix}{^0T_{k-1}}{^{k-1}T’_k}{^kT_i}\begin{bmatrix}{p_{g_i}}\\1\end{bmatrix}
\end{equation}
ここで、\(m_i\)はリンク\(i\)の質量、\(g\)は重力加速度、\({^0T_{k-1}}\)は座標\(\Sigma_0\)から見た座標\(\Sigma_{k-1}\)の座標の位置姿勢を表す同次変換行列、\({^{k-1}T’_k}\)は同次変換行列\({^{k-1}T_k}\)を\(\theta_k\)で微分した行列、\(p_{g_i}\)は座標系\(\Sigma_i\)におけるリンク\(i\)の重心位置ベクトルを表します。上式の導出は以降の節で行います。
上記の重力補償トルクの式を実装し、CRANE-X7の重力補償を検証します。以下の動画に、重力補償を実装した状態での動作を示します。
動画で示されているように、重力を打ち消すトルクが入力されており、手先が落ちないようになります。以降の節では、ラグランジュ方程式を用いて重力補償トルクの計算式を導出します。
ラグランジュ方程式
本記事では、重力補償トルクをラグランジュ方程式から求めます。ラグランジュ方程式は、以下のように表されます。
\begin{equation}
\frac{d}{dt}\frac{\partial L}{\partial \dot\theta_i}-\frac{\partial L}{\partial \theta_i}=\tau_i
\end{equation}
\(L\)はラグラジアンで、運動エネルギー\(K\)と位置エネルギー\(U\)を用いて次のように表されます。
\begin{equation}
L(\theta,\dot\theta)=K(\theta,\dot\theta)-U(\theta)
\end{equation}
ただし、\(\theta = \begin{bmatrix}\theta_1&\cdots&\theta_7\end{bmatrix}^T\)、\(\dot\theta = \begin{bmatrix}\dot\theta_1&\cdots&\dot\theta_7\end{bmatrix}^T\)とします。これを解くことで運動方程式を得ることができます。
例として、1リンクアームを考えます。重心までの距離を\(l_g\)、重力加速度を\(g\)、質量を\(m\)、重心周りの慣性モーメントを\(I_g\)、回転角を\(\theta\)、トルクを\(\tau\)とします。運動エネルギーと重力による位置エネルギーは次のように表されます。
\begin{align}
K(\theta,\dot\theta)&=\frac{1}{2}m(l_g\dot\theta)^2+\frac{1}{2}I_g\dot\theta^2\\
U(\theta)&=mgl_g\sin\theta
\end{align}
ラグラジアンは次のように表されます。
\begin{equation}
L(\theta,\dot\theta)=K(\theta,\dot\theta)-U(\theta)=\frac{1}{2}m(l_g\dot\theta)^2+\frac{1}{2}I_g\dot\theta^2-mgl_g\sin\theta
\end{equation}
ラグランジュ方程式の各項を計算します。
\begin{align}
\frac{d}{dt}\frac{\partial L}{\partial \dot\theta}&=\frac{d}{dt}(ml_g^2\dot\theta+I_g\dot\theta)=(I_g+ml_g)\ddot\theta\\
-\frac{\partial L}{\partial \theta}&=mgl_g\cos\theta
\end{align}
これらを用いてラグランジュ方程式を整理すると、次のような運動方程式が求まります。
\begin{equation}
(I_g+ml_g)\ddot\theta+mgl_g\cos\theta=\tau
\end{equation}
同次変換
重力補償トルクを計算する前に、リンク重心位置の計算方法について説明します。前回の6自由度アームの逆運動学の記事で説明した、ローカル座標系からワールド座標系への変換について復習します。座標系\(\Sigma_1\)の回転を表す行列\(R(\theta)\)と座標系\(\Sigma_0\)から見た座標系\(\Sigma_1\)の位置\(^0p_1\)が与えられているとします。座標系\(\Sigma_1\)の点\(^1p\)は、座標系\(\Sigma_0\)での点\(^0p\)と次のような関係にあります。
\begin{equation}
{^0p}=R(\theta){^1p}+{^0p_1}
\end{equation}
これを次のように書き換えます。
\begin{equation}
\begin{bmatrix}{^0p}\\1\end{bmatrix}=\begin{bmatrix}R(\theta)&{^0p_1}\\O&1\end{bmatrix}\begin{bmatrix}{^1p}\\1\end{bmatrix}
\end{equation}
ここで、回転行列と位置ベクトルをまとめた行列
\begin{equation}
{^0T_1}=\begin{bmatrix}R(\theta)&{^0p_1}\\O&1\end{bmatrix}
\end{equation}
を同次変換行列と呼び、これを用いると変換は次のように表されます。
\begin{equation}
\begin{bmatrix}{^0p}\\1\end{bmatrix}={^0T_1}\begin{bmatrix}{^1p}\\1\end{bmatrix}
\end{equation}
同様に座標系\(\Sigma_1\)から見た座標系\(\Sigma_2\)の位置姿勢を表す同次変換行列を\({^1T_2}\)とすると、座標系\(\Sigma_2\)の点の位置\(^2p\)と座標系\(\Sigma_0\)から見た位置\(^0p\)は次の関係にあります。
\begin{equation}
\begin{bmatrix}{^0p}\\1\end{bmatrix}={^0T_1}{^1T_2}\begin{bmatrix}{^2p}\\1\end{bmatrix}
\end{equation}
このように同次変換行列の積を用いると異なる座標系の間で座標を変換することができます。
重力補償の計算方法
重力補償トルクは、運動方程式に関して\(\dot\theta=\ddot\theta=\mathbf{0}\)を代入すると求まります。前節の1リンクアームの場合では、次のように求まります。
\begin{equation}
\tau=mgl_g\cos\theta
\end{equation}
次にCRANE-X7の場合を考えます。1リンクアームと同様に、ラグランジュ方程式を求めて\(\dot\theta=\ddot\theta=\mathbf{0}\)を代入します。運動エネルギー\(K\)と位置エネルギー\(U\)は次のように表されます。
\begin{align}
K&=\sum_{i=1}^7\left(\frac{1}{2}m_i|{^0\dot p_{g_i}}|^2+\frac{1}{2}I_{g_i}\dot\theta_i^2\right)\\
U&=\sum_{i=1}^7m_igz_i
\end{align}
ここで、\({^0p_{g_i}}\)は座標系\(\Sigma_0\)から見たリンク\(i\)の重心の位置ベクトルを表します。\({^0p_{g_i}}\)は次のように表されます。
\begin{equation}
\begin{bmatrix}{^0p_{g_i}}\\1\end{bmatrix}={^0T_1}\cdots{^{i-1}T_i}{p_{g_i}}={^0T_i}\begin{bmatrix}{p_{g_i}}\\1\end{bmatrix}
\end{equation}
ここで、\({p_{g_i}}\)は座標系\(\Sigma_i\)から見たリンク\(i\)の重心の位置ベクトルを表します。\({^0T_i}\)は同次変換行列の積であり、各同次変換行列は次に挙げるロールピッチヨー角に関連する各軸の回転のうちいずれかを含んでいます。
\begin{align}
R_x(\theta_i)&=
\begin{bmatrix}
1&0&0\\
0&\cos\theta_i&-\sin\theta_i\\
0&\sin\theta_i&\cos\theta_i
\end{bmatrix}\\
R_y(\theta_i)&=
\begin{bmatrix}
\cos\theta_i&0&\sin\theta_i\\
0&1&0\\
-\sin\theta_i&0&\cos\theta_i
\end{bmatrix}\\
R_z(\theta_i)&=
\begin{bmatrix}
\cos\theta_i&-\sin\theta_i&0\\
\sin\theta_i&\cos\theta_i&0\\
0&0&1
\end{bmatrix}
\end{align}
これらから、\({^0p_{g_i}}\)が\(\sin\theta\)と\(\cos\theta\)で表されることがわかります。したがって、運動エネルギー\(K\)は\(s=\sin\theta\)と\(c=\cos\theta\)を用いて次のように表されます。
\begin{equation}
K=K(\dot\theta,s,c)
\end{equation}
一方で、位置エネルギー\(U\)は次のように表されます。
\begin{equation}
U=U(\theta)
\end{equation}
リンク\(k\)についてラグランジュ方程式の第1項を計算します。
\begin{align}
\frac{d}{dt}\frac{\partial L}{\partial \dot\theta_k}&=\frac{d}{dt}\frac{\partial K(\dot\theta,s,c)}{\partial \dot\theta_k}\\
&=\frac{\partial}{\partial \dot\theta}\left(\frac{\partial K}{\partial \dot\theta_k}\right)\frac{d\dot\theta}{dt}
+\frac{\partial}{\partial s}\left(\frac{\partial K}{\partial \dot\theta_k}\right)\frac{ds}{dt}
+\frac{\partial}{\partial c}\left(\frac{\partial K}{\partial \dot\theta_k}\right)\frac{dc}{dt}\\
&=\frac{\partial}{\partial \dot\theta}\left(\frac{\partial K}{\partial \dot\theta_k}\right)\ddot\theta
+\frac{\partial}{\partial s}\left(\frac{\partial K}{\partial \dot\theta_k}\right)\frac{\partial s}{\partial \theta}\dot\theta
+\frac{\partial}{\partial c}\left(\frac{\partial K}{\partial \dot\theta_k}\right)\frac{\partial s}{\partial \theta}\dot\theta
\end{align}
\(\dot\theta=\ddot\theta=\mathbf{0}\)のとき、
\begin{equation}
\frac{d}{dt}\frac{\partial L}{\partial \dot\theta_k}=0
\end{equation}
となり、第1項は0となります。
つぎにラグランジュ方程式の第2項を計算します。
\begin{equation}
-\frac{\partial L}{\partial \theta_k}=-\sum_{i=1}^7\left(\frac{1}{2}m_i\frac{\partial}{\partial \theta_k}|{^0\dot p_{g_i}}|^2\right)+\frac{\partial U}{\partial \theta_k}
\end{equation}
\({^0\dot p_{g_i}}\)を計算すると、
\begin{equation}
{^0\dot p_{g_i}}=\frac{d}{dt}{^0p_{g_i}}(s,c)=\frac{\partial ^0p_{g_i}}{\partial s}\frac{ds}{dt}+\frac{\partial ^0p_{g_i}}{\partial c}\frac{dc}{dt}=\frac{\partial ^0p_{g_i}}{\partial s}\frac{\partial s}{\partial \theta}\dot\theta+\frac{\partial ^0p_{g_i}}{\partial c}\frac{\partial c}{\partial \theta}\dot\theta
\end{equation}
\(|{^0\dot p_{g_i}}|^2\)の偏微分を計算すると、
\begin{equation}
\frac{\partial}{\partial \theta_k}|{^0\dot p_{g_i}}|^2=2{^0\dot p_{g_i}}\cdot\frac{\partial {^0\dot p_{g_i}}}{\partial \theta_k}
\end{equation}
\(\dot\theta=\ddot\theta=\mathbf{0}\)のとき、\({^0\dot p_{g_i}}=\mathbf{0}\)より
\begin{equation}
\frac{\partial}{\partial \theta_k}|{^0\dot p_{g_i}}|^2=0
\end{equation}
これを用いると、ラグランジュ方程式の第2項は次のように求まります。
\begin{equation}
-\frac{\partial L}{\partial \theta_k}=\frac{\partial U}{\partial \theta_k}
\end{equation}
各項の計算から、\(\dot\theta=\ddot\theta=\mathbf{0}\)のとき、ラグランジュ方程式は次のようになります。
\begin{equation}
\frac{\partial U}{\partial \theta_k}=\tau_k
\end{equation}
最後に、\(U\)の偏微分を求めます。
\begin{equation}
\frac{\partial U}{\partial \theta_k}=\sum_{i=1}^7m_ig\frac{\partial z_i}{\partial \theta_k}
\end{equation}
関節\(k\)が変化しても\(i<k\)を満たすリンク\(i\)の重心位置は変化しないため
\begin{equation}
\frac{\partial z_i}{\partial \theta_k}=0
\end{equation}
よって、\(i=k,\cdots,7\)に関する総和になります。
\begin{equation}
\frac{\partial U}{\partial \theta_k}=\sum_{i=k}^7m_ig\frac{\partial z_i}{\partial \theta_k}
\end{equation}
\(z_i\)の偏微分について
\begin{equation}
\frac{\partial z_i}{\partial \theta_k}=\frac{\partial}{\partial \theta_k}\begin{bmatrix}0&0&1&0\end{bmatrix}{^0T_ip_{g_i}}=\begin{bmatrix}0&0&1&0\end{bmatrix}{^0T_{k-1}}\frac{d {^{k-1}T_k}}{d \theta_k}{^kT_i}\begin{bmatrix}{p_{g_i}}\\1\end{bmatrix}
\end{equation}
ここで、
\begin{equation}
{^{k-1}T’_k}=\frac{d {^{k-1}T_k}}{d \theta_k}
\end{equation}
とおけば、
\begin{equation}
\frac{\partial z_i}{\partial \theta_k}=\frac{\partial}{\partial \theta_k}\begin{bmatrix}0&0&1&0\end{bmatrix}{^0T_ip_{g_i}}=\begin{bmatrix}0&0&1&0\end{bmatrix}{^0T_{k-1}}{^{k-1}T’_k}{^kT_i}\begin{bmatrix}{p_{g_i}}\\1\end{bmatrix}
\end{equation}
したがって、重力補償トルクは次のように求まります。
\begin{equation}
\tau_k=\frac{\partial U}{\partial \theta_k}=\sum_{i=k}^7m_ig\begin{bmatrix}0&0&1&0\end{bmatrix}{^0T_{k-1}}{^{k-1}T’_k}{^kT_i}\begin{bmatrix}{p_{g_i}}\\1\end{bmatrix}
\end{equation}
\({^{k-1}T’_k}\)は、回転軸に応じて次のいずれかになります。
\begin{align}
{^{k-1}T’_k}=
\begin{cases}
\begin{bmatrix}
0&0&0&0\\
0&-\sin\theta_k&-\cos\theta_k&0\\
0&\cos\theta_k&-\sin\theta_k&0\\
0&0&0&0
\end{bmatrix}&（x軸回転）\\
\begin{bmatrix}
-\sin\theta_k&0&\cos\theta_k&0\\
0&1&0&0\\
-\cos\theta_k&0&-\sin\theta_k&0\\
0&0&0&0
\end{bmatrix}&（y軸回転）\\
\begin{bmatrix}
-\sin\theta_k&-\cos\theta_k&0&0\\
\cos\theta_k&-\sin\theta_k&0&0\\
0&0&0&0\\
0&0&0&0
\end{bmatrix}&（z軸回転）
\end{cases}
\end{align}
まとめ
本記事では、ラグランジュ方程式に基づいて重力補償を実装する方法について解説しました。これとPID制御を組み合わせることで、柔らかな制御を行うことができます。柔らかな制御では、PIDゲインが小さいため、人と衝突したり、人を巻き込んだりした場合に強い力が発生しづらくなります。この機能は、人の近くで働く協働ロボットには欠かせません。また、柔らかい制御によって耐衝撃性が向上するため、転倒や衝突によるロボットの故障リスクを低減することができます。
前回の記事で実装した逆運動学と躍度最小軌道のような軌道生成を組み合わせることで、衝突しても安全なピックアンドプレイスシステムを構築することができます。CRANE-X7開発研修は、これで終了です。この研修では、技術を習得するだけでなく、チームでの開発方法を習得したり、コミュニケーション能力を高めたりすることも目的としました。研修で身につけた能力を今後の仕事で活かしていきたいと思います。
