30.4.1. 모드 전환 시의 관성 및 운동량(Momentum) 보존을 고려한 초기 설정점(Setpoint) 상태 전이(State Transition) 로직

비행 중인 기체가 수동 제어 모드(Manual/Stabilized)나 다른 자동 임무(Mission/Offboard)에서 Loiter 모드로 전환될 때, 기체는 필연적으로 기존의 궤적 속도와 병진 운동량(Translational Momentum)을 지니고 있다. PX4-Autopilot은 모드 전환 시 발생할 수 있는 급격한 궤적 이탈(Bump)이나 기구학적 한계를 초과하는 제어 입력 포화(Saturation)를 방지하기 위해, 운동량을 보존하며 부드럽게 초기 설정점(Setpoint)을 갱신하는 상태 전이 로직을 채택하고 있다.

1. 초기 상태 전이(State Transition) 메커니즘 개요

모드 간 전환이 일어나는 동적 순간(Transient State)에, 하위 위치 제어기(PositionController)는 이전 비행 모드에서 사용하던 궤적 출력의 비행 연속성을 우선적으로 확보해야 한다. PX4는 메인 상태 머신 상에서 통합 모드가 Loiter로 변경되는 즉시, 기체의 현재 센서 추정 상태(Estimated State)를 역산하여 가상의 “초기 제동 설정점(Initial Braking Setpoint)“을 생성한다.

속도 및 가속도 연속성 보장: 기존의 기체 속도 벡터( $v_0$ )를 $0$ 으로 즉각 하드 리셋(Hard Reset)하는 대신, 현재의 이동 방향 벡터를 보존하면서 내부 가속도 제한(Kinematic Limits) 한도 내에서 속도가 서서히 줄어드는 감속 궤적을 산출하여 피드포워딩(Feed-forwarding)한다.
오버슈트(Overshoot) 및 역풍 쏠림 방지: 순간적인 정지 좌표계(Hold Position) 설정으로 인해 구동 모터가 과부하 또는 비대칭 출력을 내어 자세가 뒤집히는 것을 막기 위해, 운동량 보존 법칙에 근거하여 기체가 스스로 멈출 수 있는 정지 예정 위치(Stopping Point)를 전방 경로에 미리 투사(Projection)한다.

2. Ardupilot 모델과의 연속성 알고리즘 아키텍처 차이

Ardupilot의 경우 위치 유지 모드(AC_Loiter)로의 전환 즉시, 조종기의 현재 스틱 위치나 GPS 현재 위치를 기반으로 새로운 가상의 고무줄 타겟(Leash) 반경을 만들어 기체를 당겨 유지시키는 방식에 비중을 둔다.
반면, PX4-Autopilot은 FlightTaskAuto 또는 위치 제어의 전처리 과정인 FlightTaskAutoMapper 아키텍처를 이용하여, 저크(Jerk)와 최대/최소 가속도가 사전에 이산화되어 제한된 물리 엔진 모델을 실시간으로 역통과(Backward Pass)시켜 목표점을 찾는다. 즉, PX4는 현재의 운동 상태에서 멈춤 상태까지 가장 매끄럽게 연결되는 다항식 궤적(Trajectory) 생성에 설계 초점을 맞추어 모드 전환 충격을 소산시킨다.

3. 초기 설정점 갱신 및 제동 궤적 산출 수학적 모델

소스 코드 loiter.cpp가 활성화되어 on_activation() 루프 구간에 진입하면, 네비게이터는 하위 제어기로 목표 설정이 담긴 position_setpoint_triplet_s 메시지를 통해 초기 제어 좌표 전이를 구성한다.

3.1 능동적인 동적 정지점(Stopping Point) 투사 계산식

비행체가 전환된 시점에 초기 속도 $\vec{v_0}$ 를 가질 때, 기체에 세팅된 최대 수평 감속 가속도 파라미터 $\vec{a_{max}}$ (MPC_ACC_HOR_MAX 등 참조)를 적용하였을 때 동역학적으로 완전히 정지할 수 있는 최소 정지 거리 벡터 $\vec{d_{stop}}$ 는 다음과 같은 에너지 보존 및 가속도 단순 적분 모델식에 기초하여 일차적으로 근사된다.

$\vec{d_{stop}} = \frac{\Vert \vec{v_0} \Vert^2}{2 \cdot \Vert \vec{a_{max}} \Vert} \cdot \frac{\vec{v_0}}{\Vert \vec{v_0} \Vert}$

