대한민국 육군 다목적 무인차량(Arion-SMET UGV) 야지 자율주행 3D 라이다 주행가능성 격자 지도(Costmap) & 원격사격통제체계(RCWS) 1,000Hz 0-Alloc C# 자율전투 엔진

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

namespace ZeliDesk.Defense.UnmannedGroundVehicle
{
    /// <summary>
    /// 3D 라이다 포인트 구조체 (16바이트 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public readonly struct LidarPoint
    {
        public readonly float X;
        public readonly float Y;
        public readonly float Z;
        public readonly float Intensity;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public LidarPoint(float x, float y, float z, float intensity)
        {
            X = x; Y = y; Z = z; Intensity = intensity;
        }
    }

    /// <summary>
    /// UGV 주행 및 RCWS 사격 제어 출력 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct UgvCombatControlOutput
    {
        public float TargetSteeringAngleRad; // 조향각 지령 (rad)
        public float TargetSpeedMps;         // 주행 속도 지령 (m/s)
        public float RcwsTurretAzimuthDeg;   // RCWS 방위각 지령 (deg)
        public float RcwsGunElevationDeg;    // RCWS 고각 지령 (deg)
        public float LocalMinCost;           // 최적 경로 비용 값
        public bool IsObstacleDetected;      // 전방 5m 내 저지 장애물 존재 여부
    }

    /// <summary>
    /// 아리온-스멧 UGV 야지 Costmap & RCWS 1,000Hz 0-Alloc C# 자율전투 엔진
    /// </summary>
    public sealed class ArionSmetCombatEngine
    {
        private const int GridSize = 64; // 64x64 국소 비용 격자 지도 (셀당 0.25m = 16m x 16m)
        private const float CellResolutionM = 0.25f;
        private const float MaxTraversableSlopeRad = 0.52f; // 최대 등판각 30도 (0.52 rad)
        private const float GroundClearanceM = 0.32f;       // 지상고 32cm

        // 사전 할당된 0-Alloc 2차원 비용 배열 (1D 선형 배열 매핑)
        private readonly float[] _costmap = new float[GridSize * GridSize];
        private readonly float[] _elevationMin = new float[GridSize * GridSize];
        private readonly float[] _elevationMax = new float[GridSize * GridSize];

        // RCWS 내부 필터 상태
        private float _currentTurretAzRad;
        private float _currentGunElRad;

        public ArionSmetCombatEngine()
        {
            Array.Clear(_costmap, 0, _costmap.Length);
            _currentTurretAzRad = 0f;
            _currentGunElRad = 0f;
        }

        /// <summary>
        /// 1,000Hz 실시간 틱: 라이다 포인트 클라우드 및 관성 센서로부터 주행 명령 및 RCWS 안정화 지령 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateTick(
            ReadOnlySpan<LidarPoint> points,
            float rollRateRps, float pitchRateRps, float yawRateRps, // 차체 자이로
            float targetAimAzRad, float targetAimElRad,             // 표적 조준선
            float dt,
            ref UgvCombatControlOutput output)
        {
            // 1. 격자 높이 버퍼 초기화
            for (int i = 0; i < _elevationMin.Length; i++)
            {
                _elevationMin[i] = float.MaxValue;
                _elevationMax[i] = float.MinValue;
            }

            // 2. 포인트 클라우드 격자 투영 (GC 0바이트)
            int halfGrid = GridSize / 2;
            for (int i = 0; i < points.Length; i++)
            {
                ref readonly var pt = ref points[i];
                int gx = (int)(pt.X / CellResolutionM) + halfGrid;
                int gy = (int)(pt.Y / CellResolutionM) + halfGrid;

                if ((uint)gx < GridSize && (uint)gy < GridSize)
                {
                    int idx = gy * GridSize + gx;
                    if (pt.Z < _elevationMin[idx]) _elevationMin[idx] = pt.Z;
                    if (pt.Z > _elevationMax[idx]) _elevationMax[idx] = pt.Z;
                }
            }

            // 3. 야지 주행가능성 비용 계산 (단차 및 거칠기)
            bool frontObstacle = false;
            for (int y = 0; y < GridSize; y++)
            {
                for (int x = 0; x < GridSize; x++)
                {
                    int idx = y * GridSize + x;
                    if (_elevationMin[idx] != float.MaxValue)
                    {
                        float stepHeight = _elevationMax[idx] - _elevationMin[idx];
                        float cost = MathF.Min(1.0f, stepHeight / GroundClearanceM);
                        _costmap[idx] = cost;

                        // 전방 5m 반경 내 고위험 장애물 감지
                        if (x > halfGrid && MathF.Abs(y - halfGrid) < 8 && cost > 0.85f)
                        {
                            frontObstacle = true;
                        }
                    }
                    else
                    {
                        _costmap[idx] = 0.1f; // 관측되지 않은 셀 기본 비용
                    }
                }
            }

            // 4. 경로 제어 (장애물 회피 조향각 결정)
            float steerAngle = 0f;
            float speedCmd = 3.5f; // 기본 12.6 km/h
            if (frontObstacle)
            {
                steerAngle = 0.35f; // 우측 회피 조향 (약 20도)
                speedCmd = 1.2f;    // 감속
            }

            // 5. RCWS 2축 자이로 피드포워드 안정화
            // 차체 회전에 반대되는 토크 및 각도 보상
            float azFeedForward = -yawRateRps * dt;
            float elFeedForward = -pitchRateRps * dt;

            _currentTurretAzRad += (targetAimAzRad - _currentTurretAzRad) * 0.2f + azFeedForward;
            _currentGunElRad += (targetAimElRad - _currentGunElRad) * 0.2f + elFeedForward;

            // 6. 결과 기록
            output.TargetSteeringAngleRad = steerAngle;
            output.TargetSpeedMps = speedCmd;
            output.RcwsTurretAzimuthDeg = _currentTurretAzRad * (180.0f / MathF.PI);
            output.RcwsGunElevationDeg = _currentGunElRad * (180.0f / MathF.PI);
            output.LocalMinCost = frontObstacle ? 0.9f : 0.15f;
            output.IsObstacleDetected = frontObstacle;
        }
    }
}