Conversions Between Common Orientation Representations

This article explains how to convert between orientation representations commonly used in robotics. It includes the necessary mathematical equations as well as code examples showing how to integrate these conversions into your own applications.

If you want to utilize these transformations interactively, try our Pose Conversion GUI tool.

For a more in-depth explanation of pose representations, check out Position, Orientation and Coordinate Transformations.

Roll-Pitch-Yaw to Rotation Matrix

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

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

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

다음은 두 가지 다른 예입니다.

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

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)