29.4.1.2. 요(Yaw) 입력의 동적 적분: 오일러 각 누적 적분 방정식 코드 및 짐벌 락(Gimbal Lock) 방지를 위한 회전각 임계 범위 정규화 로직

1. 요(Yaw) 축 비행 제어의 역학적 특수성

멀티로터 기체의 비행 제어 시스템에서 롤(Roll)과 피치(Pitch) 제어는 기체가 즉각적으로 특정 각도에 도달하고 유지하는 절대 각도 추종(Absolute Angle Tracking) 방식을 따른다. 반면, 요(Yaw) 제어는 조종자의 의도가 근본적으로 다르다. 조종자가 요 스틱을 좌/우로 움직일 때, 이는 기체가 특정 방위각(예: 북쪽 45도)을 영구적으로 지향하라는 의미가 아니라, 스틱을 놓을 때까지 특정 각속도(Angular Rate)로 지속적으로 회전하라는 의미를 갖는다.

따라서 PX4-Autopilot v1.14 내 FlightTaskManualPosition 아키텍처는 조종기로부터 인가된 요 스틱 입력값(정규화된 $[-1.0, 1.0]$ 범위의 무차원 변수)을 직접 방위 목표수치에 대입할 수 없으며, 시간에 따른 누적 연산 즉, 동적 적분(Dynamic Integration)을 수행해야만 연속적인 3차원 목표 오일러 각(Euler Angle) 데이터 구조를 완성할 수 있다.

2. 오일러 각 누적 적분 방정식과 코드 구현체

요 축의 각속도 명령은 MPC_MAN_Y_MAX 파라미터(기본 지상 관제 시스템인 QGroundControl v4.3에서 ‘Manual Yaw Rate Max’ 설정값, 단위: deg/s)를 통해 라디안 계의 회전 각속도 한계치로 비례 확장된다. PX4 소스 코드 상에서, 스틱 입력에 의해 생성된 목표 각속도는 제어 루프의 주기(Time Step, $dt$ )마다 이전 단계의 요 방위각에 누적 적분되어 새로운 제어 목표 각도를 갱신하게 된다.

이를 수학적인 연속 시계열 누적 적분 방정식(Sequential Integration Equation)으로 표현하면 다음과 같다.
$\psi_{target}(t) = \psi_{target}(t-dt) + \left( Input_{yaw} \cdot \psi_{max\_rate} \right) \times dt$

이러한 수치 적분(Numerical Integration) 과정은 src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp 모듈 내에서 다음과 유사한 C++ 알고리즘을 통해 매 틱(Tick)마다 계산된다.

// 1. 수동 조종 입력값을 목표 각속도(rad/s)로 환산
float yaw_rate_setpoint = _manual_control_setpoint.yaw * math::radians(_param_mpc_man_y_max.get());

// 2. 제어 루프 주기 dt를 곱하여 이전 요 목표각에 누적 적분을 수행
_yaw_setpoint += yaw_rate_setpoint * dt;

이 루프는 시스템 베이스라인에 맞춰 고속으로 동작(일반적으로 250Hz 이상)하므로, 조종자의 미세한 스틱 컨트롤에도 결과적으로 매분 매초 연속적으로 매끄럽게 변화하는 요 방위각 계적( $\psi_{target}$ )이 산출된다.

회전각 임계 범위 정규화와 짐벌 락(Gimbal Lock) 방지 기술

오일러 각 누적 적분기를 구동할 때 발생하는 치명적인 공학적 난제는 바로 누적 방위각의 영구적 발산과 특이점에 의한 짐벌 락(Gimbal Lock) 위험성이다. 기체가 요잉 제어를 통해 한 방향으로 계속 맴돌기(Spinning) 기동을 수행할 경우, 수학적으로 요 각도는 $2\pi, 4\pi, 6\pi \dots$ 로 무한히 커질 수 있으며, 이는 행렬 대수계의 부동 소수점 오버플로(Floating-point Overflow) 및 3차원 투영 시 치명적인 안정성 왜곡을 유발한다.

이를 막기 위해 PX4-Autopilot은 매 적분 연산 직후 오일러 각의 범위를 $[-\pi, \pi]$ (즉, -180도 ~ +180도) 구간 상으로 강제 사상시키는 회전각 임계 범위 정규화(Normalization) 로직을 적용하고 있다.

#include <matrix/math.hpp>

// ... 적분 연산 직후 실행 ...
// 최적화된 matrix 라이브러리의 wrap_pi 함수를 통한 모듈로(Modulo) 연산 및 정규화
_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);

이 때 호출되는 matrix::wrap_pi 함수 내부에서는 라디안 기저각 한계를 초과하는 변수 데이터에 대해 스플라이싱 규칙을 적용하며, 제어 루프 파라미터들의 절댓값 크기 $\vert \theta \vert$ 기준 평가를 거치게 된다.

요 누적 적분 및 정규화 파이프라인 레이아웃

전술된 제어 이론을 체계적으로 구조화한 아키텍처는 아래 다이어그램과 같다.

graph TD
    A[Yaw RC Stick Input] -->|x MPC_MAN_Y_MAX| B_Rate[Target Yaw Rate]
    B_Rate -->|Integration x dt| C_Integrator((+))
    Z_Feedback[Previous Yaw Setpoint] --> C_Integrator
    
    C_Integrator --> D_RawAngle[Raw Accumulated Yaw Angle]
    
    D_RawAngle --> E_Condition{"Is |Yaw| > PI ?"}
    
    E_Condition -->|Yes| F_Wrap[Invoke matrix::wrap_pi Normalization]
    E_Condition -->|No| G_SafeAngle[Safe Yaw Setpoint]
    
    F_Wrap --> G_SafeAngle
    G_SafeAngle -.->|Feed to Next Step dt| Z_Feedback
    G_SafeAngle --> H_Euler[To Attitude Controller <br> / Euler-Quat Core]
    
    style F_Wrap fill:#ffcccc,stroke:#ff0000,stroke-width:2px

이러한 강건한 설계 덕분에 구세대 무인기 프레임워크나 ArduPilot의 아주 초기 버전들에서 간혹 발생했던 불필요한 자세 제어기 락 현상이 완벽히 차단된다. 나아가 ROS2 DDS 미들웨어를 연동하거나 무인 군집 자율 비행(Swarm Path Planning) 환경에서의 오프보드(Offboard) 경로 추종 알고리즘을 커스텀 개발할 때에도, QGC 관제 통신망(MAVLink)이 다루는 요 방위각 데이터는 항시 $[-\pi, \pi]$ 영역으로 정규화된다는 도메인 지식을 인지하고 있어야 로직 상의 방위 오계산을 미연에 방지할 수 있다.

핵심 소스 참조 아카이브: PX4 수학 라이브러리 src/lib/matrix