姿勢制御
本ページではOMRON NX/NJシリーズ向けの姿勢制御ライブラリについて解説します。
オイラー角、クォータニオン、回転行列を利用した3次元回転計算や座標変換の考え方を紹介しています。
姿勢制御とは
姿勢制御とは物体の向きを制御する技術です。
ロボットアーム、AGV、ドローン、産業機械などで利用されます。
3次元空間での姿勢表現にはオイラー角、回転行列、クォータニオンなどが利用されます。
オイラー角での姿勢の計算
オイラー角のメリット
- 理解しやすい
- 人が角度を扱いやすい
- PLCでも実装しやすい
オイラー角のデメリット
- ジンバルロックが発生する
- 補間処理が難しい
回転の例
オイラー角 X軸=50度、Y軸=50度、Z軸=50度
物体座標系でXYZ型で回転する例
この動画は計算の手順を説明するもので実際の計算は最初の位置から最後の位置（赤）を直接算出します
オイラー角
紙飛行機の先端方向をX軸として翼方向をY軸、垂直翼方向をZ軸として考えます
姿勢を変えるにあたり、XYZそれぞれの軸を回転して姿勢を変えるとして、X,Y,Zの回転角度をまとめてオイラー角と呼びます
オイラー角の型
オイラー角は回転する順番によって12種類の型があり、回転する順番が変わると算出結果も変わります
| X Y Z | X Y X |
| X Z Y | X Z X |
| Y X Z | Y X Y |
| Y Z X | Y Z Y |
| Z X Y | Z X Z |
| Z Y X | Z Y Z |
物体座標系と固定座標系
物体座標系は紙飛行機の先端が常にX軸方向というように物体の座標系で計算し、固定座標系は座標軸周りで計算します
このライブラリでは物体座標系はEulerToMatrix, matRotate3Dを使って、固定座標系ではmatRotateXYZで計算できると思います
注意する点
オイラー角での変換はジンバルロックが発生することがあります
ジンバルロックとは座標変換の際に２つの回転軸が同じ向きになってしまい３軸が２軸になってしまい無効になった１軸が回転できなくなる現象のことです
これを回避するために回転する順場を変える事で回避することになります
紙飛行機について
紙飛行機は５点の回転の計算をして図形を回転しています
ちなみに
この動画は説明用にPythonで作成したグラフを撮影したものです
クォータニオンでの姿勢の計算
クォータニオンのメリット
- ジンバルロックが発生しない
- 回転補間が容易
- 高速に計算できる
- ロボット制御で広く利用される
クォータニオン
回転軸（３次元ベクトル）と回転角からなる４つの成分で回転を表現します
オイラー角とは違いジンバルロックが発生することはありません
また、クォータニオンは計算がシンプルなので計算コストが削減され高速に計算することができます
考え方
現在の姿勢から目的の姿勢にするためには回転する軸と回転角度を決定すれば目的の姿勢に回転することができるというものです
目的の姿勢がどんな向きでも可能です
１点の回転を考えると次の図のようになります
紙飛行機の５点を回転軸を中心にそれぞれ回転すると以下のようになります
回転軸と回転角度の求め方
初期位置のクォータニオン
\[
Q1=
\begin{bmatrix}
q_w \\
q_x \\
q_y \\
q_z \\
\end{bmatrix}
=
\begin{bmatrix}
1 \\
point.x \\
point.y \\
point.z \\
\end{bmatrix}
\]
目標位置のオイラー角をクォータニオンに変換
\[
Q2=EulerToQuaternion(th, order)
\]
回転軸（単位ベクトル）
\[
\begin{align}
V=&arcsin(Q2.img)\times2 \\
T=&\frac{V}{norm(V)}
\end{align}
\]
回転を表すクォータニオン
\[
\begin{align}
R.real=&cos\frac{Q2.real}{2} \tag1 \\
R.imag=&T\times sin\frac{Q2.real}{2}\tag2 \\
\end{align}
\]
回転の計算
\[
out = R \times Q1 \times \bar{R}
\]
(1)・・・回転角度
(2)・・・回転軸
回転軸と回転角度の関係
クォータニオンは回転軸と回転角度を4つの値で表現したものです。
よくある質問
オイラー角とクォータニオンの違いは？
オイラー角は角度で表現し、クォータニオンは回転軸と回転量で表現します。
ジンバルロックとは？
オイラー角で発生する自由度の消失現象です。
産業用ロボットではどちらが使われますか？
内部計算ではクォータニオンや回転行列が利用されることが多いです。