659.42 선형 보간 (LERP)과 구면 선형 보간 (SLERP)

1. 개요

선형 보간(Linear Interpolation, LERP)과 구면 선형 보간(Spherical Linear Interpolation, SLERP)은 TF2 라이브러리에서 이산적으로 발행된 변환 데이터로부터 임의 시점의 변환을 생성하는 데 사용되는 두 가지 핵심 보간 방법이다. LERP는 병진(translation) 성분에, SLERP는 회전(rotation) 성분에 각각 적용된다. 두 방법은 기하학적 특성이 근본적으로 다른 공간에서 동작하며, 각각의 수학적 배경과 수치적 특성을 정확히 이해하는 것이 로봇 시스템의 좌표 변환 정확도를 보장하는 데 필수적이다.

2. 선형 보간 (LERP)

2.1 수학적 정의

LERP는 두 점 사이의 직선 경로 위에서 중간 점을 생성하는 가장 기본적인 보간 방법이다. 시작점 \mathbf{a}와 끝점 \mathbf{b} 사이에서 보간 비율 \alpha \in [0, 1]에 대하여:

\text{LERP}(\mathbf{a}, \mathbf{b}, \alpha) = (1 - \alpha) \mathbf{a} + \alpha \mathbf{b}

3차원 벡터의 각 성분에 대해 독립적으로 적용하면:

\begin{aligned} x(\alpha) &= (1 - \alpha) x_a + \alpha x_b \\ y(\alpha) &= (1 - \alpha) y_a + \alpha y_b \\ z(\alpha) &= (1 - \alpha) z_a + \alpha z_b \end{aligned}

2.2 LERP의 기하학적 해석

LERP는 유클리드 공간(Euclidean space) \mathbb{R}^3에서 두 점을 연결하는 직선 위의 점을 생성한다. 보간 비율 \alpha가 0에서 1로 증가하면, 보간된 점은 \mathbf{a}에서 \mathbf{b}로 등속 직선 운동한다.

2.3 TF2에서의 LERP 적용

TF2에서 LERP는 TransformStamped 메시지의 transform.translation 필드에 적용된다. 캐시에 저장된 두 시점 t_n과 t_{n+1}에서의 병진 벡터 \mathbf{p}_n과 \mathbf{p}_{n+1} 사이에서, 조회 시간 t_q에 대한 보간 비율 \alpha를 계산하고 LERP를 적용한다.

\alpha = \frac{t_q - t_n}{t_{n+1} - t_n}

\mathbf{p}(t_q) = (1 - \alpha) \mathbf{p}_n + \alpha \mathbf{p}_{n+1}

2.4 LERP의 특성

2.5 LERP의 오차 분석

비등속 또는 곡선 운동에서 LERP의 오차를 분석한다. 실제 운동 경로가 2차 함수 \mathbf{p}(t) = \mathbf{p}_0 + \mathbf{v}_0 t + \frac{1}{2}\mathbf{a} t^2로 표현될 때, LERP의 최대 오차는:

\epsilon_{\text{max}} = \frac{1}{8} |\mathbf{a}| \cdot (\Delta t)^2

여기서 \mathbf{a}는 가속도 벡터, \Delta t = t_{n+1} - t_n은 보간 구간이다. 이 결과는 보간 구간의 제곱에 비례하여 오차가 증가함을 보여준다. 따라서 변환 발행 주파수를 2배로 높이면 오차는 4배 감소한다.

3. 구면 선형 보간 (SLERP)

3.1 회전 보간의 특수성

회전(rotation)은 유클리드 공간이 아닌 회전군 SO(3) 위에서 정의된다. SO(3)는 비유클리드적(non-Euclidean) 다양체(manifold)이므로, 일반적인 LERP를 직접 적용할 수 없다. 쿼터니언으로 표현된 회전은 4차원 단위 구면(unit sphere) S^3 위에 존재하며, 이 구면 위에서의 보간에는 SLERP가 필요하다.

3.2 수학적 정의

두 단위 쿼터니언 \mathbf{q}_0와 \mathbf{q}_1 사이의 SLERP는 다음과 같이 정의된다.

