대한민국 해군 소해함·무인기뢰처리기(MDV) 고주파 합성개구소나(SAS) 심해 기뢰 탐지 해저면 영상화 및 와류 외란 보상 1,000Hz 0-Alloc C# 호버링 제어 엔진

제1장: 수중 음속 프로파일 및 합성개구소나(SAS) 개구면 합성 수식

심해 해수 매질에서 수온 $T(^\circ\text{C})$, 염분 $S(\text{ppt})$, 수심 $z(\text{m})$에 따른 음속 $c(T, S, z)$는 Mackenzie 공식에 의해 산출되며, 플랫폼 이동 속도 $v$와 핑 반복 주파수(PRF) 간 나이퀴스트 공간 샘플링 조건 및 합성개구 방위각 해상도 $\delta_x$는 다음과 같이 정의됩니다:

제2장: 포센(Fossen) 6자유도 유체동역학 및 외란 관측기(DOB) 호버링 제어

수중 무인기뢰처리기(MDV)의 6자유도 비선형 운동방정식은 강체 질량 $\mathbf{M}_{RB}$, 유체 부가질량 $\mathbf{M}_A$, 코리올리-구심력 $\mathbf{C}(\boldsymbol{\nu})$, 수동 저항 댐핑 $\mathbf{D}(\boldsymbol{\nu})$, 중력-부력 복원력 $\mathbf{g}(\boldsymbol{\eta})$의 결합으로 나타납니다:

제3장: 1,000Hz 0-Alloc C# MDV SAS 정밀 호버링 제어 엔진

1ms 실시간 주기에서 가비지 컬렉션(GC) 멈춤 없이 DVL, INS 센서 융합 데이터로부터 외란을 추정하고 4축 벡터드 쓰러스터를 제어하는 완전 무할당 C# 코어입니다:

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

namespace ZeliDesk.Defense.MdvHovering
{
    /// 

    /// MDV 6자유도 실시간 텔레메트리 (스택 전용 0-Alloc)
    /// 
    public readonly struct MdvTelemetry
    {
        public readonly Vector3 PositionM;       // 서지(X), 스웨이(Y), 심도(Z)
        public readonly Vector3 VelocityMps;     // 선체 기준 속도 (u, v, w)
        public readonly Vector3 EulerAnglesRad;  // 롤(phi), 피치(theta), 요(psi)
        public readonly Vector3 AngularRateRps;  // 각속도 (p, q, r)
        public readonly float WaterDensityKgM3;  // 해수 밀도 rho
        public readonly float TargetDepthM;      // 목표 심도 (m)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MdvTelemetry(Vector3 pos, Vector3 vel, Vector3 angles, Vector3 angRate, float density, float targetDepth)
        {
            PositionM = pos;
            VelocityMps = vel;
            EulerAnglesRad = angles;
            AngularRateRps = angRate;
            WaterDensityKgM3 = density;
            TargetDepthM = targetDepth;
        }
    }

    /// 

    /// 4축 수평/수직 벡터드 쓰러스터 제어 출력
    /// 
    public readonly struct ThrusterOutput
    {
        public readonly float BowPortN;      // 선수 좌현 쓰러스터 (N)
        public readonly float BowStbdN;      // 선수 우현 쓰러스터 (N)
        public readonly float SternPortN;    // 선미 좌현 쓰러스터 (N)
        public readonly float SternStbdN;    // 선미 우현 쓰러스터 (N)
        public readonly float VerticalForeN; // 수직 전방 쓰러스터 (N)
        public readonly float VerticalAftN;  // 수직 후방 쓰러스터 (N)
        public readonly bool IsPhaseCoherent;// SAS 개구면 위상 일관성 유지 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ThrusterOutput(float bp, float bs, float sp, float ss, float vf, float va, bool coherent)
        {
            BowPortN = bp;
            BowStbdN = bs;
            SternPortN = sp;
            SternStbdN = ss;
            VerticalForeN = vf;
            VerticalAftN = va;
            IsPhaseCoherent = coherent;
        }
    }

    /// 

