Conversions Between Common Orientation Representations

이 문서에서는 로봇 공학에서 흔히 사용되는 방향 표현 방식 간의 변환 방법을 설명합니다. 필요한 수학 공식과 함께 이러한 변환을 애플리케이션에 통합하는 방법을 보여주는 코드 예제도 포함되어 있습니다.

이러한 변환 기능을 대화형으로 활용하고 싶다면, 저희의 Pose Conversion GUI 도구를 사용해 보세요.

포즈 표현에 대한 더 자세한 설명은 Position, Orientation and Coordinate Transformations 을 참조하세요.

이러한 변환은 포즈의 회전(방향) 부분만 변경하며, 이동(translation) 부분은 변경 없이 그대로 유지됩니다. Zivid 포인트 클라우드가 밀리미터 단위이기 때문에 Zivid 변환 행렬은 이동 값을 밀리미터(mm) 단위로 요구합니다. 대부분의 로봇 컨트롤러는 이동 값을 미터 단위로 보고하므로, 로봇 포즈를 Zivid 행렬로 변환할 때는 이동 값에 1000을 곱해야 합니다.

Roll-Pitch-Yaw to Rotation Matrix

Roll-pitch-yaw는 방향을 나타내는 일반적인 용어입니다. 각각은 단일 축을 중심으로 한 회전 각도를 나타내며 결합하여 완전한 회전을 나타냅니다. 그러나 정확히 무엇을 나타내는지는 명확하지 않습니다. 다음과 같은 혼란을 겪을 수 있습니다.

각각 어떤 축을 중심으로 회전합니까?
이 축은 고정되어 있습니까, 아니면 이동 중입니까?
회전은 어떤 순서로 정의됩니까(여러 가지 가능성이 있음)?

회전 순서는 종종 x-y-z 또는 z-y'-x'' 로 표시됩니다. 여기서 x, y 및 z 는 회전하는 축을 나타냅니다. ' 는 축의 고정 여부를 나타내는 데 사용됩니다. 고정 축을 중심으로 하는 회전을 extrinsic rotation 이라고 합니다. 움직이는 축을 중심으로 회전하는 것을 intrinsic rotation 이라고 합니다.

아래 두 예제는 외재적(extrinsic) 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}\]

참고

두 예에서 각도가 동일하다고 가정하면 동일한 최종 회전 행렬을 나타내지 않습니다.

최종 회전 행렬이 동일하다고 가정하면 두 예제 간에 각도가 동일하지 않습니다.

정의가 도입되었습니다. 롤 각도는 움직이는 축에 대한 첫 번째 회전에 할당되고, 피치는 두 번째, 요는 세 번째 회전에 할당됩니다.

C++

rollPitchYawListToRotationMatrix(rpyList);

C#

RollPitchYawListToRotationMatrix(rpyList);

Python

roll_pitch_yaw_to_rotation_matrix(rpy_list)

Rotation Vector to Axis-Angle

회전 벡터 \(\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}\]

Axis-Angle to Quaternion

축 \(\boldsymbol{u}\) 과 각도 \(\theta\) 는 다음과 같이 unit quaternion \(\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}\]

Quaternion to Rotation Matrix

unit quaternion \(\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}\]

quaternion이 \(({q_w}^2 + {q_x}^2 + {q_y}^2 + {q_z}^2 = 1)\) 로 정규화되어 있다고 가정합니다. 그렇지 않은 경우, 다음 방정식을 사용하여 변환을 수행하기 전에 정규화해야 합니다.

\[n = \sqrt{{q_w}^2+{q_x}^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}")

Rotation Matrix to Quaternion

회전 행렬 \(\boldsymbol{R}\) 을 다음과 같이 unit quaternion \(\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}")

Quaternion to Axis-Angle

unit 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}")

Axis-Angle to Rotation Vector

축 \(\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}")

Rotation Matrix to Roll-Pitch-Yaw