\text{SLERP}(\mathbf{q}_0, \mathbf{q}_1, \alpha) = \frac{\sin((1-\alpha)\Omega)}{\sin\Omega} \mathbf{q}_0 + \frac{\sin(\alpha\Omega)}{\sin\Omega} \mathbf{q}_1

여기서 \Omega는 두 쿼터니언 사이의 각도이며, 내적으로 계산된다.

\cos\Omega = \mathbf{q}_0 \cdot \mathbf{q}_1 = x_0 x_1 + y_0 y_1 + z_0 z_1 + w_0 w_1

3.3 SLERP의 기하학적 해석

SLERP는 4차원 단위 구면 S^3 위에서 두 점을 연결하는 대원(great arc) 위의 점을 생성한다. 대원은 구면 위에서의 최단 경로(측지선, geodesic)이므로, SLERP는 두 회전 사이의 최단 경로를 따라 등속 각속도로 회전하는 보간을 수행한다.

3.4 SLERP의 특수 경우

3.4.1 경우 1: \Omega \approx 0 (거의 동일한 회전)

\sin\Omega \approx 0이 되어 분모가 0에 접근하는 수치적 불안정(numerical instability)이 발생한다. 이 경우 NLERP(Normalized LERP)를 대안으로 사용한다.

\text{NLERP}(\mathbf{q}_0, \mathbf{q}_1, \alpha) = \text{normalize}\left((1 - \alpha)\mathbf{q}_0 + \alpha\mathbf{q}_1\right)

TF2에서는 \Omega가 일정 임계값(threshold) 이하일 때 자동으로 NLERP로 전환하여 수치적 안정성을 보장한다.

3.4.2 경우 2: \mathbf{q}_0 \cdot \mathbf{q}_1 < 0 (반대 반구)

쿼터니언은 이중 피복(double cover) 특성을 가지며, \mathbf{q}와 -\mathbf{q}는 동일한 회전을 나타낸다. 내적이 음수이면 두 쿼터니언이 반대 반구에 위치하며, 이 경우 SLERP는 긴 경로(장호, long arc)를 따라 보간한다. 최단 경로를 보장하기 위해 한쪽 쿼터니언의 부호를 반전한다.

\text{if } \mathbf{q}_0 \cdot \mathbf{q}_1 < 0: \quad \mathbf{q}_1 \leftarrow -\mathbf{q}_1, \quad \Omega \leftarrow \pi - \Omega

3.4.3 경우 3: \Omega \approx \pi (거의 반대 회전)

\Omega가 \pi에 가까우면 두 회전이 거의 정반대이며, 보간 경로가 유일하지 않다. 이 경우 임의의 중간 경로가 선택될 수 있으므로 결과가 불안정할 수 있다. 실제 로봇 시스템에서 연속적인 변환 사이에 \pi 근처의 회전 차이가 발생하는 것은 극히 드물며, 이는 보통 변환 데이터의 오류를 의미한다.

3.5 SLERP의 특성

4. LERP와 SLERP의 비교

5. NLERP: SLERP의 경량 대안

5.1 정의

NLERP(Normalized LERP)는 쿼터니언에 LERP를 적용한 후 정규화하는 방법이다.

\text{NLERP}(\mathbf{q}_0, \mathbf{q}_1, \alpha) = \frac{(1 - \alpha)\mathbf{q}_0 + \alpha\mathbf{q}_1}{\|(1 - \alpha)\mathbf{q}_0 + \alpha\mathbf{q}_1\|}

5.2 SLERP와 NLERP의 비교

작은 회전 차이(\Omega < 0.01 rad 정도)에서 SLERP와 NLERP의 결과는 실질적으로 동일하다. TF2에서는 변환 발행 주파수가 충분히 높으면(>20 Hz) 연속적인 변환 사이의 회전 차이가 작으므로, NLERP로도 충분한 정확도를 얻을 수 있다.

6. TF2에서의 구현 세부사항

6.1 보간 알고리즘의 의사 코드

