Conversions Between Common Orientation Representations

이 기사에서는 로봇에 일반적인 방향 표현 간의 변환을 위한 방정식을 제시합니다. 포즈 표현에 대한 자세한 내용은 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)