このページの一部は機械翻訳であり、まだ人による確認が行われていません。

一般的な方向表現間の変換

この記事では、ロボット工学で一般的に使用される方向表現間の変換方法について説明します。必要な数式と、これらの変換を独自の アプリケーション に組み込む方法を示すコード例も含まれています。

これらの変換をインタラクティブに利用したい場合は、弊社の Pose Conversion GUI ツールをお試しください。

姿勢表現に関するより詳細な説明については、 位置、方向、座標変換 を参照してください。

これらの変換は姿勢の回転 (方向) 部分のみを変更し、並進部分はそのまま引き継がれます。Zivid の点群はミリメートル単位であるため、Zivid 変換行列は並進をミリメートル (mm) 単位で想定しています。ほとんどのロボットコントローラーは並進をメートル単位で報告するため、ロボットの姿勢を Zivid 行列に変換する際は並進を 1000 倍してください。

ロール-ピッチ-ヨーから回転行列へ

ロール-ピッチ-ヨーは方向を表す一般的な用語です。それぞれは 1 つの軸の周りの回転角度を表し、それらを組み合わせると完全な回転を表します。ただし、それらが正確に何を表すかについては明確ではありません。これらは次のような混乱を招く可能性があります。

  • それぞれはどの軸を中心に回転しますか?

  • これらの軸は固定されていますか、それとも移動しますか?

  • 回転はどの順序で定義されますか (複数の可能性があります)?

回転の順序は x-y-z または z-y'-x'' で表されることがよくあります。ここで x 、 y および z は、回転の対象となる軸を示します。 ' は軸が固定されているかどうかを示すために使用されます。固定軸の周りの回転は extrinsic rotation と呼ばれます。移動軸の周りの回転は intrinsic rotation と呼ばれます。

以下の 2 つの例は、外的 x-y-z 順序と外的 z-y-x 順序の変換を示しています。

x-y-z または z-y'-x'' の場合、ロール \(\phi\) 、ピッチ \(\theta\) 、およびヨー \(\psi\) 角度は次のように回転行列 \(R\) に変換できます。