회전 매트릭스에서 롤, 피치, 요 각도를 결정하는 것은 간단하지 않습니다. 여러 가지, 때로는 무한한 솔루션이 있을 수도 있습니다. 이를 위해서는 몇 가지 기준에 따라 여러 솔루션 중 하나를 선택할 수 있는 알고리즘이 필요합니다.

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

로봇 포즈를 변환 행렬로 변환할 때 가장 어려운 부분은 로봇 컨트롤러가 어떤 방향 표현 방식을 사용하는지 파악하는 것입니다. 표현 방식을 알고 나면 위의 수학적 변환은 명확하지만, 로봇 제조사는 이를 명확하게 명시하는 경우가 드물며, 동일한 용어(예: 롤-피치-요)라도 로봇마다 다른 회전 순서를 의미할 수 있습니다.

이 섹션에서는 특정 로봇에서 확인된 규칙들을 정리하여, 위의 변환 방법을 다시 알아낼 필요 없이 올바른 변환을 선택할 수 있도록 합니다.

경고

이 정보는 특정 로봇 모델을 사용한 경험을 통해 수집된 것으로, 펌웨어 버전, 모델, 또는 제조사 전반에 대해 철저히 테스트되지 않았습니다. 변환 결과를 신뢰하기 전에 항상 이를 검증하세요. 예를 들어, 조합된 변환 행렬을 로봇 티치 펜던트에 표시된 포즈와 비교해 확인할 수 있습니다.

Universal Robots

Universal Robots는 TCP 포즈를 6개 요소로 구성된 벡터 [x, y, z, rx, ry, rz] 로 표시합니다. 처음 세 요소는 이동(translation)이고, 나머지 세 요소는 롤-피치-요가 아니라 rotation vector (각도만큼 스케일된 축-각도, Rodrigues 표현이라고도 함)로 표현된 방향입니다. 스크립트 및 RTDE 인터페이스(예: get_actual_tcp_pose)는 이동 값을 미터 단위로 보고하는 반면, 티치 펜던트는 이를 밀리미터 단위로 표시합니다.

4x4 변환 행렬을 구성하려면:

스크립트 또는 RTDE 인터페이스에서 포즈를 읽어온 경우 미터에서 변환하여 이동 값이 밀리미터 단위인지 확인하세요.
회전 벡터를 회전 행렬로 변환합니다: rotation vector to axis-angle, 이후 axis-angle to quaternion, 이후 quaternion to rotation matrix 순서로 진행합니다.
회전 행렬과 이동 벡터를 4x4 변환 행렬에 넣습니다.

Yaskawa

Yaskawa 컨트롤러는 포즈를 이동 값에 이어 세 개의 오일러 각도(Rx, Ry, Rz)로 보고하며, 순서는 외재적(extrinsic) xyz 순서입니다.

4x4 변환 행렬을 구성하려면:

이동 값이 밀리미터 단위인지 확인하세요.
표의 회전 순서를 사용하여 Roll-Pitch-Yaw to Rotation Matrix 변환을 통해 오일러 각도를 회전 행렬로 변환합니다.
회전 행렬과 이동 벡터를 4x4 변환 행렬에 넣습니다.

Known Euler-Angle Orders

다음 표는 로봇 제조사별로 확인된 오일러 각도 순서를 정리한 것입니다. 절차는 위의 Yaskawa와 동일하며, 제조사마다 오일러 각도 순서만 다릅니다. 이를 시작점으로 활용하되, 동일한 제조사라도 모델과 펌웨어에 따라 순서가 다를 수 있으므로 실제 로봇에서 반드시 확인하세요.

로봇 제조사	오일러 각도 순서	참고
Universal Robots	회전 벡터(오일러 아님)	축-각도 / Rodrigues
Fanuc	외재적 xyz	W-P-R로 보고됨
Yaskawa	외재적 xyz
Kawasaki	내재적 zyz
KUKA	내재적 zyx	A-B-C로 보고됨
Doosan	내재적 zyz
ABB	내재적 zyx	컨트롤러는 quaternion도 함께 제공합니다
Hyundai	내재적 xyz
Robostar	외재적 xyz
Nachi	내재적 zyx