29.6.2.1. 제어 모드 전환 시 튐(Jerking) 방지: 전환 직전의 EKF 상태 추정치를 목표 자세로 동기화하여 초기 오차율을 영점화(Zeroing)하는 C++ 구현부

1. 개요 및 튐(Jerking) 방지 원리

범플리스 트랜스퍼(Bumpless Transfer)를 달성하기 위한 가장 일차원적이고 강력한 방법은, **새로운 제어루프가 가동되는 바로 첫 번째 사이클(Tick)에서 산출되는 비례 오차(P-Error)를 수학적으로 0으로 덮어쓰는 것(Zeroing)**이다.

비행 제어기 내부의 오차 방정식은 통상적으로 Error(t) = Setpoint(t) - EKF_Estimate(t) 로 정의된다. 조종자가 아크로(Acro) 모드에서 자세 안정화(Stabilized) 모드로 전환하는 순간( $t_0$ ), 조종기의 스틱은 중앙(0도)을 지시하고 있더라도 기체의 실제 자세 EKF_Estimate(t_0)는 기울어져 있을 수 있다. 이 때 Setpoint(t_0)를 스틱 입력인 0도가 아닌, 기체의 현재 상태 EKF_Estimate(t_0)와 동일하게 임의 셋팅해주면 $Error(t_0) = 0$ 이 되어 P 제어기의 급가속 출력(튐 현상)을 원천적으로 차단할 수 있다. 본 절에서는 PX4-Autopilot의 C++ 구문 내에서 이 로직이 어떻게 구현되는지 추적한다.

2. 상태 추정치(EKF)의 목표(Setpoint) 동기화 아키텍처

비행 모드가 전환될 때 호출되는 플라이트 태스크(Flight Task)의 activate() 또는 모드 이니셜라이저(Initializer)는 시스템의 vehicle_attitude 토픽(EKF2 모듈 산출물)을 직접 구독(Subscribe)한다. 전환 시나리오에 따른 동기화 전략은 다음과 같다.

자세/각도 제어(Attitude Control): 피치(Pitch)와 롤(Roll)의 경우, 보통 조종자가 스틱을 놓은 상태(중앙 복귀)라면 기체의 현재 각도를 캡처하여 새로운 목표값으로 저장하고, 스무딩 로직(Slew Rate Limiting)을 거쳐 0도로 천천히 감쇠시킨다. 헤딩(Yaw)의 경우 현재 각도를 목표 방위로 강제 잠금(Lock)하여 기체가 불필요하게 돌아가는 것을 막는다.
좌표/위치 제어(Position Control): 고도(Altitude)와 XY 위치의 경우, 전환 순간의 vehicle_local_position 데이터를 셋포인트 변수에 덮어씌워 기체가 전환 위치에 그대로 정지(Hovering)하도록 만든다.

3. 오차 제로잉 흐름도 (Mermaid 설계도)

안정화 제어기가 깨어나는 첫 번째 사이클에서 일어나는 오차 영점화의 프로세스 플로우이다.

graph TD
    A[조종기 Mode 변경 이벤트 발생] --> B[FlightTask::activate 호출]
    
    B --> C[EKF2 모듈로부터 실시간 q 쿼터니언 센서 융합 데이터 취득]
    
    C --> D[쿼터니언을 오일러 각 Euler Roll/Pitch/Yaw 로 변환]
    
    D --> E[조종기 스틱 입력 무시 및 EKF 상태로 표본 캡처]
    
    E --> F[내부 Setpoint 변수에 EKF 자세 할당]
    F --> G(초기 비례 오차 산출: Setpoint - Current_Attitude = 0)
    
    G --> H[PID 출력 0으로 초기 기동]
    H --> I[이후 스틱 입력값으로 전환하는 Smoothing/Easing 함수 동작]

4. 소스 코드 깊은 분석 (`mc_att_control` 및 `FlightTask` 스택)

PX4 펌웨어에서 전환 직전의 상태 추정 데이터를 목표 셋포인트로 동기화하는 가장 대표적인 코드 예시는 src/modules/flight_mode_manager/tasks 패키지 내부와, 자세 제어 모듈 단에서 확인할 수 있다.

// 수동 지향 위치 제어 태스크 활성화 메서드 (간단화된 의사 코드)
bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
    // ... 부모 메서드 처리 루틴 생략 ...

    // 1. 헤딩(Yaw) 튐 방지
    // 전환 순간 EKF에서 산출된 현재 요우(Yaw) 각도를 그대로 목표(Setpoint)로 복사한다.
    // 이는 기체가 전환 직후 엉뚱한 방향을 바라보려 몸을 돌리는 것을 완벽하게 방지한다.
    _yaw_setpoint = _yaw;

    // 2. 수직 하강/상승 튐 방지 (Altitude Zeroing)
    // 조종기의 스로틀 스틱이 중앙 데드존에 위치해 있다면 고도를 고정해야 하므로,
    // EKF의 현재 고도 _position(2)를 고도 셋포인트로 복사한다.
    if (stick_in_center_deadzone()) {
        _position_setpoint(2) = _position(2);
        _velocity_setpoint(2) = 0.0f;
    } else {
        // 스틱이 이미 조작 중이라면 현재 하강/상승 속도를 복사하여 속도계 오차를 없앤다
        _velocity_setpoint(2) = _velocity(2);
    }
    
    return true;
}

이와 더불어 수동 자세 제어 안정화 로직(ManualControl) 내에서도 각속도와 각도의 매핑을 부드럽게 통합하는 로직이 적용되어 있다. EKF 상태(_yaw, _position 등)를 새로운 세트포인트 멤버 변수(_yaw_setpoint)로 복사(=)하는 단 한 줄의 대입 연산이, 제어 불연속성을 물리 공간에서 차단하는 가장 핵심적인 C++ 구현부이다.

5. Ardupilot 대비 아키텍처 구현 차이

상태 추정치 기반의 오차 제로잉(Zeroing) 기법에 있어 두 시스템은 명명법 및 설계 계층에서 차이를 보인다.

PX4-Autopilot:
uORB 메시지를 구독하여 EKF 추정 데이터를 획득한 후, FlightTask라는 고급 추상화 객체 내부에서 지역 변수(Setpoint)를 조작하여 덮어쓰는 방식을 취한다. 전환 처리가 제어 코어(Controller Core)가 아닌 태스크 관리자(Task Manager) 레이어에서 독립적으로 이루어진다는 특징이 있다.
Ardupilot:
센서 추정 라이브러리인 AHRS 모듈과 제어 코어인 AC_AttitudeControl이 매우 밀접하게 직결되어 있다. Ardupilot은 제어를 이완시킨다는 의미로 relax_attitude_controllers() 또는 relax_bf_rate_controller() 라는 메서드를 모드 단위로 init() 함수에서 직접 호출한다. 이 메서드 내부는 “목표 각도를 현재 AHRS 롤/피치 표본으로 덮어씌운다(target = current)“라는 본질적으로 PX4와 동일한 수학적 과정을 거친다.

PX4의 Slew Rate 필터와 범플리스 제로잉이 얽힌 FlightTask 기반 아키텍처는, 각 제어 모듈 개발자가 자세한 내부 PID 통제 로직을 신경 쓰지 않아도 되도록 잘 정의된 C++ 상속(Inheritance) 인터페이스를 제공한다.