\[\begin{split}\boldsymbol{R}_{x-y-z} = &\boldsymbol{R}_{z-{y}^{'}-{x}^{''}} =\boldsymbol{R}_{z,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{x,\psi} = \\ \begin{bmatrix} cos{\phi} & -sin{\phi} & 0 \\ sin{\phi} & cos{\phi} & 0\\ 0 & 0 & 1\end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\psi} & -sin{\psi} \\ 0 & sin{\psi} & cos{\psi} \end{bmatrix}\end{split}\]

z-y-x または x-y'-z'' の場合、ロール \(\phi\) 、ピッチ \(\theta\) 、およびヨー \(\psi\) 角度は次のように回転行列 \(R\) に変換できます。

\[\begin{split}\boldsymbol{R}_{z-y-x} = &\boldsymbol{R}_{x-{y}^{'}-{z}^{''}} =\boldsymbol{R}_{x,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{z,\psi} = \\ \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\phi} & -sin{\phi} \\ 0 & sin{\phi} & cos{\phi} \end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} cos{\psi} & -sin{\psi} & 0 \\ sin{\psi} & cos{\psi} & 0\\ 0 & 0 & 1\end{bmatrix}\end{split}\]

注釈

2 つの例で角度が同じであると仮定すると、それらは同じ最終的な回転行列を表しません。

最終的な回転行列が同じであると仮定すると、2 つの例の間で角度は同一ではありません。

定義が導入されています。ロール角は移動軸を中心とした最初の回転に割り当てられ、ピッチは 2 番目、ヨーは 3 番目です。

ソースに移動

ソース

rollPitchYawListToRotationMatrix(rpyList);
ソースに移動

ソース

RollPitchYawListToRotationMatrix(rpyList);
ソースに移動

ソース

roll_pitch_yaw_to_rotation_matrix(rpy_list)

回転ベクトルから軸角度へ

回転ベクトル \(\boldsymbol{r}\) は、次のように軸 \(\boldsymbol{u}\) と角度 \(\theta\) に変換できます。

\[\theta = \sqrt{{r_x}^2+{r_y}^2+{r_z}^2}\]
\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{r_x}{\theta} & \frac{r_y}{\theta} & \frac{r_z}{\theta} \\ \end{bmatrix}\end{split}\]

軸角度から四元数へ

軸 \(\boldsymbol{u}\) と角度 \(\theta\) は、次のように単位四元数 \(\boldsymbol{q}\) に変換できます。

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \cos{\frac{\theta}{2}} & u_x \sin{\frac{\theta}{2}} & u_y \sin{\frac{\theta}{2}} & u_z \sin{\frac{\theta}{2}} \\ \end{bmatrix}\end{split}\]

クォータニオンから回転行列へ

単位四元数 \(\boldsymbol{q}\) は、次のように回転行列 \(\boldsymbol{R}\) に変換できます。

\[\begin{split}\boldsymbol{R} = \begin{bmatrix} 1 - 2 {q_{y}}^2 - 2 {q_{z}}^2 & 2 q_{x} q_{y} - 2 q_{z} q_{w} & 2 q_{x} q_{z} - 2 q_{y} q_{w} \\ 2 q_{x} q_{y} - 2 q_{z} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{z}}^2 & 2 q_{y} q_{z} - 2 q_{x} q_{w} \\ 2 q_{x} q_{z} - 2 q_{y} q_{w} & 2 q_{y} q_{z} - 2 q_{x} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{y}}^2\\ \end{bmatrix}\end{split}\]

クォータニオンが正規化されている \(({q_w}^2 + {q_x}^2 + {q_y}^2 + {q_z}^2 = 1)\) ことを前提としています。そうでない場合は、変換を行う前に次の式を使用して正規化する必要があります。

\[\begin{split}n = \\sqrt{{q_w}^2+{q_x}^2+{q_y}^2+{q_z}^2}\end{split}\]
\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \frac{q_w}{n} & \frac{q_x }{n} & \frac{q_y }{n} & \frac{q_z }{ n}\\ \end{bmatrix}\end{split}\]
ソースに移動

ソース

const Eigen::Matrix3f rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
std::cout << "Rotation Matrix from Quaternion:\n"
          << rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;
ソースに移動

ソース

var rotationMatrixFromQuaternion = QuaternionToRotationMatrix(quaternion);
Console.WriteLine("Rotation Matrix from Quaternion:\n" + MatrixToString(rotationMatrixFromQuaternion));
ソースに移動

ソース

rotation_matrix_from_quaternion = quaternion_to_rotation_matrix(quaternion)
print(f"Rotation Matrix from Quaternion:\n{rotation_matrix_from_quaternion}")

回転行列から四元数へ

回転行列 \(\boldsymbol{R}\) は、次のように単位四元数 \(\boldsymbol{q}\) に変換できます。

\[q_w = \frac{\sqrt{1 + r_{11} + r_{22}+ r_{33}}}{2}\]
\[\begin{split}\boldsymbol{q} = \begin{bmatrix} q_w & \frac{r_{32} - r_{23}}{4 q_w} & \frac{r_{13} - r_{31}}{4 q_w} & \frac{r_{21} - r_{12}}{4 q_w} \\ \end{bmatrix}\end{split}\]
ソースに移動

ソース

const Eigen::Quaternionf quaternion(rotationMatrix);
std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;
ソースに移動

ソース

var quaternion = RotationMatrixToQuaternion(rotationMatrix);
Console.WriteLine("Quaternion:\n" + MatrixToString(quaternion.Transpose()));
ソースに移動

ソース

quaternion = rotation_matrix_to_quaternion(rotation_matrix)
print(f"Quaternion:\n{quaternion}")

クォータニオンから軸角度へ

単位四元数 \(\boldsymbol{q}\) は、次のように軸 \(\boldsymbol{u}\) と角度 \(\theta\) に変換できます。

\[\theta = 2 \cos^{-1}{q_w}\]
\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{q_x}{\sqrt{1-{q_w}^2}} & \frac{q_y}{\sqrt{1-{q_w}^2}} & \frac{q_z}{\sqrt{1-{q_w}^2}} \\ \end{bmatrix}\end{split}\]

これは、回転行列から軸角度への変換に役立ちます。以下のコードサンプル実装を参照してください。

ソースに移動

ソース

const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
          << axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;
ソースに移動

ソース

var axisAngle = RotationMatrixToAxisAngle(rotationMatrix);
Console.WriteLine("AxisAngle:\n" + MatrixToString(axisAngle.Axis.Transpose()) + ", " + String.Format(" {0:G4} ", axisAngle.Angle));
ソースに移動

ソース

axis_angle = rotation_matrix_to_axis_angle(rotation_matrix)
print(f"AxisAngle:\n{axis_angle.axis}, {axis_angle.angle:.4f}")

軸角度から回転ベクトルへ

軸 \(\boldsymbol{u}\) と角度 \(\theta\) は、次のように回転ベクトル \(\boldsymbol{r}\) に変換できます。

