Quaternion fundamentals

목표: ROS 2에서 쿼터니언 사용의 기본을 배웁니다.

배경

쿼터니언은 회전 행렬보다 간결한 방식으로 방향을 나타내는 4-튜플 표현입니다. 쿼터니언은 3차원 회전이 포함된 상황을 분석하는 데 매우 효율적입니다. 쿼터니언은 로봇 공학, 양자 역학, 컴퓨터 비전 및 3D 애니메이션과 같은 다양한 분야에서 널리 사용됩니다.

더 많은 수학적 개념에 대해 알아보려면 위키백과 를 참조할 수 있습니다. 또한 3blue1brown 에서 제작한 Visualizing quaternions 이라는 탐색 가능한 비디오 시리즈를 살펴볼 수 있습니다.

이 튜토리얼에서는 ROS 2에서 쿼터니언과 변환 방법이 어떻게 작동하는지에 대해 배우게 됩니다.

필수 전제 조건

이 튜토리얼을 따르려면 ROS 2 패키지에서 사용하는 tf2 라이브러리를 설치하고 이해하는 것이 좋습니다. 그러나 이것은 강제적인 요구 사항이 아니며 다른 여러 기하학 변환 라이브러리 중 하나를 사용할 수 있습니다. transforms3d, scipy.spatial.transform, pytransform3d, numpy-quaternion 또는 blender.mathutils 와 같은 라이브러리를 살펴볼 수 있습니다.

쿼터니언의 구성 요소

ROS 2는 회전을 추적하고 적용하기 위해 쿼터니언을 사용합니다. 쿼터니언은 4개의 구성 요소 (x, y, z, w) 를 가집니다. ROS 2에서는 w 가 마지막에 위치하지만 Eigen과 같은 일부 라이브러리에서는 w 가 첫 번째 위치에 배치될 수 있습니다. x/y/z 축 주위의 회전이 없는 일반적으로 사용되는 쿼터니언은 (0, 0, 0, 1) 이며 다음과 같이 생성할 수 있습니다:

#include <tf2/LinearMath/Quaternion.h>
...

tf2::Quaternion q;
// Create a quaternion from roll/pitch/yaw in radians (0, 0, 0)
q.setRPY(0, 0, 0);
// Print the quaternion components (0, 0, 0, 1)
RCLCPP_INFO(this->get_logger(), "%f %f %f %f",
            q.x(), q.y(), q.z(), q.w());

쿼터니언의 크기는 항상 1이어야 합니다. 수치 오류로 인해 쿼터니언 크기가 1이 아닌 경우 ROS 2에서 경고가 출력됩니다. 이러한 경고를 피하려면 쿼터니언을 정규화(normalize)하십시오:

q.normalize();

ROS 2에서의 쿼터니언 유형

ROS 2에서는 두 가지 쿼터니언 데이터 유형인 tf2::Quaternion 과 이와 동등한 geometry_msgs::msg::Quaternion 을 사용합니다. C++에서 이들 간의 변환을 수행하려면 tf2_geometry_msgs 의 메서드를 사용하십시오.

C++

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...

tf2::Quaternion tf2_quat, tf2_quat_from_msg;
tf2_quat.setRPY(roll, pitch, yaw);
// Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);

// Convert geometry_msgs::msg::Quaternion to tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);

Python

from geometry_msgs.msg import Quaternion
...

# Create a list of floats, which is compatible with tf2
# Quaternion methods
quat_tf = [0.0, 1.0, 0.0, 0.0]

# Convert a list to geometry_msgs.msg.Quaternion
msg_quat = Quaternion(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])

쿼터니언 연산

1 롤(roll), 피치(pitch), 요(yaw)로 생각한 다음 쿼터니언으로 변환하기

축 주위의 회전을 생각하기는 쉽지만 쿼터니언 관점에서 생각하기는 어렵습니다. 제안하는 방법은 롤(X 축 주위), 피치(Y 축 주위), 요(Z 축 주위)의 목표 회전을 계산한 다음 쿼터니언으로 변환하는 것입니다.

# quaternion_from_euler method is available in turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py
q = quaternion_from_euler(1.5707, 0, -1.5707)
print(f'The quaternion representation is x: {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')

2 쿼터니언 회전 적용

한 쿼터니언의 회전을 포즈에 적용하려면 이전 포즈의 쿼터니언에 원하는 회전을 나타내는 쿼터니언을 곱하기만 하면 됩니다. 이 곱셈의 순서가 중요합니다.

C++

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...

tf2::Quaternion q_orig, q_rot, q_new;

q_orig.setRPY(0.0, 0.0, 0.0);
// Rotate the previous pose by 180* about X
q_rot.setRPY(3.14159, 0.0, 0.0);
q_new = q_rot * q_orig;
q_new.normalize();

Python

q_orig = quaternion_from_euler(0, 0, 0)
# Rotate the previous pose by 180* about X
q_rot = quaternion_from_euler(3.14159, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)

3 쿼터니언 반전

쿼터니언을 반전하는 간단한 방법은 w-구성 요소를 부정하는 것입니다:

q[3] = -q[3]

4 상대 회전

동일한 프레임에서 두 쿼터니언인 q_1 및 q_2 가 있다고 가정해 보겠습니다. q_1 을 q_2 로 변환하는 상대 회전인 q_r 을 다음과 같이 찾고자 합니다:

q_2 = q_r * q_1

행렬 방정식을 해결하는 방식과 유사하게 q_r 을 찾을 수 있습니다. q_1 을 반전하고 양쪽에 곱합니다. 다시 한 번 강조하면 곱셈의 순서가 중요합니다.

q_r = q_2 * q_1_inverse

다음은 이전 로봇 포즈에서 현재 로봇 포즈로의 상대 회전을 얻는 예제입니다 (Python):

def quaternion_multiply(q0, q1):
    """
    Multiplies two quaternions.

    Input
    :param q0: A 4 element array containing the first quaternion (q01, q11, q21, q31)
    :param q1: A 4 element array containing the second quaternion (q02, q12, q22, q32)

    Output
    :return: A 4 element array containing the final quaternion (q03,q13,q23,q33)

    """
    # Extract the values from q0
    w0 = q0[0]
    x0 = q0[1]
    y0 = q0[2]
    z0 = q0[3]

    # Extract the values from q1
    w1 = q1[0]
    x1 = q1[1]
    y1 = q1[2]
    z1 = q1[3]

    # Computer the product of the two quaternions, term by term
    q0q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
    q0q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
    q0q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
    q0q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1

    # Create a 4 element array containing the final quaternion
    final_quaternion = np.array([q0q1_w, q0q1_x, q0q1_y, q0q1_z])

    # Return a 4 element array containing the final quaternion (q02,q12,q22,q32)
    return final_quaternion

q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w # Negate for inverse

q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w

qr = quaternion_multiply(q2, q1_inv)

요약

이 튜토리얼에서는 쿼터니언의 기본 개념과 반전 및 회전과 같은 관련 수학적 작업에 대해 배웠습니다. 또한 ROS 2에서의 사용 예제와 두 개의 별도 쿼터니언 클래스 간의 변환 방법에 대해 알아보았습니다.