참고. 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