スクリュー理論㉒円筒座標ロボットアームの順運動学

Last updated at 2025-09-20Posted at 2025-09-20

　はじめまして、りょーつといいます。高専出身の大学院2年生です。研究の専門は力学や機構学で、Qiitaでは主に制御工学や数学に関する記事を書いています。本稿はスクリュー理論の解説(布教)をする22個目の記事です。前回までの記事は以下のリンクを参照してください。

　スクリュー理論は剛体の運動を記述する方法で、ロボット工学などでよく使われています。日本の高専や大学ではDH法を使った記述を学ぶことが多いですが、国際的にはスクリュー理論を使った記述が一般的となってきているようです。スクリュー理論を扱う日本語の文献は少ないので、この記事が誰かの助けになればいいなと思います。

1. はじめに

　今週の記事では、スクリュー理論を使って円筒座標型ロボットアームの順運動学を計算してみようと思います。前回の記事で一般のシリアルリンクロボットアームの順運動学について紹介しましたが、今週はその具体例として円筒座標型ロボットアームの順運動学を計算します。同次変換行列を行列指数で表す操作をたくさん使うため、それらを身に着けているとスムーズです。

2. 円筒座標ロボットアームのスクリューの設定

　今回計算に用いるのは4自由度の円筒座標ロボットアームです。キャチロボバトルコンテスト2025に出場する予定のロボットを使用します。ロボットの外観を図1に貼っておきます。

図1　キャチロボバトルコンテスト出場【ぷりん】

　第1関節はYaw軸まわりの回転関節、第2関節は動径方向へ伸縮する直動関節、第3関節もYaw軸まわりの回転関節、第4関節は鉛直方向へ伸縮する直動関節となります。関節の配置について図2に示します。また主要な寸法(変数)については図3に示します。

図2　関節の配置

図3　主要な寸法と座標系の定義

　各関節に関するスクリューを第1関節から順に$\boldsymbol{\zeta}_1$、$\boldsymbol{\zeta}_2$、$\boldsymbol{\zeta}_3$、$\boldsymbol{\zeta}_4$とします。スクリューは回転軸$\boldsymbol{\hat{\omega}}$と並進速度$\boldsymbol{v}$によって構成されています。

\boldsymbol{\zeta}_k
=
\begin{bmatrix}
\boldsymbol{\hat{\omega}}_k \\
\boldsymbol{v}_k \\
\end{bmatrix}
\ \ \ \ , \ k = 1,2,3,4 
\tag{1}

　第1関節の中心を原点として固定座標系$\Sigma_s$を考えます。固定座標系$\Sigma_s$のX軸をエンドエフェクタの方向、Z軸を鉛直上向きとします。

　第1関節と第3関節の回転軸は共にZ軸まわりの回転関節であるため、以下がなりたちます。

\boldsymbol{\hat{\omega}}_1
=
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{2}

\boldsymbol{\hat{\omega}}_3
=
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{3}

また、第2関節と第4関節は共に直動関節であるため、以下がなりたちます。

\boldsymbol{\hat{\omega}}_2
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{4}

\boldsymbol{\hat{\omega}}_4
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{5}

次に並進速度を考えます。直動関節の場合はシンプルで、直動で移動する方向の単位ベクトルを考えればよいです。

\boldsymbol{v}_2
=
\begin{bmatrix}
1 \\
0 \\
0 \\
\end{bmatrix}
\tag{6}

\boldsymbol{v}_4
=
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{7}

次に第1、第3関節の速度について考えていきましょう。各回転軸の位置ベクトルを$\boldsymbol{r}_1$、$\boldsymbol{r}_3$とすると、それらは以下のように表されます。

\boldsymbol{r}_1
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{8}

\boldsymbol{r}_3
=
\begin{bmatrix}
L \\
0 \\
0 \\
\end{bmatrix}
\tag{9}

　並進速度$\boldsymbol{r}_1$、$\boldsymbol{r}_3$は、各関節の回転によって原点に生じる速度を意味するので、以下のように計算できます。

\boldsymbol{v}_1
=
\boldsymbol{\hat{\omega}}_1
\times
(-\boldsymbol{r}_1)
=
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
\times
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{10}

\boldsymbol{v}_3
=
\boldsymbol{\hat{\omega}}_3
\times
(-\boldsymbol{r}_3)
=
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
\times
\begin{bmatrix}
-L \\
0 \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
-L \\
0 \\
\end{bmatrix}
\tag{11}

