대한민국 해군 무인잠수정(UUV) 심해 음향-광학 융합 자율 유도 및 모함 수중 도킹 1,000Hz 0-Alloc C# 유도제어 엔진

제1장: 심해 USBL 음향 측위 및 근거리 광학 비전 센서 융합

모함에 장착된 음향 트랜스폰더와 UUV 수신기 사이의 음향 전파 시간차(ToF) 및 위상차($\Delta \phi$)로부터 산출된 USBL 상대 위치와, 카메라 투영 행렬을 통한 광학 마커 위치의 오차 공분산 융합은 다음과 같습니다:

제2장: 모함 후류 흡입 와류 극복 및 비선형 슬라이딩 모드 제어(SMC)

모함 선미 프로펠러 및 선체 주위의 불균일 후류장 $w_{\text{wake}}$ 속에서 도킹 축선 오차 $\mathbf{e} = [x_e, y_e, z_e]^T$를 0으로 수렴시키기 위한 슬라이딩 곡면 $\mathbf{s}$와 제어 입력 $\mathbf{u}$는 다음과 같습니다:

제3장: 1,000Hz 0-Alloc C# UUV 자율 도킹 제어 엔진 아키텍처

수중 6자유도 동역학 및 슬라이딩 모드 추력을 1ms 주기로 무할당 계산하는 순수 C# 구조체입니다:

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

namespace ZeliDesk.Defense.UuvDocking
{
    /// <summary>
    /// UUV 항법 상태 및 모함 도킹 콘 상대 위치 데이터 (0-Alloc)
    /// </summary>
    public readonly struct UuvDockingSensorState
    {
        public readonly Vector3 RelativePosM;   // 도킹 입구 기준 UUV 3D 상대 위치 (m)
        public readonly Vector3 RelativeVelMps; // 3D 상대 속도 벡터 (m/s)
        public readonly float RollRad;          // 탄체 롤 각도 (rad)
        public readonly float PitchRad;         // 피치 각도 (rad)
        public readonly float YawRad;           // 요 각도 (rad)
        public readonly bool IsOpticalLocked;   // 광학 마커 락온 상태 (10m 이내)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UuvDockingSensorState(Vector3 pos, Vector3 vel, float roll, float pitch, float yaw, bool optLock)
        {
            RelativePosM = pos;
            RelativeVelMps = vel;
            RollRad = roll;
            PitchRad = pitch;
            YawRad = yaw;
            IsOpticalLocked = optLock;
        }
    }

    /// <summary>
    /// 수중 추진기 5축 출력 지령 구조체 (N)
    /// </summary>
    public readonly struct UuvActuatorCommand
    {
        public readonly float MainSurgeThrustN;   // 주 추진기 전진 추력 (N)
        public readonly float BowSwayThrustN;     // 선수 좌우 횡추력기 (N)
        public readonly float SternSwayThrustN;   // 선미 좌우 횡추력기 (N)
        public readonly float BowHeaveThrustN;    // 선수 상하 수직추력기 (N)
        public readonly float SternHeaveThrustN;  // 선미 상하 수직추력기 (N)
        public readonly bool IsLatchedSuccessfully;// 최종 어뢰관 래칭 완료 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UuvActuatorCommand(float surge, float bSway, float sSway, float bHeave, float sHeave, bool latched)
        {
            MainSurgeThrustN = surge;
            BowSwayThrustN = bSway;
            SternSwayThrustN = sSway;
            BowHeaveThrustN = bHeave;
            SternHeaveThrustN = sHeave;
            IsLatchedSuccessfully = latched;
        }
    }

    /// <summary>
    /// 1,000Hz 결정론적 UUV 심해 자율 도킹 제어 엔진
    /// </summary>
    public ref struct UuvAutonomousDockingEngine
    {
        private const float UuvMassKg = 320.0f;        // UUV 질량 (320kg)
        private const float LambdaPos = 1.8f;          // 슬라이딩 곡면 계수
        private const float SmcGainK = 180.0f;         // SMC 불연속 이득 (N)
        private const float BoundaryLayerEps = 0.05f;  // 경계층 두께 (5cm)
        private const float MaxThrustN = 250.0f;       // 단일 추력기 최대 추력

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UuvActuatorCommand UpdateDocking(ref UuvDockingSensorState state, float dt)
        {
            Vector3 posError = state.RelativePosM;
            Vector3 velError = state.RelativeVelMps;

            // 1. 슬라이딩 곡면 벡터 s = velError + Lambda * posError
            Vector3 sVector = velError + posError * LambdaPos;

            // 2. 경계층 채터링 방지 포화 함수 sat(s / eps)
            Vector3 satS = new Vector3(
                Math.Clamp(sVector.X / BoundaryLayerEps, -1.0f, 1.0f),
                Math.Clamp(sVector.Y / BoundaryLayerEps, -1.0f, 1.0f),
                Math.Clamp(sVector.Z / BoundaryLayerEps, -1.0f, 1.0f)
            );

            // 3. 요구 제어력 계산 F_cmd = -K * sat(s) - Damping * vel
            float surgeForce = -SmcGainK * satS.X - 45.0f * velError.X;
            float swayForce  = -SmcGainK * satS.Y - 65.0f * velError.Y;
            float heaveForce = -SmcGainK * satS.Z - 65.0f * velError.Z;

            // 도킹 콘 진입 속도 프로파일 스케줄링 (진입 시 0.2m/s 서행)
            if (posError.X < 2.0f && posError.X > 0.0f)
            {
                float targetSurgeSpeed = 0.25f;
                surgeForce = (targetSurgeSpeed - state.RelativeVelMps.X) * 200.0f;
            }

            surgeForce = Math.Clamp(surgeForce, -MaxThrustN, MaxThrustN);
            swayForce  = Math.Clamp(swayForce,  -MaxThrustN, MaxThrustN);
            heaveForce = Math.Clamp(heaveForce, -MaxThrustN, MaxThrustN);

            // 추력기 5축 분배 (선수/선미 대칭 분배)
            float bSway = swayForce * 0.5f;
            float sSway = swayForce * 0.5f;
            float bHeave = heaveForce * 0.5f;
            float sHeave = heaveForce * 0.5f;

            // 최종 래칭 판정 (도킹 중심 축선 오차 3cm 이내 진입 완료)
            float alignmentMissM = MathF.Sqrt(posError.Y * posError.Y + posError.Z * posError.Z);
            bool isLatched = (posError.X <= 0.05f && posError.X >= -0.10f) && (alignmentMissM <= 0.035f);

            return new UuvActuatorCommand(surgeForce, bSway, sSway, bHeave, sHeave, isLatched);
        }
    }
}

제4장: 심해 도킹 콘 진입 및 선체 후류 보상 시계열 검증

3노트로 서행 중인 KSS-III 잠수함 후방 50m 지점에서 UUV가 어뢰관으로 진입하는 1,000Hz 유도 시계열 데이터입니다: