29.4.3. 자세 제어기(Attitude Controller - Outer Loop) 핵심 소스 (`src/modules/mc_att_control/AttitudeControl.cpp`)

29.4.3. 자세 제어기(Attitude Controller - Outer Loop) 핵심 소스 (src/modules/mc_att_control/AttitudeControl.cpp)

1. 멀티로터 캐스케이드(Cascade) 자세 제어 루프 개요

PX4-Autopilot의 멀티로터 비행 제어 알고리즘은 항공우주 제어공학의 정석이자 학술적 표준 기법인 캐스케이드 P-PID(Cascade P-PID) 이중 제어 루프 아키텍처를 시스템 전반에 걸쳐 아주 엄격하게 채택 통제하고 있다. 이는 외부 루프(Outer Loop)인 자세 제어기(Attitude Controller)와 더 깊숙한 내부 루프(Inner Loop)인 각속도 제어기(Rate Controller)로 핵심 소프트웨어 기능이 완전히 상호 독립 분리되어 한 방향 직렬로 연결된 파이프라인 데이터 구조 체인을 견고히 의미한다.

이 중 자세 제어기(Attitude Controller) 모듈에 해당하는 내부 C++ mc_att_control 앱(App)은 기체의 현재 전역 3차원 기울기(Attitude 쿼터니언 상태량)를 상위 지시 목표 자세 각도에 칼같이 수렴시키는 1차적 전단 책임을 총괄 지휘한다. 이 모듈이 연산 후 도출하여 출력하는 최종 물리 결과 산출물은, 모터를 직접 하드웨어적으로 돌리는 PWM 및 DShot 신호 제어값이 절대로 아니며, 오직 기체가 최종 목표 자세로 부드럽고 신속하게 기동 이동하기 위해 바로 다음 시간 틱 스텝(Time Step)에서 순간 초 단위로 민첩하게 달성 도달해야만 할 목표 3축 각속도(Target Angular Rate, uORB Message: vehicle_rates_setpoint) 벡터 변수 배열이다.

즉, PX4 핵심 자세 제어기는 단지 P(비례, Proportional) 에러 곱셈 제어만을 순수하게 수행 연산하는 기하학적 각도 오차 역산 증폭기(Kinematic Attitude Error Calculator Amplifier) 역할에 철저히 가깝다. 본 학술 섹션에서는 PX4-Autopilot v1.14 최신 안정 오픈 소스 코드 구조 트리 중 src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp 내부에 C++ 클래스로 강력히 구현된 이 외부 외부 P루프(Outer P-Loop) 프레임워크의 핵심 수학 아키텍처와 논리 제어 전개 구조를 매우 깊숙이 해부 분석한다.

2. 자세 제어기(AttitudeControl) C++ 캡슐화 클래스 아키텍처

AttitudeControl 핵심 수식 클래스는 PX4의 방대한 uORB 메시징 통신망 미들웨어 인프라와의 복잡다단한 시스템 종속성(Dependencies)을 완벽하게 절단 분리 캡슐화시켜, 오직 순수한 입출력 수학 연산만을 독립 수행하도록 독립 라이브러리 형태로 매우 가볍고 경량화 되게 설계(Decoupled Mathematical Design Architecture) 구축되어 있다.

이 코어 클래스의 구동 생명 핵심은 OS 스케줄러에 의해 무한 주기적으로 호출 점검되는 update() 메서드 함수 퍼블릭 인터페이스이다. 매뉴얼(Manual) 안정화 수동 제어이든 지상관제소 QGC 웨이포인트 이동 자율 임무(Auto Mission)든 출처에 완벽히 무관하게 순수 목표 쿼터니언 수학 오더가 입력 수신되면, 이 메소드 펌프는 하위 백그라운드 칼만 센서 추정기(EKF2)로부터 현재 기체의 물리적 쿼터니언 정답 기하 상태를 전달받아, 기체가 지금 즉시 추종 따라가야 할 기하학적 목표 P, Q, R(Roll angular rate, Pitch rate, Yaw rate) 포맷 속도 벡터를 단 1~2밀리초(ms) 시간 내에 즉시 번개처럼 연산 도출 배출해낸다.

2.1 핵심 제어 외부 P 루프(Outer P-Loop) 수식 전개 연산 과정