以上のことから、円筒座標ロボットアームのスクリューが以下のようにもとまりました。

\boldsymbol{\zeta}_1
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{12}

\boldsymbol{\zeta}_2
=
\begin{bmatrix}
0 \\
0 \\
0 \\
1 \\
0 \\
0 \\
\end{bmatrix}
\tag{13}

\boldsymbol{\zeta}_3
=
\begin{bmatrix}
0 \\
0 \\
1 \\
0 \\
-L \\
0 \\
\end{bmatrix}
\tag{14}

\boldsymbol{\zeta}_4
=
\begin{bmatrix}
0 \\
0 \\
0 \\
0 \\
0 \\
1 \\
\end{bmatrix}
\tag{15}

3. 円筒座標ロボットアームの順運動学

　順運動学をゴリゴリ計算する前に、スクリューと行列指数をつかって同次変換行列を表す方法についておさらいします。スクリュー$\boldsymbol{\zeta}_k$と角度$\theta_k$に対応する同次変換行列$e^{[\boldsymbol{\zeta}_k] \theta_k}$は以下のような形式をとります。

e^{[\boldsymbol{\zeta}_k] \theta_k}
=
\begin{bmatrix}
e^{[\boldsymbol{\hat{\omega}}_k] \theta_k} & G_k(\theta_k) \boldsymbol{v}_k\\
\boldsymbol{0}^T & 1 \\
\end{bmatrix}
\tag{16}

関数$G_k(\theta_k)$は、回転行列$e^{[\boldsymbol{\hat{\omega}}_k] \theta_k}$を定積分した関数として以下のように定義されます。

G_k(\theta_k)
=
\int_{b}^{a}
e^{[\boldsymbol{\zeta}_k] \theta_k} d \theta_k
\tag{17}

順番に丁寧に計算してみましょう。まずは回転行列$e^{[\boldsymbol{\zeta}_k] \theta_k}$からです。第1、第3関節である、Yaw軸周りの回転行列はよく知られた形式であり、以下のように記述できます。

e^{[\boldsymbol{\hat{\omega}}_1] \theta_1}
=
\begin{bmatrix}
\cos{\theta_1} & -\sin{\theta_1} & 0 \\
\sin{\theta_1} & \cos{\theta_1} & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\tag{18}

e^{[\boldsymbol{\hat{\omega}}_3] \theta_3}
=
\begin{bmatrix}
\cos{\theta_3} & -\sin{\theta_3} & 0 \\
\sin{\theta_3} & \cos{\theta_3} & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\tag{19}

また、第2、第4関節である直動関節は回転を伴わないため、回転行列は単位行列になります。

e^{[\boldsymbol{\hat{\omega}}_2] \theta_2}
=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\tag{20}

e^{[\boldsymbol{\hat{\omega}}_4] \theta_4}
=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\tag{21}

次に$G_k(\theta_k)$を求めていきましょう。シンプルに(18)～(21)式を1回定積分すればよいだけです。

G_1(\theta_1)
=
\begin{bmatrix}
\sin{\theta_1} & -(1 - \cos{\theta_1}) & 0 \\
1 - \cos{\theta_1} & \sin{\theta_1} & 0 \\
0 & 0 & \theta_1 \\
\end{bmatrix}
\tag{22}

G_2(\theta_2)
=
\begin{bmatrix}
\theta_2 & 0 & 0 \\
0 & \theta_2 & 0 \\
0 & 0 & \theta_2 \\
\end{bmatrix}
\tag{23}

G_3(\theta_3)
=
\begin{bmatrix}
\sin{\theta_3} & -(1 - \cos{\theta_3}) & 0 \\
1 - \cos{\theta_3} & \sin{\theta_3} & 0 \\
0 & 0 & \theta_3 \\
\end{bmatrix}
\tag{24}

G_4(\theta_4)
=
\begin{bmatrix}
\theta_4 & 0 & 0 \\
0 & \theta_4 & 0 \\
0 & 0 & \theta_4 \\
\end{bmatrix}
\tag{25}

最後に(22)～(25)式の行列$G_k(\theta_k)$と(6)(7)(10)(11)式の速度$\boldsymbol{v}_k$の積を計算しておきましょう。

