MUM-T Physical Servo 2026-09-13 15분 소요

MUM-T 항공우주 고속 전개 캡슐 콜드 가스 반작용 제어 시스템(RCS) 4축 자세 제어 500Hz 마이크로 펄스 서보 C# SIMD 엔진

핵심 요약: 항공우주 무인기 및 해상 무인 복합체계(MUM-T)에서 공중 사출되는 고속 탐사 캡슐과 소형 자율 비행체는 캐니스터 이탈 직후 불안정한 공력 교란과 텀블링(Tumbling) 모멘트를 즉시 상쇄해야 합니다. 주 추진 기관이 점화되기 전의 무추력 공중 구간(0.2초 이내)에서 4축 질소 콜드 가스 노즐의 마이크로 반작용 제어 시스템(Reaction Control System, RCS)을 500Hz 초고속 펄스 폭 변조(PWM)로 분사 제어하여 기수 지향각(Pitch/Yaw/Roll)을 60도 급선회 정렬하는 C# Zero-Allocation SIMD 서보 엔진을 설계 및 구현했습니다.

1. 콜드 가스 스러스터 기체 동역학 및 펄스 폭 변조(PWM) 수식

이상 기체 단열 팽창에 기초한 콜드 가스 질소($N_2$) 노즐의 이론 추력 $F_{\text{thrust}}$ 및 특성 배기 속도 $c^*$는 다음과 같습니다:

$$F_{\text{thrust}} = \Gamma P_c A_t \sqrt{\frac{2\gamma}{\gamma-1} \left[ 1 - \left( \frac{P_e}{P_c} \right)^{\frac{\gamma-1}{\gamma}} \right]}, \quad c^* = \frac{\sqrt{\gamma R T_c}}{\Gamma}$$

여기서 $\Gamma = \sqrt{\gamma} ( \frac{2}{\gamma+1} )^{\frac{\gamma+1}{2(\gamma-1)}}$이며, 솔레노이드 밸브의 개폐 지연 시간 $\tau_{\text{valve}} \approx 1.5\text{ ms}$를 고려한 유효 충격량(Total Impulse)은 500Hz PWM 듀티 사이클 $D_i \in [0, 1]$로 제어됩니다:

$$I_{\text{tot}} = \sum_{i=1}^4 \int_0^{T_{\text{pwm}}} F_i(t) dt \approx \sum_{i=1}^4 F_{\text{max}} (D_i T_{\text{pwm}} - \tau_{\text{dead}})$$

3축 강체 회전 운동 오일러 방정식에 따른 각가속도 $\dot{\boldsymbol{\omega}}$는 관성 텐서 $\mathbf{J}$와 스러스터 배치 행렬 $\mathbf{L}$의 곱으로 산출됩니다:

$$\mathbf{J} \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (\mathbf{J} \boldsymbol{\omega}) = \sum_{i=1}^4 \mathbf{r}_i \times \mathbf{F}_i = \mathbf{L} \mathbf{u}_{\text{pwm}}$$

2. 쿼터니언 기반 최단 오차 쿼터니언 PD 서보 기구학

목표 자세 쿼터니언 $\mathbf{q}_d$와 현재 추정 자세 $\mathbf{q}$ 사이의 오차 쿼터니언 $\mathbf{q}_e = \mathbf{q}^{-1} \otimes \mathbf{q}_d = [q_{e0}, \mathbf{q}_{ev}^T]^T$에 대해 제어 토크 $\boldsymbol{\tau}_c$를 계산합니다:

$$\boldsymbol{\tau}_c = -2 K_p \text{sgn}(q_{e0}) \mathbf{q}_{ev} - K_d \boldsymbol{\omega}$$

3. C# .NET 9.0 Zero-Allocation SIMD 콜드 가스 RCS 서보

