はじめまして、りょーつといいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする23個目の記事です。前回までの記事は以下のリンクを参照してください。
スクリュー理論① 外積の歪対称行列表現
スクリュー理論② 行列の指数関数
スクリュー理論③ 行列指数と回転行列の関係
スクリュー理論④ 回転行列の成分表示
スクリュー理論⑤ 回転行列による座標系の表現
スクリュー理論⑥ 剛体の速度・加速度の座標変換公式の導出
スクリュー理論⑦ 三次元空間における右手系の回転行列
スクリュー理論⑧ 回転行列による座標変換
スクリュー理論⑨ 回転行列によるベクトルの回転
スクリュー理論⑩ 回転行列と多様体の関係
スクリュー理論⑪ 同次変換行列の導入
スクリュー理論⑫ 同次変換行列による座標系の表現と基底変換
スクリュー理論⑬ 歪対称行列表現の回転座標変換
スクリュー理論⑭ 同次変換行列の微分とツイストの導入
スクリュー理論⑮ スクリューの導入と行列指数による同次変換行列の表現
スクリュー理論⑯ アジョイント変換
スクリュー理論⑰ 回転行列の行列対数
スクリュー理論⑱ 回転行列の積分
スクリュー理論⑲ 行列指数を用いた同次変換行列の成分表示
スクリュー理論⑳ 同次変換行列の行列対数
スクリュー理論㉑ シリアルリンクロボットアームの順運動学
スクリュー理論㉒ 円筒座標ロボットアームの順運動学
スクリュー理論は剛体の運動を記述する方法で、ロボット工学などでよく使われています。日本の高専や大学ではDH法を使った記述を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。スクリュー理論を扱う日本語の文献は少ないので、この記事が誰かの助けになればいいなと思います。
目次
1.はじめに
2.円筒座標ロボットアームの順運動学
3.円筒座標ロボットアームのヤコビアン
4.おわりに
1. はじめに
今週の記事では、円筒座標ロボットアームのヤコビアンを計算してみようと思います。前回の記事でキャチロボ出場ロボットの順運動学を計算してみましたが、本記事では、そのロボットのヤコビアンを計算してみようと思います。ヤコビアンは各関節の角速度からエンドエフェクタのツイストを得る線形変換です。
スクリュー理論に登場するヤコビアンは複数種類ありますが、本記事で紹介するのはその中で最も直感的で理解しやすいヤコビアンです。他のヤコビアンについては次週以降に解説します。
2. 円筒座標ロボットアームの順運動学
今回用いるのは4自由度の円筒座標ロボットアームです。キャチロボバトルコンテスト2025に出場したロボットです。
第1関節はYaw軸まわりの回転関節、第2関節は動径方向へ伸縮する直動関節、第3関節もYaw軸まわりの回転関節、第4関節は鉛直方向へ伸縮する直動関節となります。関節の配置について図1、主要な寸法(変数)について図2に示します。
図2 関節の配置
図3 主要な寸法と座標系の定義
上記のロボットについてエンドエフェクタの初期姿勢(を表す同次変換行列)を
M
=
\begin{bmatrix}
1 & 0 & 0 & L \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & H - B \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{1}
としたとき、順運動学の計算結果は以下のように与えられます。
M'
=
e^{[\boldsymbol{\zeta}_1] \theta_1}
e^{[\boldsymbol{\zeta}_2] \theta_2}
e^{[\boldsymbol{\zeta}_3] \theta_3}
e^{[\boldsymbol{\zeta}_4] \theta_4}
M
=
\begin{bmatrix}
\cos{(\theta_1 + \theta_3)} & -\sin{(\theta_1 + \theta_3)} & 0 & (L + \theta_2) \cos{\theta_1} \\
\sin{(\theta_1 + \theta_3)} & \cos{(\theta_1 + \theta_3)} & 0 & (L + \theta_2) \sin{\theta_1} \\
0 & 0 & 1 & H - B + \theta_4\\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{2}
ここまでは前回の記事の復習になります。このとき、回転軸$\boldsymbol{\hat{\omega}}$の向きが一定である場合に限りますが、(2)式の計算結果を4次元ベクトルにまとめることができます。
まず(2)の計算結果を$z$軸まわりの角度$\theta$に関する回転行列$Rz(\theta)$と並進ベクトル$\boldsymbol{q}$を用いて表してみましょう。するとオイラー角をつかった行列演算でよく見る行列の形式になります。
M'
=
\begin{bmatrix}
Rz(\theta_1 + \theta_3) & \boldsymbol{q}\\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{3}
Rz(\theta_1 + \theta_3)
=
\begin{bmatrix}
\cos{(\theta_1 + \theta_3)} & -\sin{(\theta_1 + \theta_3)} & 0 \\
\sin{(\theta_1 + \theta_3)} & \cos{(\theta_1 + \theta_3)} & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\tag{4}
\boldsymbol{q}
=
\begin{bmatrix}
(L + \theta_2) \cos{\theta_1} \\
(L + \theta_2) \sin{\theta_1} \\
H - B + \theta_4 \\
\end{bmatrix}
\tag{5}
(4)式より、エンドエフェクタの姿勢は$z$軸まわりにしか回転しないことが分かります。そのため、回転行列ではなく、$z$軸まわりの回転角という1情報でエンドエフェクタの姿勢を記述できます。
エンドエフェクタの位置・姿勢の情報をまとめるベクトルを$\boldsymbol{r}$と定義します。
\boldsymbol{r}
=
\begin{bmatrix}
\phi \\
\boldsymbol{q} \\
\end{bmatrix}
=
\begin{bmatrix}
\phi \\
x \\
y \\
z \\
\end{bmatrix}
\tag{6}
$\phi$はエンドエフェクタの姿勢、$x$、$y$、$z$はエンドエフェクタの位置を表しています。(4)(5)式から必要な要素を抽出し、(6)式に代入すると以下のベクトルが得られます。ある意味(7)式も順運動学の計算結果と言ってよいでしょう。
\boldsymbol{r}
=
\boldsymbol{r}(\theta_1, \theta_2, \theta_3, \theta_4)
=
\begin{bmatrix}
\theta_1 + \theta_3 \\
(L + \theta_2) \cos{\theta_1} \\
(L + \theta_2) \sin{\theta_1} \\
H - B + \theta_4 \\
\end{bmatrix}
\tag{7}
順運動学を使うと関節角度の情報からエンドエフェクタの位置・姿勢$\boldsymbol{r}$が計算できます。では、エンドエフェクタの時間変化$\frac{d \boldsymbol{r}}{dt}$はどうなるのでしょうか？実はこれを与える線形写像(計算)がヤコビアンであり、次章以降で解説していきます。
3. 円筒座標ロボットアームのヤコビアン
本章では順運動学の時間微分を考えることで図1のロボットのヤコビアンを導出します。(6)式の右辺は各関節角度$\theta_1$～$\theta_4$の関数であるとともに時間$t$の関数であるとします。少し見づらいですが、しっかり表記すると以下のようになります。
\boldsymbol{r}
=
\begin{bmatrix}
\phi \big( \theta_1(t), \theta_2(t), \theta_3(t), \theta_4(t) \big) \\
x \big( \theta_1(t), \theta_2(t), \theta_3(t), \theta_4(t) \big) \\
y \big( \theta_1(t), \theta_2(t), \theta_3(t), \theta_4(t) \big) \\
z \big( \theta_1(t), \theta_2(t), \theta_3(t), \theta_4(t) \big) \\
\end{bmatrix}
\tag{8}
ベクトル$\boldsymbol{r}$の時間変化を考えるために(8)式の構成要素の全微分を考えてみましょう。チェインルールにより以下の4式がなりたちます。似たような式がたくさん登場しますが、分かりやすいように全て書き出してみます。
d \phi
=
\frac{\partial \phi}{\partial \theta_1} d \theta_1
+
\frac{\partial \phi}{\partial \theta_2} d \theta_2
+
\frac{\partial \phi}{\partial \theta_3} d \theta_3
+
\frac{\partial \phi}{\partial \theta_4} d \theta_4
\tag{9}
d x
=
\frac{\partial x}{\partial \theta_1} d \theta_1
+
\frac{\partial x}{\partial \theta_2} d \theta_2
+
\frac{\partial x}{\partial \theta_3} d \theta_3
+
\frac{\partial x}{\partial \theta_4} d \theta_4
\tag{10}
d y
=
\frac{\partial y}{\partial \theta_1} d \theta_1
+
\frac{\partial y}{\partial \theta_2} d \theta_2
+
\frac{\partial y}{\partial \theta_3} d \theta_3
+
\frac{\partial y}{\partial \theta_4} d \theta_4
\tag{11}
d z
=
\frac{\partial z}{\partial \theta_1} d \theta_1
+
\frac{\partial z}{\partial \theta_2} d \theta_2
+
\frac{\partial z}{\partial \theta_3} d \theta_3
+
\frac{\partial z}{\partial \theta_4} d \theta_4
\tag{12}
さらにチェインルールにより(9)～(12)式について$t$の微分を考えると以下の関係が得られます。ただし、$\dot{w} = \frac{dw}{dt}$を意味しています。
\frac{d \phi}{dt}
=
\frac{\partial \phi}{\partial \theta_1} \frac{d \theta_1}{dt}
+
\frac{\partial \phi}{\partial \theta_2} \frac{d \theta_2}{dt}
+
\frac{\partial \phi}{\partial \theta_3} \frac{d \theta_3}{dt}
+
\frac{\partial \phi}{\partial \theta_4} \frac{d \theta_4}{dt}
\therefore \ \
\dot{\phi}
=
\frac{\partial \phi}{\partial \theta_1} \dot{\theta_1}
+
\frac{\partial \phi}{\partial \theta_2} \dot{\theta_2}
+
\frac{\partial \phi}{\partial \theta_3} \dot{\theta_3}
+
\frac{\partial \phi}{\partial \theta_4} \dot{\theta_4}
\tag{13}
\frac{d x}{dt}
=
\frac{\partial x}{\partial \theta_1} \frac{d \theta_1}{dt}
+
\frac{\partial x}{\partial \theta_2} \frac{d \theta_2}{dt}
+
\frac{\partial x}{\partial \theta_3} \frac{d \theta_3}{dt}
+
\frac{\partial x}{\partial \theta_4} \frac{d \theta_4}{dt}
\therefore \ \
\dot{x}
=
\frac{\partial x}{\partial \theta_1} \dot{\theta_1}
+
\frac{\partial x}{\partial \theta_2} \dot{\theta_2}
+
\frac{\partial x}{\partial \theta_3} \dot{\theta_3}
+
\frac{\partial x}{\partial \theta_4} \dot{\theta_4}
\tag{14}
\frac{d y}{dt}
=
\frac{\partial y}{\partial \theta_1} \frac{d \theta_1}{dt}
+
\frac{\partial y}{\partial \theta_2} \frac{d \theta_2}{dt}
+
\frac{\partial y}{\partial \theta_3} \frac{d \theta_3}{dt}
+
\frac{\partial y}{\partial \theta_4} \frac{d \theta_4}{dt}
\therefore \ \
\dot{y}
=
\frac{\partial y}{\partial \theta_1} \dot{\theta_1}
+
\frac{\partial y}{\partial \theta_2} \dot{\theta_2}
+
\frac{\partial y}{\partial \theta_3} \dot{\theta_3}
+
\frac{\partial y}{\partial \theta_4} \dot{\theta_4}
\tag{15}
\frac{d z}{dt}
=
\frac{\partial z}{\partial \theta_1} \frac{d \theta_1}{dt}
+
\frac{\partial z}{\partial \theta_2} \frac{d \theta_2}{dt}
+
\frac{\partial z}{\partial \theta_3} \frac{d \theta_3}{dt}
+
\frac{\partial z}{\partial \theta_4} \frac{d \theta_4}{dt}
\therefore \ \
\dot{z}
=
\frac{\partial z}{\partial \theta_1} \dot{\theta_1}
+
\frac{\partial z}{\partial \theta_2} \dot{\theta_2}
+
\frac{\partial z}{\partial \theta_3} \dot{\theta_3}
+
\frac{\partial z}{\partial \theta_4} \dot{\theta_4}
\tag{16}
とても長い式が続いてみづらいので、(13)～(16)式を1つの行列演算にまとめたいと思います。
\begin{bmatrix}
\dot{\phi} \\
\dot{x} \\
\dot{y} \\
\dot{z} \\
\end{bmatrix}
=
\begin{bmatrix}
\frac{\partial \phi}{\partial \theta_1} & \frac{\partial \phi}{\partial \theta_2} & \frac{\partial \phi}{\partial \theta_3} & \frac{\partial \phi}{\partial \theta_4} \\
\frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} & \frac{\partial x}{\partial \theta_3} & \frac{\partial x}{\partial \theta_4} \\
\frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} & \frac{\partial y}{\partial \theta_3} & \frac{\partial y}{\partial \theta_4} \\
\frac{\partial z}{\partial \theta_1} & \frac{\partial z}{\partial \theta_2} & \frac{\partial z}{\partial \theta_3} & \frac{\partial z}{\partial \theta_4} \\
\end{bmatrix}
\begin{bmatrix}
\dot{\theta_1} \\
\dot{\theta_2} \\
\dot{\theta_3} \\
\dot{\theta_4} \\
\end{bmatrix}
\tag{17}
とてもスッキリしましたね。(17)式は各関節の角速度情報$\dot{\theta_1}$～$\dot{\theta_4}$をエンドエフェクタのツイスト(もどき)に写す線形変換を意味しています。そしてその変換行列$J$がヤコビアンと呼ばれているんです。
J
=
\begin{bmatrix}
\frac{\partial \phi}{\partial \theta_1} & \frac{\partial \phi}{\partial \theta_2} & \frac{\partial \phi}{\partial \theta_3} & \frac{\partial \phi}{\partial \theta_4} \\
\frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} & \frac{\partial x}{\partial \theta_3} & \frac{\partial x}{\partial \theta_4} \\
\frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} & \frac{\partial y}{\partial \theta_3} & \frac{\partial y}{\partial \theta_4} \\
\frac{\partial z}{\partial \theta_1} & \frac{\partial z}{\partial \theta_2} & \frac{\partial z}{\partial \theta_3} & \frac{\partial z}{\partial \theta_4} \\
\end{bmatrix}
\tag{18}
ヤコビアンのイメージがなんとなく見えたところで、図2のロボットのヤコビアン$J$を計算してみましょう。やることはシンプルで、(18)式の構成要素を16個、気合で計算するだけです。具体的には(7)式の右辺の偏微分をたくさん計算します。
実際に計算した結果を以下にまとめます。要素が16個もあるので列挙が大変ですが、いちど丁寧に書き下しておきます。
\frac{\partial \phi}{\partial \theta_1} = 1
\tag{19}
\frac{\partial \phi}{\partial \theta_2} = 0
\tag{20}
\frac{\partial \phi}{\partial \theta_3} = 1
\tag{21}
\frac{\partial \phi}{\partial \theta_4} = 0
\tag{22}
\frac{\partial x}{\partial \theta_1} = -(L+\theta_2) \sin{\theta_1}
\tag{23}
\frac{\partial x}{\partial \theta_2} = \cos{\theta_1}
\tag{24}
\frac{\partial x}{\partial \theta_3} = 0
\tag{25}
\frac{\partial x}{\partial \theta_4} = 0
\tag{26}
\frac{\partial y}{\partial \theta_1} = (L + \theta_2) \cos{\theta_1}
\tag{27}
\frac{\partial y}{\partial \theta_2} = \sin{\theta_1}
\tag{28}
\frac{\partial y}{\partial \theta_3} = 0
\tag{29}
\frac{\partial y}{\partial \theta_4} = 0
\tag{30}
\frac{\partial z}{\partial \theta_1} = 0
\tag{31}
\frac{\partial z}{\partial \theta_2} = 0
\tag{32}
\frac{\partial z}{\partial \theta_3} = 0
\tag{33}
\frac{\partial z}{\partial \theta_3} = 1
\tag{34}
(19)～(34)式を行列にまとめると円筒座標ロボットアームのヤコビアンが得られます。
J
=
\begin{bmatrix}
1 & 0 & 1 & 0 \\
-(L + \theta_2) \sin{\theta_1} & \cos{\theta_1} & 0 & 0 \\
(L + \theta_2) \cos{\theta_1} & \sin{\theta_1} & 0 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{35}
上述のとおり、ヤコビアンを使うことで、関節の角速度$\dot{\theta_1}$～$\dot{\theta_4}$からエンドエフェクタの状態の時間変化$\dot{\boldsymbol{r}}$を得ることができます。
一方で、(17)式の操作とは反対の操作を行うことで、エンドエフェクタの状態の時間変化$\dot{\boldsymbol{r}}$を満たすような入力角速度$\dot{\theta_1}$～$\dot{\theta_4}$を計算することもできます(キャチロボでもこの計算を使っていました)。このような操作はヤコビアン$J$が正方行列であれば(17)式の両辺に左から逆行列$J^{-1}$をかければ簡単に求めることができます。ただし、逆行列が存在しないような場合も存在し、そのような状態は特異姿勢と呼ばれます。これについて話し始めると長くなってしまうので、また後ほど別の記事でまとめようと思います。
(17)式の関係を、順運動学の速度バージョンという意味合いをこめて順速度運動学と呼んだりします。また、その逆の操作は順運動学の反対が逆運動学であるのと同様に、逆速度運動学と読んだりします。
4. おわりに
今週の記事では、キャチロボに使用したロボットの順速度運動学を導出してみました。
ヤコビアンを使うと速度運動学を記述するだけではなく、ロボットの動かしやすさ(可操作性)の評価などを行うこともできます。また、ヤコビアンの意味をより抽象的にとらえると、ロボットの動きを表す高次元曲面の接平面どうしを結ぶ写像であると捉えることができます！(これらについては後ほどしっかりまとめます..)
さらに、スクリュー理論を使うと順運動学の微分という形ではなく、スクリューの情報をもとに直接ヤコビアンを導出することも可能です。これはスクリュー理論の非常に強力な一面であり、沼りやすいポイントでもあります。
来週は今週導出した順速度運動学の対にあたる逆速度運動学の導出をしてみたいと考えています。もちろん例題としてキャチロボのロボットの解析も行います。楽しみにしててください～
今週も最後まで読んでいただき、ありがとうございました！(^▽^)