G_1(\theta_1) \boldsymbol{v}_1
=
\begin{bmatrix}
\sin{\theta_1} & -(1 - \cos{\theta_1}) & 0 \\
1 - \cos{\theta_1} & \sin{\theta_1} & 0 \\
0 & 0 & \theta_1 \\
\end{bmatrix}
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
0 \\
\end{bmatrix}
\tag{26}

G_2(\theta_2) \boldsymbol{v}_2
=
\begin{bmatrix}
\theta_2 & 0 & 0 \\
0 & \theta_2 & 0 \\
0 & 0 & \theta_2 \\
\end{bmatrix}
\begin{bmatrix}
1 \\
0 \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
\theta_2 \\
0 \\
0 \\
\end{bmatrix}
\tag{27}

G_3(\theta_3) \boldsymbol{v}_3
=
\begin{bmatrix}
\sin{\theta_3} & -(1 - \cos{\theta_3}) & 0 \\
1 - \cos{\theta_3} & \sin{\theta_3} & 0 \\
0 & 0 & \theta_3 \\
\end{bmatrix}
\begin{bmatrix}
0 \\
-L \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
L(1 - \cos{\theta_3}) \\
-L \sin{\theta_3}\\
0 \\
\end{bmatrix}
\tag{28}

G_4(\theta_4) \boldsymbol{v}_4
=
\begin{bmatrix}
\theta_4 & 0 & 0 \\
0 & \theta_4 & 0 \\
0 & 0 & \theta_4 \\
\end{bmatrix}
\begin{bmatrix}
0 \\
0 \\
1 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
\theta_4 \\
\end{bmatrix}
\tag{29}

以上の計算をまとめると、同次変換行列が以下のように求まります。くどいですが、しっかり成分表示しておきます。

e^{[\boldsymbol{\zeta}_1] \theta_1}
=
\begin{bmatrix}
\cos{\theta_1} & -\sin{\theta_1} & 0 & 0 \\
\sin{\theta_1} & \cos{\theta_1} & 0  & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{30}

e^{[\boldsymbol{\zeta}_2] \theta_2}
=
\begin{bmatrix}
1 & 0 & 0 & \theta_2 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{31}

e^{[\boldsymbol{\zeta}_3] \theta_3}
=
\begin{bmatrix}
\cos{\theta_3} & -\sin{\theta_3} & 0 & L(1 - \cos{\theta_3}) \\
\sin{\theta_3} & \cos{\theta_3} & 0  & -L \sin{\theta_3} \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{32}

e^{[\boldsymbol{\zeta}_4] \theta_4}
=
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & \theta_4 \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{33}

最後にエンドエフェクタの初期コンフィギュレーション$M$を設定しておきます。エンドエフェクタの姿勢は静止座標系$\Sigma_s$と同じであるとすれば、

M
=
\begin{bmatrix}
1 & 0 & 0 & L \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & H - B \\
0 & 0 & 0 & 1 \\
\end{bmatrix}
\tag{34}

となります。

　準備が整ったので順運動学をゴリゴリ計算してみましょう。前回の記事とは異なり、根元側の関節を第1関節とした点に注意してください。関節変形後のコンフィギュレーションを$M'$とします。

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
\tag{35}

(35)式に(30)～(34)式の同次変換行列を代入してゴリゴリ掛け算をして整理することで以下の計算結果が得られます。

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{36}

これで順運動学が計算できました！よく見る形式の式が出てきたので、直観的にも合ってそうですね。

4. おわりに

　今週はキャチロボ本番ということで、キャチロボに使用するロボットの順運動学を計算しました。普段は抽象的な内容の記事が多かったので、今週の記事はイメージをつかみやすかった&使い方が分かりやすかったのではないでしょうか？大会では順運動学を微分したようなものである速度運動学を使っているのでそこまで書ききりたかったのですが、週に1回の更新ペースでは間に合いませんでした...大会後もスクリュー理論やキャチロボの機体に関する記事を毎週土曜日に更新予定です。
　今週も最後まで読んでいただきありがとうございました！！

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up

スクリュー理論㉒ 円筒座標ロボットアームの順運動学

目次

1. はじめに

2. 円筒座標ロボットアームのスクリューの設定

3. 円筒座標ロボットアームの順運動学

4. おわりに

スクリュー理論㉒円筒座標ロボットアームの順運動学