    /// 해군 MDV 고주파 SAS 호버링 및 외란 상쇄 제어 엔진
    /// 
    public ref struct MdvSasHoveringEngine
    {
        private const float MassKg = 180.0f;          // 무인기뢰처리기 공기 중 질량
        private const float BuoyancyN = 182.0f * 9.81f;// 약 2% 양성 부력 (수중 안전성)
        private const float MetacentricZgZbM = 0.05f; // 메타센트릭 높이 (G-B 수직 거리 5cm)
        private const float CutoffFreqRad = 35.0f;    // 외란 관측기(DOB) 차단 주파수 (rad/s)
        private const float MaxDisplacementMm = 0.15f;// SAS 위상 일관성 허용 변위 (0.15mm)

        /// 

        /// 1,000Hz 결정론적 수중 6자유도 호버링 및 SAS 위상 왜곡 보상 연산
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static ThrusterOutput EvaluateHovering(
            in MdvTelemetry tel,
            ref Vector3 estDisturbanceForceN,
            ref Vector3 prevVelocityMps)
        {
            const float dt = 0.001f; // 1ms 제어 루프

            // 1. 가속도 추정 및 Fossen 댐핑력 산출
            Vector3 accel = (tel.VelocityMps - prevVelocityMps) / dt;
            prevVelocityMps = tel.VelocityMps;

            // 선체 수평/수직 항력 (Damping Force: D_lin * v + D_quad * v * |v|)
            Vector3 dampingForce = new Vector3(
                (25.0f * tel.VelocityMps.X) + (40.0f * tel.VelocityMps.X * MathF.Abs(tel.VelocityMps.X)),
                (35.0f * tel.VelocityMps.Y) + (55.0f * tel.VelocityMps.Y * MathF.Abs(tel.VelocityMps.Y)),
                (50.0f * tel.VelocityMps.Z) + (80.0f * tel.VelocityMps.Z * MathF.Abs(tel.VelocityMps.Z))
            );

            // 2. 부력-중력 복원력 계산
            float roll = tel.EulerAnglesRad.X;
            float pitch = tel.EulerAnglesRad.Y;
            float netBuoyancyN = BuoyancyN - (MassKg * 9.81f);
            float restoringPitchTorqueNm = -BuoyancyN * MetacentricZgZbM * MathF.Sin(pitch);
            float restoringRollTorqueNm = -BuoyancyN * MetacentricZgZbM * MathF.Sin(roll);

            // 3. 외란 관측기(DOB): 외력 = 관성력 + 항력 - 제어력 - 부력
            Vector3 totalInertialForce = (MassKg + 120.0f) * accel; // 부가질량 약 120kg 포함
            Vector3 rawDisturbance = totalInertialForce + dampingForce;
            
            // 1차 저역통과필터 기반 외란 적분 갱신
            float alpha = CutoffFreqRad * dt;
            estDisturbanceForceN = Vector3.Lerp(estDisturbanceForceN, rawDisturbance, alpha);

            // 4. 위치 오차 및 호버링 복원력 계산 (목표 속도 = 0)
            Vector3 posError = new Vector3(0.0f - tel.PositionM.X, 0.0f - tel.PositionM.Y, tel.TargetDepthM - tel.PositionM.Z);
            Vector3 cmdForce = (posError * 220.0f) - (tel.VelocityMps * 180.0f) - estDisturbanceForceN;

            // 심도 유지 힘에 순부력 상쇄 결합
            cmdForce.Z -= netBuoyancyN;

            // 5. 자세(Roll, Pitch, Yaw) 안정화 토크 계산
            float cmdYawTorque = -tel.EulerAnglesRad.Z * 45.0f - tel.AngularRateRps.Z * 25.0f;
            float cmdPitchTorque = -pitch * 35.0f - tel.AngularRateRps.Y * 18.0f - restoringPitchTorqueNm;

            // 6. 4축 수평 벡터드 쓰러스터 할당 (Cross configuration)
            float bp = (cmdForce.X * 0.35f) + (cmdForce.Y * 0.35f) - (cmdYawTorque * 0.5f);
            float bs = (cmdForce.X * 0.35f) - (cmdForce.Y * 0.35f) + (cmdYawTorque * 0.5f);
            float sp = (cmdForce.X * 0.35f) - (cmdForce.Y * 0.35f) - (cmdYawTorque * 0.5f);
            float ss = (cmdForce.X * 0.35f) + (cmdForce.Y * 0.35f) + (cmdYawTorque * 0.5f);

            // 7. 수직 2축 쓰러스터 할당 (Pitch & Heave 분배)
            float vf = (cmdForce.Z * 0.5f) + (cmdPitchTorque * 0.6f);
            float va = (cmdForce.Z * 0.5f) - (cmdPitchTorque * 0.6f);

            // 8. SAS 개구면 위상 일관성(Phase Coherence) 판정
            float currentDisplacementMm = MathF.Sqrt(posError.X * posError.X + posError.Y * posError.Y) * 1000.0f;
            bool isCoherent = currentDisplacementMm <= MaxDisplacementMm;

            return new ThrusterOutput(
                Math.Clamp(bp, -150.0f, 150.0f),
                Math.Clamp(bs, -150.0f, 150.0f),
                Math.Clamp(sp, -150.0f, 150.0f),
                Math.Clamp(ss, -150.0f, 150.0f),
                Math.Clamp(vf, -200.0f, 200.0f),
                Math.Clamp(va, -200.0f, 200.0f),
                isCoherent
            );
        }
    }
}

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