29.5.3.2. PID 연산 코어 (`src/lib/pid/pid.cpp` 호출부): 오차 계산, 비례항, 적분항 누적, 저주파 통과 필터가 적용된 미분항 연산의 순차적 시퀀스 분석

29.5.3.2. PID 연산 코어 (src/lib/pid/pid.cpp 호출부): 오차 계산, 비례항, 적분항 누적, 저주파 통과 필터가 적용된 미분항 연산의 순차적 시퀀스 분석

멀티로터의 아크로(Acro) 모드를 포함한 모든 회전익 수동 제어의 근간은 비례-적분-미분(PID) 제어기다. PX4-Autopilot은 제어 로직의 재사용성과 수학적 무결성을 보장하기 위해, PID 연산의 핵심 수학 수식을 src/lib/pid/pid.cpp라는 공용 라이브러리로 분리하여 관리한다.

본 절에서는 mc_rate_control 모듈이 이 PID 라이브러리를 호출하여 3차원 각속도 오차를 실제 모터 토크 제어량으로 변환하는 순차적 연산 시퀀스를 소스 코드 레벨에서 상세히 분석한다.

1. PID 제어 연산 시퀀스 개요

PX4의 Rate Controller 내부에서 매 루프마다 호출되는 단일 축(Single Axis) PID 연산은 다음과 같은 정교한 4단계 파이프라인을 거친다.

  1. 오차(Error) 연산: 목표 궤적과 실제 센서 계측치 간의 차이 계산
  2. 비례항(Proportional Term) 연산: 현재 오차에 대한 즉각적이고 선형적인 모터 출력 반응
  3. 적분항(Integral Term) 누적 연산: 과거 오차의 누적을 통한 정상 상태 오차(Steady-state Error) 보상
  4. 미분항(Derivative Term) 연산: 미래 오차 변동성을 예측하여 격렬한 움직임에 제동(Damping)을 거는 과정 (저주파 통과 필터 포함)

2. 핵심 소스 코드 분석 (pid_calculate)

다음 코드는 PX4의 표준 PID 수학 라이브러리인 src/lib/pid/pid.cpp에 구현된 pid_calculate() 함수의 뼈대이다.

// src/lib/pid/pid.cpp 내 PID 연산 핵심 흐름

float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
    // 1. 오차 연산 (목표값 - 현재 센서값)
    float error = sp - val;

    // 2. 비례항(P-Term) 연산
    // 오차에 비례 게인(Kp)을 곱하여 즉각적인 반응성 확보
    float output = error * pid->kp;

    // 3. 미분항(D-Term) 연산 및 노이즈 억제 필터
    if (pid->kd > 0.0f) {
        // 미분항의 노이즈 폭주를 막기 위해 D-Term 전용 Low-Pass 필터(지수 가중 이동 평균) 적용
        // val_dot은 각속도의 차분(또는 자이로스코프에서 직접 측정한 가속도)
        float derivative = val_dot;
        
        // 저주파 통과 필터 최신화
        pid->derivative_filter_state = pid->derivative_filter_state + 
                                       pid->derivative_filter_alpha * (derivative - pid->derivative_filter_state);
        
        // 필터링된 미분값에 미분 게인(Kd)을 곱하여 기존 출력값에 가산(음의 피드백 제동 역할)
        output += pid->derivative_filter_state * pid->kd;
    }

    // 4. 적분항(I-Term) 누적 연산
    if (pid->ki > 0.0f) {
        // 이전 프레임까지 누적된 적분값에 델타 시간(dt)을 곱한 현재 오차를 더함
        pid->integral += error * dt;

        // 적분 포화 방지(Anti-Windup)를 위한 제한(Clipping)
        pid->integral = math::constrain(pid->integral, -pid->integral_limit, pid->integral_limit);

        // 결과물에 적분 게인(Ki)을 곱하여 최종 출력에 가산
        output += pid->integral * pid->ki;
    }

    return output;
}

2.1 각 연산 시퀀스별 동역학적 의미

  1. 오차 계산 및 비례항(P) 적용:
    제어기의 가장 기초 세팅이다. 조종사가 스틱을 밀면(목표 각속도 sp 발생), 기체가 회전하지 않는 한 오차(error)가 최대로 발생한다. 이 오차 통계에 K_p가 1:1 곱해져 모터의 초기 가속 토크를 형성한다. 값이 높을수록 스틱 반응(Snappiness)이 예리해진다.
  2. 미분항(D) 연산과 저주파 통과 필터(LPF):
    PX4의 PID 코어에서 가장 주목할 부분은 “오차의 미분(Derivative of Error)“을 사용하지 않고 “계측값의 미분(Derivative of Measurement, val_dot)” 을 사용한다는 점이다. 목표값(스틱 입력)이 계단 함수처럼(Step-change) 급변할 때 미분항이 무한대로 튀는 킥(Derivative Kick) 현상을 방지하기 위함이다.
    또한, 자이로스코프 센서 노이즈가 D항을 거치며 엄청나게 증폭되는 현상(모터 과열의 주범)을 막기 위해 1차 저주파 통과 필터(derivative_filter_alpha)가 펌웨어 레벨에서 수학적으로 강제 적용되어 있다.
  3. 적분항(I) 동작 절차:
    기체의 무게 중심이 안 맞거나 바람 등의 외부 저항이 있으면, P항과 D항만으로는 목표 각속도에 영원히 도달하지 못하는 ’유격’이 발생한다. K_i는 이 미세한 잔류 오차를 매 프레임(dt) 누적하여 거대한 힘으로 부풀려 출력 벡터에 가산함으로써, 기체의 회전 궤적을 끝까지 스틱에 일치시킨다.

3. Ardupilot 제어 아키텍처와의 비교

Ardupilot의 AC_PID 라이브러리 역시 위상 시퀀스나 수학적 목적은 완벽히 동일하다. 그러나 구현 철학의 차이가 존재한다.
Ardupilot은 AC_PID::get_pid()라는 거대한 메서드 안에서 P, I, D 게인 연산과 와인드업 검사를 위한 플래그 검사(Limit Flagging), Slew-rate 제한 등을 모두 한 덩어리로 묶어 블랙박스처럼 처리한다.
반면 PX4의 src/lib/pid/pid.cpp는 극단적일 정도로 절제된 C 스타일의 함수 원형 유지에 집중하며, 적분 포화(Wind-up)에 대한 시스템 레벨의 예외 처리(Actuator Saturation 방지 등) 판단 로직은 이 수학 라이브러리 밖인 RateControl 객체 본문에 위임(Delegating)하여 수학 단위 알고리즘과 비행 상태 로직을 완전히 캡슐화·분리(Decoupling)하였다는 아키텍처적 우수성을 갖는다.