대한민국 공군 FA-50 블록-20 경전투기 공대지 타깃팅 포드(ATP) 3축 관성 안정화 짐벌 시선(LOS) 지향 및 EO/IR 1,000Hz 0-Alloc C# 조준추적 엔진

제1장: 3축 짐벌 기구학 및 기체 각속도 외란 분리 모델

전투기 기체 좌표계 $\{B\}$의 각속도 $\boldsymbol{\omega}_B = [p, q, r]^T$와 짐벌 외측 롤축($\phi_g$), 중간 요축($\psi_g$), 내측 피치축($\theta_g$) 관성 센서 $\{S\}$ 간의 3차원 좌표 변환 및 각속도 관계식은 다음과 같습니다:

제2장: EO/IR 영상 픽셀 미스거리 및 지상 표적 추적(Tracking) 폐루프

초당 60프레임 EO/IR 센서에서 추출된 표적 중심 픽셀 오차 $(\Delta u, \Delta v)$와 렌즈 초점거리 $f_{\text{lens}}$로부터 시선 오차각을 산출하고 1,000Hz로 보간하는 추적 제어 법칙은 다음과 같습니다:

제3장: 1,000Hz 0-Alloc C# FA-50 타깃팅 포드 짐벌 제어 엔진

3축 짐벌 자이로 피드백과 기체 비행 상태를 1ms 주기로 무할당 융합 연산하여 3축 모터 전류를 통제하는 순수 C# 실시간 엔진입니다:

