초·중·고 인공지능(AI) 로봇 자율주행 교육과 국방 무인 수색 차량(UGV): ZeliDesk의 온디바이스 2D/3D LiDAR SLAM, Cartographer 점군(Point Cloud) 매핑, 및 0.05초 동적 장애물 회피 TEB 로컬 플래너 아키텍처

using System;
using System.Numerics;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Robotics.Slam
{
    public unsafe sealed class LidarScanMatcherBnB
    {
        public const float Resolution = 0.05f; // 5cm 격자 해상도
        public const int GridWidth = 400;      // 20m x 20m 서브맵 영역
        public const int GridHeight = 400;

        private readonly byte[] _probabilityGrid = new byte[GridWidth * GridHeight];

        public struct CandidatePose
        {
            public float X;
            public float Y;
            public float Yaw;
            public float Score;
        }

        /// 

        /// 다중 해상도 분기 한정(Branch-and-Bound) 기반 고속 포즈 정합
        /// 
        public CandidatePose MatchScan(ReadOnlySpan pointCloud, Vector2 initialGuessPos, float initialGuessYaw)
        {
            CandidatePose bestPose = new CandidatePose { Score = -1.0f };
            float searchWindowMeters = 0.4f; // ±40cm 탐색 반경
            float searchWindowYaw = MathF.PI / 12.0f; // ±15도 각도 탐색

            fixed (byte* pGrid = _probabilityGrid)
            {
                // 각도 및 위치 그리드 탐색 루프 (SIMD 벡터 점수 누산)
                for (float dyaw = -searchWindowYaw; dyaw <= searchWindowYaw; dyaw += 0.02f)
                {
                    float testYaw = initialGuessYaw + dyaw;
                    float cosTheta = MathF.Cos(testYaw);
                    float sinTheta = MathF.Sin(testYaw);

                    for (float dx = -searchWindowMeters; dx <= searchWindowMeters; dx += Resolution)
                    {
                        for (float dy = -searchWindowMeters; dy <= searchWindowMeters; dy += Resolution)
                        {
                            Vector2 testPos = initialGuessPos + new Vector2(dx, dy);
                            float currentScore = ComputeMatchScore(pGrid, pointCloud, testPos, cosTheta, sinTheta);

                            if (currentScore > bestPose.Score)
                            {
                                bestPose.X = testPos.X;
                                bestPose.Y = testPos.Y;
                                bestPose.Yaw = testYaw;
                                bestPose.Score = currentScore;
                            }
                        }
                    }
                }
            }

            return bestPose;
        }

        private float ComputeMatchScore(byte* pGrid, ReadOnlySpan pointCloud, Vector2 pos, float cosTheta, float sinTheta)
        {
            float scoreSum = 0.0f;
            int validPoints = 0;

            for (int i = 0; i < pointCloud.Length; i++)
            {
                Vector2 pt = pointCloud[i];
                // 2D 강체 회전 변환
                float rx = cosTheta * pt.X - sinTheta * pt.Y + pos.X;
                float ry = sinTheta * pt.X + cosTheta * pt.Y + pos.Y;

                int gx = (int)(rx / Resolution) + (GridWidth / 2);
                int gy = (int)(ry / Resolution) + (GridHeight / 2);

                if (gx >= 0 && gx < GridWidth && gy >= 0 && gy < GridHeight)
                {
                    scoreSum += pGrid[gy * GridWidth + gx];
                    validPoints++;
                }
            }

            return validPoints > 0 ? (scoreSum / (validPoints * 255.0f)) : 0.0f;
        }
    }
}

using System;
using System.Collections.Generic;
using System.Numerics;

namespace ZeliDesk.Robotics.Navigation
{
    public sealed class TebLocalPlannerOptimizer
    {
        public float MaxLinearVelocity { get; set; } = 1.2f;  // 최대 전진 속도 1.2 m/s
        public float MaxAngularVelocity { get; set; } = 1.8f; // 최대 각속도 1.8 rad/s
        public float SafetyDistance { get; set; } = 0.45f;    // 장애물 최소 안전 반경 45cm

        public struct TrajectoryPoint
        {
            public Vector2 Position;
            public float Theta;
            public float Dt; // 시간 간격
        }

        /// 

        /// 20Hz(50ms) 주기 동적 장애물 회피 궤적 최적화
        /// 
        public (float LinearVel, float AngularVel) ComputeVelocityCommands(
            List band, 
            ReadOnlySpan dynamicObstacles)
        {
            if (band.Count < 2) return (0.0f, 0.0f);

            // 1. 탄성 밴드 시공간 완화(Elastic Relaxation) 및 장애물 척력(Repulsive Force) 인가
            for (int iter = 0; iter < 5; iter++) // 고속 수렴 5회 반복
            {
                for (int i = 1; i < band.Count - 1; i++)
                {
                    var pt = band[i];
                    Vector2 force = Vector2.Zero;

                    // A. 탄성 복원력 (경로 단축 및 부드러운 곡률)
                    Vector2 prevPos = band[i - 1].Position;
                    Vector2 nextPos = band[i + 1].Position;
                    Vector2 elasticForce = (prevPos + nextPos) * 0.5f - pt.Position;
                    force += elasticForce * 0.4f;

                    // B. 동적 장애물 척력 인가
                    for (int o = 0; o < dynamicObstacles.Length; o++)
                    {
                        Vector2 obs = dynamicObstacles[o];
                        float dist = Vector2.Distance(pt.Position, obs);
                        if (dist < SafetyDistance && dist > 1e-4f)
                        {
                            Vector2 repDir = Vector2.Normalize(pt.Position - obs);
                            float repMagnitude = (SafetyDistance - dist) / dist;
                            force += repDir * (repMagnitude * 1.5f);
                        }
                    }

                    // 위치 갱신
                    pt.Position += force * 0.1f;
                    band[i] = pt;
                }
            }

            // 2. 첫 번째 세그먼트로부터 차동 구동 속도 명령 도출
            Vector2 deltaPos = band[1].Position - band[0].Position;
            float targetHeading = MathF.Atan2(deltaPos.Y, deltaPos.X);
            float headingError = WrapAngle(targetHeading - band[0].Theta);

            float v = Math.Clamp(deltaPos.Length() / band[0].Dt, 0.0f, MaxLinearVelocity);
            float w = Math.Clamp(headingError / band[0].Dt, -MaxAngularVelocity, MaxAngularVelocity);

            // 급선회 시 전진 속도 자동 감속 안전 가드
            v *= MathF.Max(0.0f, MathF.Cos(headingError));

            return (v, w);
        }

        private static float WrapAngle(float angle)
        {
            while (angle > MathF.PI) angle -= 2.0f * MathF.PI;
            while (angle < -MathF.PI) angle += 2.0f * MathF.PI;
            return angle;
        }
    }
}