29.5.3. 각속도 제어기(Rate Controller - Inner Loop) 핵심 소스 (`src/modules/mc_rate_control/RateControl.cpp`)

멀티로터 비행 제어 아키텍처에서 가장 낮은 수준(Lowest-level)의 궤환 루프이자, 기체의 물리적 안정성을 최종적으로 책임지는 핵심 두뇌가 바로 각속도 제어기(Rate Controller)이다. 아크로(Acro) 모드에서는 사용자의 스틱 입력을 직결받아 동작하며, 코스믹 레이(Cosmic Ray) 수준의 빠른 응답 속도를 내야 한다.

본 절에서는 PX4-Autopilot의 내부 루프(Inner Loop) 제어를 담당하는 mc_rate_control 모듈 내 핵심 클래스인 RateControl의 동작 인프라와 C++ 소스 코드를 체계적으로 분석한다.

1. 각속도 제어기(Rate Controller) 역할 및 시스템 아키텍처

Rate Controller의 근본적인 목적은 상위 모듈(Commander, Attitude Controller, 또는 Manual Control)로부터 전달받은 목표 각속도(vehicle_rates_setpoint) 와 자이로스코프(Gyroscope) 센서가 계측한 기체의 현재 각속도(vehicle_angular_velocity) 간의 오차(Error)를 0으로 좁히는 3차원 토크(Torque) 제어 명령을 생성하는 것이다.

1.1 제어 파이프라인

graph LR
    A[vehicle_rates_setpoint] --> C(Rate Control 코어 연산)
    B[자이로스코프 데이터 추정치] --> C
    C -->|PID 및 Feedforward 연산| D[3축 Torque 명령 생성]
    C -->|기호 복원 및 와인드업 방지| E[actuator_controls 발행]
    E --> F[Control Allocation / 믹서]

2. 핵심 소스 코드 분석 (`RateControl::update`)

src/modules/mc_rate_control/RateControl.cpp 내에 정의된 RateControl::update() 함수는 매 주기마다(일반적으로 400Hz ~ 1000Hz 틱레이트) 호출되며, 실질적인 비행 역학 PID 제어 수학을 수행한다.

// src/modules/mc_rate_control/RateControl.cpp 의 주요 연산부 발췌 및 재구성

void RateControl::update(const vehicle_rates_setpoint_s &rates_sp, const vehicle_angular_velocity_s &rates)
{
    // 1. 오차(Error) 연산: 목표 각속도에서 현재 각속도를 차감
    matrix::Vector3f rate_error;
    rate_error(0) = rates_sp.roll - rates.xyz[0];
    rate_error(1) = rates_sp.pitch - rates.xyz[1];
    rate_error(2) = rates_sp.yaw - rates.xyz[2];

    // 2. 비례(Proportional, P) 제어 연산
    // _gain_p는 시스템 파라미터(MC_ROLLRATE_P 등)로 설정된 P 게인 벡터
    matrix::Vector3f torque = rate_error.emult(_gain_p);

    // 3. 적분(Integral, I) 제어 연산 및 누적
    // Actuator(모터 믹서)가 포화(Saturation)되지 않았을 때만 오차를 누적(Anti-Windup)
    if (!_mixer_saturated) {
        _integral_error += rate_error * _dt;
    }
    
    // I 게인을 곱한 후, 설정된 최대 I 제어 한계(Limit) 캡핑(Clamping) 실시
    matrix::Vector3f integral_term = _integral_error.emult(_gain_i);
    torque += integral_term;

    // 4. 미분(Derivative, D) 제어 연산
    // 자이로 노이즈 증폭을 막기 위해 현재 각속도의 변화율(미분값)에 D 게인 곱셈
    // (보통 Low-pass Filter를 거친 각속도의 이전 프레임과의 차분을 이용)
    matrix::Vector3f rate_derivative = (rates_filtered_prev - rates.xyz) / _dt;
    torque += rate_derivative.emult(_gain_d);

    // 5. 피드포워드(Feed-Forward, FF) 연산 반영
    // 목표 각속도 자체에 비례하는 선제적 조향 힘 추가 (스틱 반응성 극대화)
    torque += matrix::Vector3f(rates_sp.roll, rates_sp.pitch, rates_sp.yaw).emult(_gain_ff);

    // 최종 연산된 3축 제어 토크들을 액추에이터 믹서로 전송(Publish)
    publish_torque_commands(torque);
}

2.1 코드 아키텍처 주요 튜닝 포인트

위의 동력학 소스 코드에서 PX4가 지향하는 제어 철학의 핵심들을 엿볼 수 있다.

벡터라이제이션(Vectorization):
PX4의 Rate Controller 내부 행렬 연산은 matrix::Vector3f 기반의 선형 대수 라이브러리를 활용한다(emult() 등). 이는 과거 Ardupilot에서 Roll, Pitch, Yaw 축을 개별적으로 풀어헤쳐 스칼라(Scalar)로 계산하던 옛 방식에 비해 코드가 우아하며 CPU 캐시 효율성(SIMD 연산 등)을 최적화하기에 용이하다.
피드포워드(Feed-Forward, FF) 제어의 독립성:
과거의 PID 알고리즘은 센서의 오차가 발생한 ’이후’에만 모터를 가동하는 후행 제어 방식(Feedback)이었다. 반면 PX4 코드에서 5단계에 명시된 _gain_ff 부분은 사용자가 롤을 치려는 의도(rates_sp)가 관측되는 즉시 예측 가능한 힘(토크)을 선제적으로 시스템에 주입한다. 이로인해 아크로 모드에서의 조종 체감 레이턴시(Latency)가 ’제로’에 수렴하게 된다.
엄격한 안티 와인드업(Anti-Windup):
외부 외란이나 극한 기동으로 인해 믹서가 최대 모터 출력값에 도달한 상태(_mixer_saturated == true)에서는, 코드가 절대 적분값(_integral_error)을 누적하지 않는다. 이를 통해 기체가 한계 상황에서 벗어났을 때 반대 방향으로 미친듯이 튀어버리는 제어 오버슈트(Overshoot) 현상을 성공적으로 억제한다.

3. Ardupilot-AC_AttitudeControl의 내부 루프와의 차별점

Ardupilot은 전통적으로 Attitude(자세각)와 Rate(각속도) 루프가 AC_AttitudeControl이라는 하나의 거대한 라이브러리 객체 안에 결합된 모놀리식(Monolithic) 성향을 보인다.
반면에 PX4는 Rate Control 모듈 컴포넌트를 완전히 src/modules/mc_rate_control/로 격리하여 마이크로서비스 관점을 지향한다. 이를 통해 멀티로터뿐만 아니라 VTOL(수직이착륙 고정익) 기체에서도 mc_rate_control 내부 C++ 바이너리를 재사용할 수 있으며, ROS2 환경의 오프보드(Offboard) 노드에서 자세(Attitude)를 무시하고 이 모듈로 목표 각속도를 바로 때려 넣는 독립 제어 확장이 무궁무진하게 가능하다.

29.5.3. 각속도 제어기(Rate Controller - Inner Loop) 핵심 소스 (src/modules/mc_rate_control/RateControl.cpp)