sequenceDiagram
    participant Commander as 커맨더 (Commander)
    participant Navigator as 네비게이터 (Navigator/loiter.cpp)
    participant EKF2 as 상태 추정기 (EKF2)
    participant PosCtrl as 위치 제어 분산기 (FlightTaskAuto)
    
    Commander->>Navigator: Loiter 모드 전환 인터럽트 명령 수신
    Navigator->>EKF2: 현재 위치 좌표계명, 속도, 3D 가속도 벡터 질의
    EKF2-->>Navigator: vehicle_local_position_s 데이터 구조체 응답
    Navigator->>PosCtrl: on_activation()에서 전이 상태 인가
    PosCtrl->>PosCtrl: 현재 비행 운동량(v_0) 기반 정지 좌표계 투사(d_stop 도출)
    PosCtrl->>PosCtrl: 파라미터(Acc 및 Jerk 한계) 점검을 통한 S-Curve 초기화
    PosCtrl-->>Navigator: 부드러운 위치/속도 궤적 Setpoint 지속 구조화 및 퍼블리싱

3.2. 상태값 위치 스냅샷(Snapshot) 메모리 저장 및 방향(Yaw) 제어 동기화

멀티로터의 제동 속도가 실계측 오차 범위 내에서 $0$ 에 수렴하는 순간, 모듈은 최종 도달 범위의 위치 스냅샷을 메모리에 고정하고 해당 점을 Hold 목표 좌표로 완전 귀속시킨다.
또한 방향(Yaw) 제어 전이 역시 중요하다. Loiter 상태 진입 시, 이전 비행 모드에서 조준하고 있던 헤딩(Heading) 각도를 일관되게 유지하거나 혹은 공기역학적 항력을 억제하기 위해 이동 방향(Velocity Vector)과 전면을 일치시킨 뒤 서서히 제동하는 방식(MPC_YAW_MODE 설정값 분기 호출)으로 상태가 이어진다.

4. 모드 전환 경계 지점(Corner Case)에서의 예외 처리 및 방어 코드(Guard Code)

PX4 소스 코드 수준에서, 이러한 운동량을 보존하는 전이 로직은 데이터 신뢰성을 엄격히 전제하고 작동하므로, 시스템 안정성을 지키기 위한 방어벽이 마련되어 있다.

속도 센서 지터(Jitter) 혹은 EKF 인스턴스 발산 필터링: GPS 교란 등으로 상태 추정기(EKF2)에서 순간적인 속도 스파이크가 튀어 비정상적으로 높은 가짜 허위 속도 $\vec{v_0}$ 가 유입되는 상황이 생길 수 있다. 이 때 전이 계산 상의 산출되는 $\vec{d_{stop}}$ 가 현재 위치를 수십 혹은 수백 미터 이탈하는 비행 버그(Fly-away)가 발생하게 된다. 이를 원천 방지하기 위해 loiter.cpp 및 경로 생성 라이브러리들은 물리적 최대 속도 상한선(MPC_XY_VEL_MAX)으로 초기 속도 스칼라를 클리핑(Clipping)하거나 이노베이션(Innovation) 테스트 통과 여부를 점검하여 신규 설정점을 검증한다.
안티플라이어웨이(Anti-Flyaway) 방벽: 산출된 정지 예상점이 현재 위치 기반 특정 안전 거리 한계를 초과하도록 무리하게 투사 연산될 경우, 가속도 및 저크 한계를 일시적으로 완화해 무조건적인 즉시 감속 모드(Hard Brake) 우선순위 로직으로 상태 트리(State Tree)를 전환한다.

5. 결론 및 MAVLink/ROS2 시스템 아키텍처로의 파급 효과

초기 설정점을 부동 소수점 예측 모델로 부드럽게 전이시키는 PX4의 로직은 MAVLink를 매개로 한 GCS 컴퓨터 기반 정밀 관제 및 ROS2 환경의 분산 오프보드 연산 환경에서 매우 중요한 동작 신뢰성(Reliability)을 확립한다.

자율 임무 수행 중 통신 단절(Data Link Loss)에 의한 Failsafe 안전 규칙이 발동하여 Loiter로 긴급 폴백(Fallback)될 때, 혹은 다수 에이전트를 운용하는 군집 비행(Swarm) 관제 체계에서 충돌 예방을 위해 전체에게 대기(Pause) 트리거를 하달할 때가 대표적이다. 이 때 각 드론 기체가 자신의 독립 운동량을 스스로 통제하며 S-Curve 제동에 자가 진입하므로, 전체 군집의 시스템 붕괴나 유체 교란파 발생을 미연에 방지하고 일시에 충돌 없이 통제될 수 있는 예측 가능한 대형(Formation) 보존력과 안전성을 획득하게 된다.