using System;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.Fa50Atp
{
    /// <summary>
    /// 전투기 비행 상태 및 포드 센서 데이터 (0-Alloc stack-only)
    /// </summary>
    public readonly struct Fa50PodTelemetryState
    {
        public readonly float AircraftRollRateRps; // 기체 롤 각속도 p (rad/s)
        public readonly float AircraftPitchRateRps;// 기체 피치 각속도 q (rad/s)
        public readonly float AircraftYawRateRps;  // 기체 요 각속도 r (rad/s)
        public readonly float GimbalAzimuthDeg;    // 짐벌 방위각 psi (deg)
        public readonly float GimbalElevationDeg;  // 짐벌 고각 theta (deg)
        public readonly float GimbalRollDeg;       // 짐벌 롤각 phi (deg)
        public readonly float EoPixelErrorX;       // 영상 추적기 X축 픽셀 오차 (pixels)
        public readonly float EoPixelErrorY;       // 영상 추적기 Y축 픽셀 오차 (pixels)
        public readonly bool TargetLockEngaged;    // 표적 락온 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Fa50PodTelemetryState(
            float p, float q, float r, float az, float el, float roll,
            float errX, float errY, bool lockEngaged)
        {
            AircraftRollRateRps = p;
            AircraftPitchRateRps = q;
            AircraftYawRateRps = r;
            GimbalAzimuthDeg = az;
            GimbalElevationDeg = el;
            GimbalRollDeg = roll;
            EoPixelErrorX = errX;
            EoPixelErrorY = errY;
            TargetLockEngaged = lockEngaged;
        }
    }

    /// <summary>
    /// 3축 짐벌 모터 토크 출력 구조체
    /// </summary>
    public readonly struct GimbalMotorOutput
    {
        public readonly float TorqueRollNm;     // 롤축 모터 명령 토크 (Nm)
        public readonly float TorqueElevationNm;// 피치(고각) 모터 명령 토크 (Nm)
        public readonly float TorqueAzimuthNm;  // 요(방위각) 모터 명령 토크 (Nm)
        public readonly float LosJitterUrad;    // 시선 잔여 진동 지터 (microradians)
        public readonly bool IsTrackingStable;  // 안정 추적 판정 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public GimbalMotorOutput(float tRoll, float tEl, float tAz, float jitter, bool stable)
        {
            TorqueRollNm = tRoll;
            TorqueElevationNm = tEl;
            TorqueAzimuthNm = tAz;
            LosJitterUrad = jitter;
            IsTrackingStable = stable;
        }
    }

    /// <summary>
    /// FA-50 3축 관성 안정화 타깃팅 포드 1,000Hz 0-Alloc 제어 엔진
    /// </summary>
    public ref struct Fa50TargetingPodEngine
    {
        private const float MaxMotorTorqueNm = 45.0f; // 직구동 토크 모터 한계
        private const float FocalLengthMm = 1200.0f;  // 망원 렌즈 초점거리
        private const float PixelPitchUm = 12.0f;     // 디텍터 픽셀 피치

        // 3축 속도 적분기 상태
        private float _azRateIntegral;
        private float _elRateIntegral;
        private float _rollRateIntegral;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public GimbalMotorOutput Step1000Hz(in Fa50PodTelemetryState state, float dt)
        {
            // 1. 영상 센서 픽셀 오차 -> 시선 각도 오차 변환 (rad)
            float pixelScaleRad = (PixelPitchUm * 1e-3f) / FocalLengthMm;
            float losErrAz = state.EoPixelErrorX * pixelScaleRad;
            float losErrEl = state.EoPixelErrorY * pixelScaleRad;

            // 2. 표적 락온 시 외부 위치 루프 속도 명령 산출
            float cmdRateAz = 0.0f;
            float cmdRateEl = 0.0f;
            if (state.TargetLockEngaged)
            {
                cmdRateAz = losErrAz * 8.5f;
                cmdRateEl = losErrEl * 8.5f;
            }

            // 3. 기체 운동 외란 피드포워드 상쇄 (Disturbance Feedforward)
            // 기체의 롤/피치/요 회전을 짐벌 각도에 맞춰 투영
            float azRad = state.GimbalAzimuthDeg * (MathF.PI / 180.0f);
            float elRad = state.GimbalElevationDeg * (MathF.PI / 180.0f);

            float bodyRateEl = state.AircraftPitchRateRps * MathF.Cos(azRad) -
                               state.AircraftYawRateRps * MathF.Sin(azRad);
            float bodyRateAz = (state.AircraftPitchRateRps * MathF.Sin(azRad) +
                                state.AircraftYawRateRps * MathF.Cos(azRad)) / MathF.Cos(elRad);
            float bodyRateRoll = state.AircraftRollRateRps;

            // 4. 내부 1,000Hz 관성 속도 루프 오차 계산
            float errRateAz = (cmdRateAz - bodyRateAz);
            float errRateEl = (cmdRateEl - bodyRateEl);
            float errRateRoll = (0.0f - bodyRateRoll); // 수평선 롤 수평 유지

            _azRateIntegral = Math.Clamp(_azRateIntegral + errRateAz * dt, -5.0f, 5.0f);
            _elRateIntegral = Math.Clamp(_elRateIntegral + errRateEl * dt, -5.0f, 5.0f);
            _rollRateIntegral = Math.Clamp(_rollRateIntegral + errRateRoll * dt, -5.0f, 5.0f);

            // 5. 토크 모터 제어 명령 계산 (PID)
            float torqueAz = (errRateAz * 65.0f) + (_azRateIntegral * 18.0f);
            float torqueEl = (errRateEl * 75.0f) + (_elRateIntegral * 22.0f);
            float torqueRoll = (errRateRoll * 50.0f) + (_rollRateIntegral * 15.0f);

            // 6. 시선 지터(Jitter) 산출 (마이크로라디안 단위)
            float totalErrRad = MathF.Sqrt(losErrAz * losErrAz + losErrEl * losErrEl);
            float jitterUrad = totalErrRad * 1e6f;
            bool isStable = jitterUrad < 35.0f; // 35 urad 이내면 정밀 레이저 조사 안정 판정

            return new GimbalMotorOutput(
                Math.Clamp(torqueRoll, -MaxMotorTorqueNm, MaxMotorTorqueNm),
                Math.Clamp(torqueEl, -MaxMotorTorqueNm, MaxMotorTorqueNm),
                Math.Clamp(torqueAz, -MaxMotorTorqueNm, MaxMotorTorqueNm),
                jitterUrad,
                isStable
            );
        }
    }
}

제4장: 2022 개정 교육과정 연계 교과 융합 지도안 (교사용 가이드)