대한민국 해군 장보고-III(KSS-III) 배치-II 잠수함 급속 잠항(Emergency Dive) 및 중성부력(Neutral Buoyancy) 능동 트림·밸러스트 1,000Hz 0-Alloc C# 수심제어 엔진

제1장: 잠수함 6자유도 수중 운동학 및 부력·중력 모멘트 평형식

잠수함의 질량을 $m$, 부체 체적을 $\nabla$, 해수 밀도를 $\rho$, 중력가속도를 $g$, 무게를 $W = m g$, 부력을 $B = \rho g \nabla$라 하고, 무게중심 $G(x_g, y_g, z_g)$와 부심 $B_c(x_b, y_b, z_b)$를 정의할 때 수직 침강력(Heave)과 피칭 모멘트(Pitch Moment)의 동역학 방정식은 다음과 같습니다:

제2장: 급속 잠항(Crash Dive) 시퀀스 및 수온약층 밀도 보상 알고리즘

적 대잠 위협 발생 시 신속하게 수면을 탈출하여 순항 심도로 전환하는 4단계 급속 잠항 제어 전략은 다음과 같습니다:

제3장: 1,000Hz 0-Alloc C# KSS-III 수심·트림 제어 엔진 구현

1ms 주기로 6자유도 센서 데이터를 읽고, 네거티브 탱크 블로우 밸브, 양방향 트림 펌프 및 X자 선미타 믹싱을 GC 할당 없이 결정론적으로 제어하는 순수 C# 코어입니다:

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.Kss3Submarine
{
    /// <summary>
    /// KSS-III 잠수함 실시간 수중 항법 텔레메트리 (0-Alloc stack-only)
    /// </summary>
    public readonly struct SubmarineTelemetry
    {
        public readonly float CurrentDepthM;    // 현재 수심 z (m)
        public readonly float DepthRateMps;     // 수직 하강/상승 속도 z_dot (m/s)
        public readonly float PitchAngleRad;    // 피치각 theta (rad)
        public readonly float PitchRateRps;     // 피치 각속도 q (rad/s)
        public readonly float ForwardSpeedMps;  // 전진 속도 u (m/s)
        public readonly float AmbientWaterDensity; // 주변 해수 밀도 rho (kg/m^3)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SubmarineTelemetry(float depth, float depthRate, float pitch, float pitchRate, float speed, float density)
        {
            CurrentDepthM = depth;
            DepthRateMps = depthRate;
            PitchAngleRad = pitch;
            PitchRateRps = pitchRate;
            ForwardSpeedMps = speed;
            AmbientWaterDensity = density;
        }
    }

    /// <summary>
    /// 밸러스트 펌프 및 타면 제어 명령 출력 구조체
    /// </summary>
    public readonly struct SubmarineControlOutput
    {
        public readonly float SailPlaneRad;       // 세일 잠항타 각도
        public readonly float SternPlaneRad;      // 선미타 피치 각도
        public readonly float TrimPumpFlowKgSec;  // 전·후 트림 이송 유량 (+: 전방->후방)
        public readonly bool NegativeTankBlow;   // 네거티브 탱크 긴급 배출(Blow) 명령
        public readonly int DivePhase;            // 급속 잠항 FSM 단계 (1~4)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SubmarineControlOutput(float sail, float stern, float pump, bool blow, int phase)
        {
            SailPlaneRad = sail;
            SternPlaneRad = stern;
            TrimPumpFlowKgSec = pump;
            NegativeTankBlow = blow;
            DivePhase = phase;
        }
    }

    /// <summary>
    /// 장보고-III 배치-II 1,000Hz 0-Alloc 급속 잠항 & 중성부력 트림 제어 코어
    /// </summary>
    public ref struct Kss3SubmarineDepthEngine
    {
        // 3,600톤급 KSS-III 배치-II 잠수함 제원 상수
        public const float SubmarineMassKg = 3600000.0f; // 3,600톤
        public const float HullCompressFactor = 0.45f;   // 선체 체적 압축 계수 (m^3 / 100m)
        public const float MaxPumpRateKgSec = 80.0f;     // 양방향 트림 펌프 최대 이송률 (80kg/s)
        public const float MaxRudderRad = 0.436f;        // 최대 타각 (25도 = 0.436 rad)

        /// <summary>
        /// 1,000Hz 결정론적 급속 잠항 및 중성부력 트림 제어 명령 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static SubmarineControlOutput EvaluateControl(
            in SubmarineTelemetry nav,
            float targetDepthM,
            ref int internalDiveState)
        {
            float depthErr = targetDepthM - nav.CurrentDepthM;
            float depthRate = nav.DepthRateMps;
            float pitch = nav.PitchAngleRad;
            float pitchRate = nav.PitchRateRps;

            float sailCmd = 0.0f;
            float sternCmd = 0.0f;
            float pumpRate = 0.0f;
            bool blowNegative = false;

            // 1. 급속 잠항 FSM 상태 전이 로직
            if (internalDiveState == 0 && depthErr > 30.0f)
            {
                internalDiveState = 1; // 1단계: 초기 플러딩 및 네거티브 탱크 적재
            }

            if (internalDiveState == 1)
            {
                // 1단계: 선수 숙임 강제 생성 및 급속 하강 유도
                sailCmd = -0.262f;  // 세일타 -15도 (하강)
                sternCmd = 0.209f;   // 선미타 +12도 (선수 숙임 피치)

                // 목표 수심 75% 도달 시 2단계로 전이
                if (nav.CurrentDepthM >= targetDepthM * 0.75f)
                {
                    internalDiveState = 2;
                }
            }
            else if (internalDiveState == 2)
            {
                // 2단계: 네거티브 탱크 해수 고압 공기 배출(Blow) 및 하강 제동
                blowNegative = true;
                sailCmd = 0.175f;   // 세일타 +10도 (기수 들기)
                sternCmd = -0.175f; // 선미타 -10도

                if (MathF.Abs(depthRate) < 0.5f || nav.CurrentDepthM >= targetDepthM * 0.95f)
                {
                    internalDiveState = 3; // 3단계: 정밀 심도 안착 및 중성부력 모드
                }
            }
            else
            {
                // 3단계 / 4단계: 정상 중성부력 유지 및 피치 수평 제어 (LQR/PD 믹싱)
                blowNegative = false;

                // 심도 에러 및 수직 속도 감쇠를 통한 수직타 제어
                float zControl = (depthErr * 0.015f) - (depthRate * 0.08f);
                float pitchControl = -(pitch * 1.8f) - (pitchRate * 2.5f);

                sailCmd = Math.Clamp(zControl + pitchControl, -MaxRudderRad, MaxRudderRad);
                sternCmd = Math.Clamp(zControl - pitchControl, -MaxRudderRad, MaxRudderRad);

                // 2. 수온약층 밀도 변화 및 피치각 오차에 따른 능동 트림 펌프 Pumping
                // 피치가 음수(선수 숙임)이면 전방 트림 탱크의 해수를 후방으로 이송 (+유량)
                float desiredPump = (-pitch * 250.0f) - (pitchRate * 180.0f) + (depthErr * 1.5f);
                pumpRate = Math.Clamp(desiredPump, -MaxPumpRateKgSec, MaxPumpRateKgSec);
            }

            return new SubmarineControlOutput(
                sailCmd,
                sternCmd,
                pumpRate,
                blowNegative,
                internalDiveState
            );
        }
    }
}

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