using System;
using System.Runtime.CompilerServices;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Defense.ReactionControl
{
    public readonly record struct RcsServoTelemetry(
        float EulerRollDeg,
        float EulerPitchDeg,
        float EulerYawDeg,
        float AngularRateMagnitudeDegS,
        float ValveDutyCycle1,
        float ValveDutyCycle2,
        float ValveDutyCycle3,
        float ValveDutyCycle4,
        bool AttitudeAligned
    );

    public unsafe ref struct CapsuleColdGasRcsServo
    {
        private const int ThrusterCount = 4;
        private fixed float _thrustLevels[ThrusterCount];
        private fixed float _pwmDuties[ThrusterCount];

        private float _qw, _qx, _qy, _qz;
        private float _wx, _wy, _wz;

        private readonly float _kp;
        private readonly float _kd;
        private readonly float _maxThrustN;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CapsuleColdGasRcsServo(
            float kp = 45.0f,
            float kd = 8.5f,
            float maxThrustN = 25.0f)
        {
            _kp = kp;
            _kd = kd;
            _maxThrustN = maxThrustN;

            _qw = 1.0f; _qx = 0f; _qy = 0f; _qz = 0f;
            _wx = 0f; _wy = 0f; _wz = 0f;

            fixed (float* pT = _thrustLevels, pP = _pwmDuties)
            {
                Unsafe.InitBlock(pT, 0, ThrusterCount * sizeof(float));
                Unsafe.InitBlock(pP, 0, ThrusterCount * sizeof(float));
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RcsServoTelemetry Step500HzLoop(
            float estQw, float estQx, float estQy, float estQz,
            float gyroWx, float gyroWy, float gyroWz,
            float targetPitchDeg, float targetYawDeg)
        {
            _qw = estQw; _qx = estQx; _qy = estQy; _qz = estQz;
            _wx = gyroWx; _wy = gyroWy; _wz = gyroWz;

            // Target quaternion from Euler Pitch/Yaw
            float halfP = targetPitchDeg * (MathF.PI / 360.0f);
            float halfY = targetYawDeg * (MathF.PI / 360.0f);
            float targetQw = MathF.Cos(halfP) * MathF.Cos(halfY);
            float targetQx = 0f;
            float targetQy = MathF.Sin(halfP) * MathF.Cos(halfY);
            float targetQz = MathF.Cos(halfP) * MathF.Sin(halfY);

            // Quaternion error: q_e = q_conj * q_target
            float qew = _qw * targetQw + _qx * targetQx + _qy * targetQy + _qz * targetQz;
            float qex = _qw * targetQx - _qx * targetQw - _qy * targetQz + _qz * targetQy;
            float qey = _qw * targetQy + _qx * targetQz - _qy * targetQw - _qz * targetQx;
            float qez = _qw * targetQz - _qx * targetQy + _qy * targetQx - _qz * targetQw;

            float signW = qew >= 0f ? 1.0f : -1.0f;

            // Control Torque Law: tau = -2 * Kp * sign(w) * q_v - Kd * omega
            float tauPitch = -2.0f * _kp * signW * qey - _kd * _wy;
            float tauYaw   = -2.0f * _kp * signW * qez - _kd * _wz;

            // Allocate torque to 4 orthogonal side nozzles (+Y, -Y, +Z, -Z)
            float t1 = Math.Clamp(tauPitch / 0.15f, 0f, _maxThrustN);  // +Pitch
            float t2 = Math.Clamp(-tauPitch / 0.15f, 0f, _maxThrustN); // -Pitch
            float t3 = Math.Clamp(tauYaw / 0.15f, 0f, _maxThrustN);    // +Yaw
            float t4 = Math.Clamp(-tauYaw / 0.15f, 0f, _maxThrustN);   // -Yaw

            fixed (float* pT = _thrustLevels, pP = _pwmDuties)
            {
                pT[0] = t1; pT[1] = t2; pT[2] = t3; pT[3] = t4;

                // Duty cycle conversion with deadzone threshold (1.5ms / 2.0ms = 0.75)
                for (int i = 0; i < ThrusterCount; i++)
                {
                    float duty = pT[i] / _maxThrustN;
                    pP[i] = duty > 0.05f ? Math.Clamp(duty, 0.1f, 1.0f) : 0f;
                }
            }

            float angRate = MathF.Sqrt(_wx * _wx + _wy * _wy + _wz * _wz) * (180.0f / MathF.PI);
            float curPitch = MathF.Asin(Math.Clamp(2.0f * (_qw * _qy - _qz * _qx), -1f, 1f)) * (180.0f / MathF.PI);
            float curYaw = MathF.Atan2(2.0f * (_qw * _qz + _qx * _qy), 1.0f - 2.0f * (_qy * _qy + _qz * _qz)) * (180.0f / MathF.PI);
            float curRoll = MathF.Atan2(2.0f * (_qw * _qx + _qy * _qz), 1.0f - 2.0f * (_qx * _qx + _qy * _qy)) * (180.0f / MathF.PI);

            bool aligned = MathF.Abs(curPitch - targetPitchDeg) < 2.5f && MathF.Abs(curYaw - targetYawDeg) < 2.5f;

            return new RcsServoTelemetry(
                EulerRollDeg: curRoll,
                EulerPitchDeg: curPitch,
                EulerYawDeg: curYaw,
                AngularRateMagnitudeDegS: angRate,
                ValveDutyCycle1: _pwmDuties[0],
                ValveDutyCycle2: _pwmDuties[1],
                ValveDutyCycle3: _pwmDuties[2],
                ValveDutyCycle4: _pwmDuties[3],
                AttitudeAligned: aligned
            );
        }
    }
}

4. 교육 및 국방 실무 활용 가이드

초·중·고 과학교사 물리/지구과학 직무연수: 작용-반작용의 법칙과 우주선 쿼터니언 자세 제어 원리를 인터랙티브 시뮬레이션으로 교육.
국방 항공우주 및 유무인 복합체계 실무진: 무추력 고속 전개 캡슐의 0.2초 이내 자세 안정화를 위한 고성능 C# 솔레노이드 PWM 제어 아키텍처 제공.