\[\begin{split}\boldsymbol{r} = \begin{bmatrix} u_x \theta & u_y \theta & u_z \theta \\ \end{bmatrix}\end{split}\]

これは、回転行列を回転ベクトルに変換する場合に便利です。以下のコードサンプル実装を参照してください。

ソースに移動

ソース

const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;
ソースに移動

ソース

var rotationVector = axisAngle.Axis * axisAngle.Angle;
Console.WriteLine("Rotation Vector:\n" + MatrixToString(rotationVector.Transpose()));
ソースに移動

ソース

rotation_vector = rotation_matrix_to_rotation_vector(rotation_matrix)
print(f"Rotation Vector:\n{rotation_vector}")

回転行列からロール-ピッチ-ヨーへ

回転行列からロール、ピッチ、ヨー角を決定するのは簡単ではありません。解は複数存在する場合があり、場合によっては無限に存在することもあります。これには、いくつかの基準に基づいて複数の解の中から 1 つを選択できるアルゴリズムが必要です。

ソースに移動

ソース

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);
ソースに移動

ソース

var rpyList = RotationMatrixToRollPitchYawList(rotationMatrix);
ソースに移動

ソース

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)

ロボット固有の姿勢規則

ロボットの姿勢を変換行列に変換する際に最も難しいのは、ロボットコントローラーがどの方向表現を使用しているかを特定することです。表現さえわかれば上記の数学的変換は明確ですが、ロボットベンダーがそれを明確に示すことはほとんどなく、同じ用語 (例えば roll-pitch-yaw) であってもロボットによって異なる回転順序を意味することがあります。

このセクションでは、特定のロボットで確認された規則をまとめています。これにより、同じことを再度調べ直すことなく、上記から適切な変換を選択できます。

警告

この情報は特定のロボットモデルでの経験に基づいて収集されたものであり、ファームウェアバージョン、モデル、ベンダーにわたって網羅的にテストされたものではありません。変換結果を利用する前に、例えば組み立てた変換行列とロボットのティーチペンダントに表示される姿勢を比較するなどして、必ず検証してください。

Universal Robots

Universal Robots は TCP 姿勢を 6 要素のベクトル [x, y, z, rx, ry, rz] として報告します。最初の 3 要素は並進であり、残りの 3 要素は roll-pitch-yaw ではなく rotation vector (角度でスケーリングされた軸角度、Rodrigues 表現とも呼ばれる) としての方向です。スクリプトインターフェースと RTDE インターフェース (例えば get_actual_tcp_pose) は並進をメートル単位で報告しますが、ティーチペンダントはミリメートル単位で表示します。

4x4 変換行列を構築するには:

  1. 並進がミリメートル単位であることを確認します。スクリプトインターフェースまたは RTDE インターフェースから姿勢を読み取った場合は、メートルから変換してください。

  2. 回転ベクトルを回転行列に変換します: 回転ベクトルから軸角度へ 、次に 軸角度から四元数へ 、そして クォータニオンから回転行列へ の順に変換します。

  3. 回転行列と並進ベクトルを 4x4 変換行列に配置します。

Yaskawa

Yaskawa のコントローラーは、姿勢を並進に続けて 3 つのオイラー角 (Rx 、 Ry 、 Rz) として、 xyz 外的順序で報告します。

4x4 変換行列を構築するには:

  1. 並進がミリメートル単位であることを確認します。

  2. 表の回転順序を使用して、 ロール-ピッチ-ヨーから回転行列へ の変換を用いてオイラー角を回転行列に変換します。

  3. 回転行列と並進ベクトルを 4x4 変換行列に配置します。

既知のオイラー角順序

次の表は、ロボットベンダーごとに確認されたオイラー角順序を示しています。手順は上記の Yaskawa の場合と同じで、ベンダーごとに異なるのはオイラー角順序のみです。これを出発点として使用しつつ、同じベンダーでもモデルやファームウェアによって順序が異なる場合があるため、必ずお使いのロボットで確認してください。

ロボットベンダー

オイラー角順序

備考

Universal Robots

回転ベクトル (オイラーではない)

軸角度 / Rodrigues

Fanuc

xyz 外的

W-P-R として報告されます

Yaskawa

xyz 外的

Kawasaki

zyz 内的

KUKA

zyx 内的

A-B-C として報告されます

Doosan

zyz 内的

ABB

zyx 内的

コントローラーはクォータニオンも提供します

Hyundai

xyz 内的

Robostar

xyz 外的

Nachi

zyx 内的