PX4의 다차원 제어 수학 모델식 심층을 수학적으로 역분석해보면, 3축 롤, 피치, 요 좌표의 자세 불일치 상태 오차에 대해 다음과 같은 비례 스칼라 벡터 방정식 전개를 거슬러 올라 엄격히 수행한다.

  1. 오차 연산 도출: \Delta q = q_{target} \otimes q_{current}^{-1} (쿼터니언 역회전 반전 산출 오차 함수 수학 연산)
  2. 비례 제어 통제 (Proportional Control Amplification): \vec{\omega}_{target} = \mathbf{K}_p \cdot \vec{\epsilon}_{angle}
  3. 피드포워드 지연 보상 (Feed-forward Bias) 가산: \vec{\omega}_{final} = \vec{\omega}_{target} + \vec{\omega}_{ff}

여기서 강력한 증폭 이득(Gain) 행렬 상수인 \mathbf{K}_p는 QGroundControl(이하 QGC) v4.3 데스크톱 그라운드 관제 파라미터 세팅망에서 개발자나 튜닝 엔지니어가 명시적으로 정해 둔 기체 고유 파라미터 MC_ROLL_P, MC_PITCH_P, MC_YAW_P 값을 각각의 X, Y, Z 대각 필수 성분 벡터로 가지는 3x3 정방 대각 행렬(Diagonal Matrix) 치환 구조이다.
이 튜닝 상수값들을 프레임 한계보다 너무 과도하게 높게 튠(Tune) 증폭하면 모터 응답이 폭주해 기체가 공중에서 고주파로 몸을 맹렬히 부르르 떠는 극단 심각 진동(Oscillation) 스파이크를 기계 피로와 함께 심하게 발생 추락시키게 되며, 너무 무기력하게 반대로 낮게 풀어서 세팅할 시에는 아주 미세한 측면 바람(Cross-wind)이나 비통제 관성에 밀려 조종 스틱 레버 피드백 조향 응답성이 마치 무거운 진흙탕 구르듯 매우 물렁 나태해지고, 위험한 응답 지연(Control Latency Sluggishness) 체감이 관성 비례하여 폭등 증가한다.

#include <matrix/math.hpp>
#include "AttitudeControl.hpp"

// 외부 Outer P 제어망 루프의 핵심 브레인 수치 연산 블록 루틴 호출
matrix::Vector3f AttitudeControl::update(matrix::Quatf q_current, matrix::Quatf q_target, float yawspeed_feedforward)
{
    // 1. 제어 루프 내 실시간 EKF2 현재 쿼터니언 상태량과 QGC 목표 쿼터니언 간의 공간상 쿼터니언 회전 오차(Error Vector) 도출 연산 공식 적용 코드
    matrix::Quatf q_error = q_target * q_current.inversed();
    
    // (여기서 에러 쿼터니언의 반대 위상 스칼라부 기하학적 반전 및 최단 회전 경로 변환 특수 수식 로직이 내부적으로 철저히 수학 보호 수행됨)
    ...
    
    // 2. 외부 비례 제어(Outer P-Control Rule): 추출된 자세 각도 오차 원시 벡터 3축 요소 배열에 P-Gain 제어 비례 계수 이득을 행렬 스칼라 배수 곱 연산(Matrix Multiplication) 앰프 처리
    matrix::Vector3f rate_setpoint = _proportional_gain.emult(euler_error_vector);

    // 3. 특히 랙(Lag)이 심한 무인항공기 요(Yaw) 축에 대한 매우 빠른 조종 스틱 직결 응답성 신속 확보를 위한 사전 피드포워드(Feedforward) 지연 시간 단축 제어항 보조 덧셈 가산 로직
    rate_setpoint(2) += yawspeed_feedforward;

    // 4. 모터 페이로드 붕괴 탈조 및 물리적 안전 한계선을 무리하게 넘기지 않기 위한, 시스템 최대 허용 절대 각속도(Max Angular Spin Rate) 상한선 Limit 소프트웨어 강제 구속
    limit_rates(rate_setpoint);

    // 최종 산출된 목표 3축 회전 각속도를 반환함
    return rate_setpoint; // 반환 즉시 대기중인 내부 Inner 루프 코어 앱인 mc_rate_control로 하달 전달 배출됨
}