function interpolateTransform(T_0, T_1, alpha):
    // 병진 보간 (LERP)
    p = (1 - alpha) * T_0.translation + alpha * T_1.translation

    // 회전 보간 (SLERP with NLERP fallback)
    q_0 = T_0.rotation
    q_1 = T_1.rotation

    // 내적 계산 및 반대 반구 처리
    dot = q_0 · q_1
    if dot < 0:
        q_1 = -q_1
        dot = -dot

    // 각도 계산
    if dot > 0.9995:  // 거의 동일한 회전
        // NLERP 사용
        q = normalize((1 - alpha) * q_0 + alpha * q_1)
    else:
        // SLERP 사용
        omega = acos(dot)
        s_0 = sin((1 - alpha) * omega) / sin(omega)
        s_1 = sin(alpha * omega) / sin(omega)
        q = s_0 * q_0 + s_1 * q_1

    return Transform(p, q)

6.2 C++ 구현 예시

#include <tf2/LinearMath/Quaternion.h>

tf2::Quaternion slerpQuaternion(
    const tf2::Quaternion & q0,
    const tf2::Quaternion & q1,
    double alpha)
{
    return q0.slerp(q1, alpha);
}

tf2::Quaternion::slerp() 메서드는 위에서 기술한 모든 특수 경우 처리를 내부적으로 수행한다.

6.3 Python 구현 예시

import numpy as np
from scipy.spatial.transform import Rotation, Slerp

def interpolate_rotation(q0, q1, alpha):
    """
    두 쿼터니언 사이의 SLERP 보간
    q0, q1: [x, y, z, w] 형태의 쿼터니언
    alpha: 보간 비율 [0, 1]
    """
    rotations = Rotation.from_quat([q0, q1])
    slerp = Slerp([0, 1], rotations)
    result = slerp(alpha)
    return result.as_quat()

7. 응용: 부드러운 궤적 생성

LERP와 SLERP를 활용하면, TF2의 이산적 변환 데이터로부터 부드러운 연속 궤적을 생성할 수 있다. 이는 궤적 시각화, 로봇 애니메이션, 경로 재구성 등에 활용된다.

// 과거 궤적의 부드러운 재구성
std::vector<geometry_msgs::msg::PoseStamped> reconstructTrajectory(
    rclcpp::Time start_time,
    rclcpp::Time end_time,
    double resolution_hz)
{
    std::vector<geometry_msgs::msg::PoseStamped> trajectory;
    double dt = 1.0 / resolution_hz;

    for (double t = 0; t <= (end_time - start_time).seconds(); t += dt) {
        rclcpp::Time query_time = start_time + rclcpp::Duration::from_seconds(t);

        try {
            auto transform = tf_buffer_->lookupTransform(
                "map", "base_link", query_time);

            geometry_msgs::msg::PoseStamped pose;
            pose.header.stamp = query_time;
            pose.header.frame_id = "map";
            pose.pose.position.x = transform.transform.translation.x;
            pose.pose.position.y = transform.transform.translation.y;
            pose.pose.position.z = transform.transform.translation.z;
            pose.pose.orientation = transform.transform.rotation;

            trajectory.push_back(pose);
        } catch (const tf2::TransformException &) {
            break;
        }
    }

    return trajectory;
}

8. 요약

LERP와 SLERP는 TF2에서 병진과 회전 성분을 각각 보간하기 위한 핵심 수학적 도구이다. LERP는 유클리드 공간에서의 직선 경로 보간을 제공하고, SLERP는 쿼터니언 구면 위에서의 최단 경로 보간을 제공한다. 두 방법 모두 등속 보간을 보장하며, TF2 내부에서 반대 반구 처리와 수치적 안정성을 위한 NLERP 폴백(fallback) 등의 견고한 구현이 적용되어 있다.

참고 문헌 및 출처

• Shoemake, K., “Animating Rotation with Quaternion Curves,” ACM SIGGRAPH Computer Graphics, Vol. 19, No. 3, pp. 245-254, 1985.
• Dam, E.B., Koch, M., Lillholm, M., “Quaternions, Interpolation and Animation,” Technical Report DIKU-TR-98/5, University of Copenhagen, 1998.
• Eberly, D., “Quaternion Algebra and Calculus,” Geometric Tools, https://www.geometrictools.com/Documentation/QuaternionAlgebra.pdf
• ROS 2 소스 코드, “tf2/LinearMath/Quaternion.h,” https://github.com/ros2/geometry2
• Foote, T., “tf: The Transform Library,” Proceedings of IEEE Conference on Technologies for Practical Robot Applications (TePRA), 2013.