대한민국 육군 K10 탄약운반장갑차 로봇형 자동 탄약 공급 암(Arm) 4자유도 유압 서보 역기구학 & K9A1 자주포 결합 고속 무인 급탄 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.ArmoredRobotics
{
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct ResupplyArmState
    {
        public Vector256<float> JointPositions; // [theta1_rad, theta2_rad, d3_m, theta4_rad, 0, 0, 0, 0]
        public Vector256<float> EndEffectorPos; // [x_m, y_m, z_m, dockingError_m, 0, 0, 0, 0]
        public float ConveyorSpeedMps;          // 탄약 이송 컨베이어 속도 (m/s)
        public float TransferredShellCount;     // 분당 이송 완료 탄약 수 (RPM)
        public bool IsDockingLocked;            // K9A1 포탑 완벽 도킹 잠금 판정
    }

    public sealed class K10AmmunitionResupplyEngine
    {
        private const float BaseArmL0 = 1.85f;    // 베이스 높이 (m)
        private const float BoomLengthL1 = 2.40f; // 제1 붐 길이 (m)
        private const float MaxExtension = 3.60f; // 텔레스코픽 최대 신축 (m)

        private ResupplyArmState _state;

        public K10AmmunitionResupplyEngine()
        {
            // 초기 관절값: 선회 0 rad, 기립 0.3 rad, 신축 1.5 m, 팁 0.1 rad
            _state.JointPositions = Vector256.Create(0.0f, 0.30f, 1.50f, 0.10f, 0, 0, 0, 0);
        }

        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public ref readonly ResupplyArmState StepResupplyControl(
            float targetX, float targetY, float targetZ,
            float chassisRollRad, float chassisPitchRad, float dt)
        {
            Span<float> q = stackalloc float[8];
            _state.JointPositions.CopyTo(q);

            float th1 = q[0], th2 = q[1], d3 = q[2], th4 = q[3];

            // 1. 차체 롤/피치 외란 보정 반영
            th2 += chassisPitchRad * 0.5f;
            th1 += chassisRollRad * 0.5f;

            // 2. 순기구학 (Forward Kinematics) 현재 엔드이펙터 위치 계산
            float cos1 = MathF.Cos(th1), sin1 = MathF.Sin(th1);
            float rArm = BoomLengthL1 * MathF.Cos(th2) + d3 * MathF.Cos(th2 + th4);
            float curX = rArm * cos1;
            float curY = rArm * sin1;
            float curZ = BaseArmL0 + BoomLengthL1 * MathF.Sin(th2) + d3 * MathF.Sin(th2 + th4);

            // 3. 목표 도킹 포트와의 위치 오차 계산
            float errX = targetX - curX;
            float errY = targetY - curY;
            float errZ = targetZ - curZ;
            float totalError = MathF.Sqrt(errX * errX + errY * errY + errZ * errZ);

            // 4. 역기구학 야코비안 전치 기반 관절 제어 입력 갱신 (P-제어)
            float kp = 2.5f;
            q[0] += (-sin1 * errX + cos1 * errY) * 0.08f * kp * dt; // 선회 서보
            q[1] += (errZ * 0.12f) * kp * dt;                       // 기립 서보
            q[2] += Math.Clamp(rArm > 0 ? (cos1 * errX + sin1 * errY) * 0.5f * kp * dt : 0, -1.2f * dt, 1.2f * dt); // 신축 유압 실린더
            q[3] += (errZ * 0.06f) * kp * dt;                       // 팁 틸트

            q[2] = Math.Clamp(q[2], 0.8f, MaxExtension);

            _state.JointPositions = Vector256.Create(q[0], q[1], q[2], q[3], 0, 0, 0, 0);
            _state.EndEffectorPos = Vector256.Create(curX, curY, curZ, totalError, 0, 0, 0, 0);

            // 5. 도킹 완료 시 고속 탄약 이송 시퀀스 가동
            if (totalError <= 0.005f) // 5mm 이내 결합 성공
            {
                _state.IsDockingLocked = true;
                _state.ConveyorSpeedMps = 1.45f;
                _state.TransferredShellCount = 12.5f; // 분당 12.5발 완전 무인 급탄
            }
            else
            {
                _state.IsDockingLocked = false;
                _state.ConveyorSpeedMps = 0.0f;
                _state.TransferredShellCount = 0.0f;
            }

            return ref _state;
        }
    }
}