3. 캐스케이드 P-PID 제어망 시스템 하이라키(Hierarchy) 데이터 파이프라인 모식도

이 거대하고 통합 집중된 외부 루프 제어 브레인 노드 수학 연산기가 자율 무인기 PX4 커널 운영체제(NuttX RTOS) 펌웨어 코어 아키텍처망 안에서 비동기 입출력을 민첩 시분할로 주고받으며 기능적으로 어떻게 캐스케이드 사슬 코어 상하 직렬 연동되어 맞물려 빈틈없이 작동하는 지는 아래의 종합 데이터 체인 망 모델(Model) 다이어그램 맵으로 명확히 도식화 증명 정리된다.

graph TD
    SETPOINT["Data Message uORB Topic: <br> vehicle_attitude_setpoint <br> (Target Destination Quaternion)"] --> P_LOOP
    STATE["State Estimator Node <br> EKF2 Filter vehicle_attitude <br> (Ground Truth Current Quaternion)"] --> P_LOOP
    
    subgraph OUTER_LOOP[Outer Loop Block: <br> Attitude Controller module (mc_att_control)]
        P_LOOP{"C++ Class: AttitudeControl::update() <br> Only Core Proportional (P) Gain Math Ops"}
    end
    
    P_LOOP -->|Output Product Pipeline: <br> Required Target Roll/Pitch/Yaw Rate <br> Metric rad/s| RATE_SETPOINT["Data Message uORB Topic: <br> vehicle_rates_setpoint"]
    
    RATE_SETPOINT --> I_LOOP
    GYRO["Sensor Framework Node: <br> Real-time Hardware IMU Gyroscope <br> (vehicle_angular_velocity msg)"] --> I_LOOP
    
    subgraph INNER_LOOP[Inner Loop Block: <br> Fast Rate Controller module (mc_rate_control)]
        I_LOOP{"C++ Class: RateControl::update() <br> High-speed Full PID Core Control"}
    end
    
    P_GAIN[(QGC Calibration Params: <br> MC_ROLL_P <br> MC_PITCH_P Constants)] -.-> P_LOOP
    PID_GAIN[(QGC Calibration Params: <br> MC_ROLLRATE_P / I / D Matrix Constants)] -.-> I_LOOP
    
    I_LOOP --> MIXER[Control Allocation Node <br> or Classic Mixer Vector Output]
    MIXER --> PWM_ACT[Physical Hardware Esc PWM/Dshot Actuator Control Bus]
    
    style P_LOOP fill:#d1e0e0,stroke:#2d8659,stroke-width:3px
    style I_LOOP fill:#e6ccff,stroke:#6600cc,stroke-width:3px

이 시스템 모듈화 분리 독립 고리 아키텍처 설계 철학적 차원에서의 구조 차이는, 클래스 상속과 단일 거대 P-PID 얽힘 복잡도가 매우 심각하게 강한 구형 옛 초기 ArduPilot 소프트웨어 아키텍처의 ModeStabilize 오버라이딩 상속 C++ 병합 클래스 등과 기술적으로 극명히 전면 대척 대비 평가된다.

첨단 생태계인 현 PX4에서는 외부 자세 제어기가 사용자 비행 모드(Nav State Flight Mode) 잡무 판단 로직과 내부적으로 완전 독립 절단 분리되어 철저히 수학 라이브러리 캡슐화 분리 격리(Isolating Abstraction API) 장막이 쳐져 있으므로, 임의의 고도화된 스웜 군집 ROS2(Robot Operating System 2) 유저 개발 인프라 노드가 독자적으로 개척 훈련시킨 특수 뉴럴 네트워크(Neural Network) 딥러닝 인공지능 강화학습 관제 등에서 창출, 추출해낸 순수 목적 vehicle_attitude_setpoint 메시지만을 유무선 미들웨어로 자유 퍼블리싱 주입 명령하더라도, 하위 PX4 물리 하드웨어 기계 제어 코어가 이 탄탄한 기하학 제어 P-루프를 100% 범용적으로 즉각 완벽하게 해석 재활용하여 일말의 수정 개조 없이 모터를 구동 비행시킬 수 있는, 타의 근접을 불허하는 매우 압도적이고 극강의 코드 결합 이식성 및 학술 자율 다목적 연구 확장 플랫폼 아키텍처 뼈대 이점을 확실히 지니고 보장한다.