참고. Quaternion (사원수)

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

위키피디아 에서 기본 수학적 개념에 대해 자세히 알아볼 수 있습니다 . 3blue1brown 이 만든 탐색 가능한 비디오 시리즈 Visualizing quaternions 도 볼 수 있습니다 .

이 튜토리얼에서는 ROS 2에서 사원수와 변환 방법이 어떻게 작동하는지 알아봅니다.

사원수의 구성 요소

ROS 2는 쿼터니언을 사용하여 회전을 추적하고 적용합니다. 쿼터니언에는 (x, y, z, w) 4개의 구성 요소가 있습니다. ROS 2에서는 w가 마지막이지만 Eigen과 같은 일부 라이브러리에서는 첫 번째 위치에 배치할 수 있습니다. 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는 경고를 인쇄합니다. 이러한 경고를 피하려면 쿼터니언을 정규화합니다.

q.normalize();

ROS2의 사원수 유형

ROS 2는 두 개의 쿼터니언 데이터 유형을 사용합니다: tf2::Quaternion및 동등한 geometry_msgs::msg::Quaternion. C++에서 두 데이터 유형을 변환하려면 tf2_geometry_msgs 메서드를 사용합니다.

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])

사원수 연산

RPY로 생각한 후 사원수로 변환

축에 대한 회전을 생각하는 것은 쉽지만, 사원수에 대한 관점에서 생각하는 것은 어렵습니다. 제안은 롤(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]}.')

사원수 회전 적용

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

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)

사원수 반전

사원수를 반전하는 쉬운 방법은 w 구성 요소에 -1 곱하는 것입니다.

q[3] = -q[3]

상대회전

같은 프레임에서 q_1,q_2두 개의 쿼터니언이 있다고 가정하고 다음과 같은 방식으로 q_1에서변환 되는 q_2상대 회전, q_r를 찾고 싶습니다.

q_2 = q_r * q_1

q_r은행렬 방정식을 푸는 것과 비슷하게 풀 수 있습니다. q_1을 역변환 하고 양쪽을 오른쪽으로 q_2를곱합니다. 다시 말하지만, 곱하는 순서가 중요합니다.

q_r = q_2 * q_1_inverse

다음은 파이썬에서 이전 로봇 포즈에서 현재 로봇 포즈로의 상대 회전을 구하는 예입니다.

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)

Last updated