대한민국 공군/방공 천궁-II(M-SAM) 콜드런치 360도 측추력 피치오버(Pitch-Over) 제어 및 1,000Hz 0-Alloc C# DACS 유도 엔진

1. 콜드런치 사출 정점 강체 회전 동역학 및 토크 모델

유도탄 주관성 모멘트 $\mathbf{J}_{\text{body}} = \operatorname{diag}(I_{xx}, I_{yy}, I_{zz})$, 각속도 벡터 $\boldsymbol{\omega}$, DACS 노즐별 추력 $F_k$, 모멘트 암 $l_k$:

사출 정점 극저속 구간($\rho v^2 / 2 \approx 0$)에서 공력 모멘트 $\mathbf{M}_{\text{aero}} \to 0$이므로 순수 추력 모멘트에 의해 피치오버 완료 시간 $t_{\text{over}} \le 0.45\,\text{s}$가 결정됩니다.

2. 쿼터니언 오차 기반 DACS 펄스폭 변조(PWPF) 분배식

목표 자세 쿼터니언 $\mathbf{q}_d$와 현재 쿼터니언 $\mathbf{q}$ 사이의 오차 쿼터니언 $\mathbf{q}_e = \mathbf{q}^{-1} \otimes \mathbf{q}_d$:

3. 1,000Hz 0-Alloc SIMD C# 천궁-II 콜드런치 DACS 엔진

1밀리초 주기로 IMU 쿼터니언을 수신하여 8채널 DACS 밸브 구동 펄스를 연산하는 실시간 C# 코어입니다.

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

namespace ZeliDesk.Defense.MissileColdLaunch
{
    /// 

    /// 사출 텔레메트리 (Stack 전용)
    /// 
    public readonly ref struct CheongungLaunchTelemetry
    {
        public readonly Quaternion CurrentOrientation; // 현재 자세 쿼터니언
        public readonly Quaternion TargetOrientation;  // 표적 지향 목표 쿼터니언
        public readonly Vector3 AngularVelocityRadS;   // 각속도 (rad/s)
        public readonly float AltitudeMeters;          // 사출 고도 (m)
        public readonly float TimeSinceEjectionSec;    // 사출 경과 시간 (s)

        public CheongungLaunchTelemetry(
            in Quaternion current, in Quaternion target, in Vector3 omega, float alt, float time)
        {
            CurrentOrientation = current;
            TargetOrientation = target;
            AngularVelocityRadS = omega;
            AltitudeMeters = alt;
            TimeSinceEjectionSec = time;
        }
    }

    /// 

    /// DACS 측추력기 밸브 구동 지령
    /// 
    public readonly ref struct CheongungDacsOutput
    {
        public readonly Vector3 CommandedTorqueNm;     // 3축 요구 제어 토크 (N*m)
        public readonly byte ActiveValveMask;          // 8채널 밸브 ON/OFF 비트마스크
        public readonly bool ReadyForMainIgnition;     // 주 로켓모터 점화 허가 신호
        public readonly float OrientationErrorDeg;     // 잔류 지향 오차 각도 (도)

        public CheongungDacsOutput(in Vector3 torque, byte mask, bool ignite, float errDeg)
        {
            CommandedTorqueNm = torque;
            ActiveValveMask = mask;
            ReadyForMainIgnition = ignite;
            OrientationErrorDeg = errDeg;
        }
    }

    /// 

    /// 천궁-II 1,000Hz 0-Alloc C# 콜드런치 DACS 엔진
    /// 
    public static class CheongungColdLaunchEngine
    {
        private const float Kp = 480.0f;
        private const float Kd = 55.0f;
        private const float AlignmentToleranceDeg = 3.5f; // 점화 허용 정렬 공차

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static CheongungDacsOutput ComputeDacsControl(
            in CheongungLaunchTelemetry telemetry)
        {
            // 1. 쿼터니언 오차 계산: q_err = inv(q) * q_d
            Quaternion invQ = Quaternion.Inverse(telemetry.CurrentOrientation);
            Quaternion qErr = Quaternion.Multiply(invQ, telemetry.TargetOrientation);

            if (qErr.W < 0.0f)
            {
                qErr = -qErr;
            }

            Vector3 errorVec = new Vector3(qErr.X, qErr.Y, qErr.Z);
            float angleRad = 2.0f * MathF.Atan2(errorVec.Length(), MathF.Abs(qErr.W));
            float angleDeg = angleRad * (180.0f / MathF.PI);

            // 2. PD 토크 계산
            Vector3 torque = (2.0f * Kp * errorVec) - (Kd * telemetry.AngularVelocityRadS);

            // 3. 8개 DACS 밸브 점화 비트마스크 생성
            byte valveMask = 0;
            if (torque.Y > 50.0f)  valveMask |= 0x01; // Pitch Up
            if (torque.Y < -50.0f) valveMask |= 0x02; // Pitch Down
            if (torque.Z > 50.0f)  valveMask |= 0x04; // Yaw Right
            if (torque.Z < -50.0f) valveMask |= 0x08; // Yaw Left

            // 사출 후 0.4초 이상 경과하고 자세 정렬 완료 시 주모터 점화 허가
            bool readyIgnite = (angleDeg <= AlignmentToleranceDeg) && (telemetry.TimeSinceEjectionSec >= 0.35f);

            return new CheongungDacsOutput(torque, valveMask, readyIgnite, angleDeg);
        }
    }
}

4. 사출 정점 360도 전방위 피치오버 비행 시험 데이터