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

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

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

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

These conversions only change the rotation (orientation) part of the pose; the translation part is carried over unchanged. The Zivid transformation matrix expects the translation in millimeters (mm), because Zivid point clouds are in millimeters. Most robot controllers report translation in meters, so multiply the translation by 1000 when converting a robot pose to the Zivid matrix.

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

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

それぞれはどの軸を中心に回転しますか?
これらの軸は固定されていますか、それとも移動しますか?
回転はどの順序で定義されますか (複数の可能性があります)?

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

以下に 2 つの異なる例を示します。

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 番目です。

C++

rollPitchYawListToRotationMatrix(rpyList);

C#

RollPitchYawListToRotationMatrix(rpyList);

Python

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 + q_x + q_y + q_z = 1)\) 。そうでない場合は、変換を行う前に次の式を使用して正規化する必要があります。

\[n = \sqrt{{q_w}^2+{q_c}^2+{q_y}^2+{q_z}^2}\]

\[\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}\]

C++

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

C#

var rotationMatrixFromQuaternion = QuaternionToRotationMatrix(quaternion);
Console.WriteLine("Rotation Matrix from Quaternion:\n" + MatrixToString(rotationMatrixFromQuaternion));

Python

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}\]

C++

const Eigen::Quaternionf quaternion(rotationMatrix);
std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;

C#

var quaternion = RotationMatrixToQuaternion(rotationMatrix);
Console.WriteLine("Quaternion:\n" + MatrixToString(quaternion.Transpose()));

Python

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}\]

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

C++

const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
          << axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;

C#

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

Python

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}\]

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

C++

const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;

C#

var rotationVector = axisAngle.Axis * axisAngle.Angle;
Console.WriteLine("Rotation Vector:\n" + MatrixToString(rotationVector.Transpose()));

Python

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

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

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

C++

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

C#

var rpyList = RotationMatrixToRollPitchYawList(rotationMatrix);

Python

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)

Robot-Specific Pose Conventions

The hardest part of converting a robot pose to a transformation matrix is identifying which orientation representation the robot controller uses. The mathematical conversions above are unambiguous once you know the representation, but robot vendors rarely state it clearly, and the same term (for example roll-pitch-yaw) can mean different rotation orders on different robots.

This section collects the conventions we have seen on specific robots, so you can pick the right conversion above without rediscovering it.

警告

This information is gathered from experience with specific robot models and is not exhaustively tested across firmware versions, models, or vendors. Always verify the result of a conversion before relying on it, for example by comparing the assembled transformation matrix against the pose shown on the robot teach pendant.

Universal Robots

Universal Robots reports the TCP pose as a six-element vector [x, y, z, rx, ry, rz]. The first three elements are the translation, and the last three are the orientation as a rotation vector (axis-angle scaled by the angle, also called the Rodrigues representation), not roll-pitch-yaw. The script and RTDE interfaces (for example get_actual_tcp_pose) report the translation in meters, while the teach pendant displays it in millimeters.

To build a 4x4 transformation matrix:

Ensure the translation is in millimeters, converting from meters if you read the pose from the script or RTDE interface.
Convert the rotation vector to a rotation matrix using the conversions above.
Place the rotation matrix and translation vector into a 4x4 transformation matrix.

Yaskawa

Yaskawa controllers report the pose as a translation followed by three Euler angles (Rx, Ry, Rz) in xyz extrinsic order.

To build a 4x4 transformation matrix:

Ensure the translation is in millimeters.
Convert the Euler angles to a rotation matrix using the Roll-Pitch-Yaw to Rotation Matrix conversion above, with the rotation order from the table.
Place the rotation matrix and translation vector into a 4x4 transformation matrix.

Known Euler-Angle Orders

The following table lists the Euler-angle orders we have seen per robot vendor. The steps are the same as for Yaskawa above; only the Euler-angle order changes from one vendor to the next. Use it as a starting point, but confirm against your own robot, as the order can differ between models and firmware from the same vendor.

Robot vendor	Euler angles order	Notes
Universal Robots	Rotation vector (not Euler)	Axis-angle / Rodrigues
Fanuc	xyz extrinsic	Reported as W-P-R
Yaskawa	xyz extrinsic
Kawasaki	zyz intrinsic
KUKA	zyx intrinsic	Reported as A-B-C
Doosan	zyz intrinsic
ABB	zyx intrinsic	Controller also exposes a quaternion
Hyundai	xyz intrinsic
Robostar	xyz extrinsic
Nachi	zyx intrinsic