AIDT 온디바이스 국방 MUM-T 무인수상정(USV) 전방감시소나(FLS) 기반 천해 암초 및 수중 장애물 실시간 경로 재계획 서보 엔진

1. 개요

연안 작전 및 서해안 도서 정찰을 수행하는 무인수상정(Unmanned Surface Vehicle, USV)은 조수간만의 차가 크고 수많은 간출암과 침몰 암초, 폐어망 및 통발이 밀집된 천해(Shallow Water) 환경을 고속(30노트 이상)으로 항해합니다. 레이더와 광학 EO 카메라는 수면 아래에 잠긴 암초나 반잠수 장애물을 탐지할 수 없으므로, 선수 하부에 장착된 3차원 전방감시소나(Forward-Looking Sonar, FLS)의 90° 원뿔형 음향 빔 데이터를 실시간 처리해야 합니다. 소나 신호로부터 추출된 장애물 복셀(Voxel) 맵을 바탕으로 조파 저항과 암초 충돌 반경을 동시 회피하는 비선형 동적 궤적을 재계획하고, 파도 요동 속에서도 정확한 타각(Rudder)을 생성해야 합니다. 본 고에서는 10,000Hz 주기로 장애물 상대 벡터를 평가하여 긴급 충돌회피 타각을 생성하는 0-Alloc C# 엔진을 구현합니다.

2. 수학적 모델링 및 지배 방정식

3D 전방감시소나(FLS)의 수신 빔 방위각 \(\phi_a\), 고저각 \(\theta_e\), 도달 거리 \(R_k\)로부터의 3차원 장애물 좌표 방정식은 다음과 같습니다:

\begin{pmatrix} X_{obs} \\ Y_{obs} \\ Z_{obs} \end{pmatrix}_{body} = R_k \begin{pmatrix} \cos\theta_e \cos\phi_a \\ \cos\theta_e \sin\phi_a \\ \sin\theta_e \end{pmatrix}, \quad Z_{obs} \ge h_{draft} + d_{safety}

USV 수평면 3자유도(전진, 좌우 횡이동, 선수 회전) 비선형 노모토-노르빈(Nomoto-Norrbin) 운동방정식은 다음과 같습니다:

T_r \ddot{\psi} + \dot{\psi} + \alpha_N \dot{\psi}^3 = K_r \delta_r(t) - K_w v_{wave}^2 \sin(\psi - \psi_{wave})

인공 포텐셜 장(APF) 기반 장애물 회피 척력 모멘트 및 안전 회피 거리 제약식은 다음과 같습니다:

\tau_{rep}(t) = \sum_{k=1}^{N_{obs}} \eta_{rep} \left( \frac{1}{d_k} - \frac{1}{d_{safe}} \right) \frac{1}{d_k^2} \text{sgn}(Y_{obs, k}) \quad (d_k \le d_{safe})

선수각 오차 영점화 및 장애물 긴급 회피 통합 타각 서보 지령식은 다음과 같습니다:

\delta_r^{cmd}(t) = K_p (\psi_{target} - \psi_{curr}) - K_d \dot{\psi} + K_{sonar} \tau_{rep}(t) \quad (\|\delta_r\| \le 35^\circ)

3. 실시간 C# 서보 제어기 구현

다음은 전방감시소나 빔 검출 데이터와 GPS/INS 항법 상태를 10,000Hz 주기로 분석하여 조타기 서보 각도를 지령하는 0-Alloc C# 엔진 구현체입니다:

UsvSonarObstacleAvoidanceServoEngine.cs

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.UsvNavigation
{
    public readonly ref struct SonarObstacleFrame
    {
        public readonly double NearestObstacleDistanceMeters;
        public readonly double NearestObstacleBearingDeg;
        public readonly double WaterDepthBelowKeelMeters;
        public readonly double CurrentSpeedKnots;
        public readonly double CurrentHeadingDegrees;
        public readonly double WaypointHeadingDegrees;

        public SonarObstacleFrame(
            double nearestObstacleDistanceMeters,
            double nearestObstacleBearingDeg,
            double waterDepthBelowKeelMeters,
            double currentSpeedKnots,
            double currentHeadingDegrees,
            double waypointHeadingDegrees)
        {
            NearestObstacleDistanceMeters = nearestObstacleDistanceMeters;
            NearestObstacleBearingDeg = nearestObstacleBearingDeg;
            WaterDepthBelowKeelMeters = waterDepthBelowKeelMeters;
            CurrentSpeedKnots = currentSpeedKnots;
            CurrentHeadingDegrees = currentHeadingDegrees;
            WaypointHeadingDegrees = waypointHeadingDegrees;
        }
    }

    public ref struct UsvSteeringCommand
    {
        public double CommandedRudderAngleDegrees;
        public double CommandedThrottlePercent;
        public double HeadingErrorDegrees;
        public bool IsObstacleCritical;
        public bool IsKeelClearanceSafe;
    }

    public ref struct UsvSonarObstacleAvoidanceServoEngine
    {
        private const double SafeDistanceMeters = 80.0;
        private const double CriticalDistanceMeters = 30.0;
        private const double MinKeelClearanceMeters = 2.5;
        private readonly double _kpHeading;
        private readonly double _kAvoidance;
        private readonly double _dt;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UsvSonarObstacleAvoidanceServoEngine(double kpHeading, double kAvoidance, double dt)
        {
            _kpHeading = kpHeading;
            _kAvoidance = kAvoidance;
            _dt = dt;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeSteeringAvoidance(in SonarObstacleFrame frame, out UsvSteeringCommand cmd)
        {
            cmd = default;
            double headingErr = frame.WaypointHeadingDegrees - frame.CurrentHeadingDegrees;
            while (headingErr > 180.0) headingErr -= 360.0;
            while (headingErr < -180.0) headingErr += 360.0;

            // Nominal heading control rudder
            double rudderCmd = _kpHeading * headingErr;

            // Sonar obstacle repulsion
            double dObs = frame.NearestObstacleDistanceMeters;
            bool isCritical = dObs < CriticalDistanceMeters;
            double throttleCmd = 100.0;

            if (dObs < SafeDistanceMeters && dObs > 1.0)
            {
                // Repel away from obstacle bearing
                double repelDirection = frame.NearestObstacleBearingDeg >= 0 ? -1.0 : 1.0;
                double repelMag = _kAvoidance * ((1.0 / dObs) - (1.0 / SafeDistanceMeters)) * 100.0;
                repelMag = Math.Clamp(repelMag, 0.0, 30.0);

                rudderCmd += repelDirection * repelMag;

                // Slow down throttle
                throttleCmd = Math.Clamp(dObs / SafeDistanceMeters * 100.0, 30.0, 100.0);
            }

            if (isCritical)
            {
                throttleCmd = 10.0; // Emergency slow speed
            }

            rudderCmd = Math.Clamp(rudderCmd, -35.0, 35.0);

            cmd.CommandedRudderAngleDegrees = rudderCmd;
            cmd.CommandedThrottlePercent = throttleCmd;
            cmd.HeadingErrorDegrees = headingErr;
            cmd.IsObstacleCritical = isCritical;
            cmd.IsKeelClearanceSafe = frame.WaterDepthBelowKeelMeters >= MinKeelClearanceMeters;
        }
    }
}

4. 교육 및 실무 적용 방안

해군사관학교 및 국방과학연구소: 무인수상정 천해 자율운항, 3차원 전방감시소나 신호처리 및 충돌회피 알고리즘 교육.
연안방어 무인수상정 전력화 사업: 암초/어망 자동 회피 온디바이스 자율제어기 소프트웨어 실장.