화성 탐사 로버 테라메카닉스(Terramechanics) 침하 & 1,000Hz 0-Alloc C# 전동 휠 슬립 트랙션 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace ZeliDesk.Stem.MarsRoverTerramechanics
{
    /// <summary>
    /// 로버 휠 속도 및 차체 선속도 계측 데이터 (스택 할당 0-Alloc)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct RoverWheelKinematics
    {
        public readonly double WheelAngularVelocityRadS; // 바퀴 각속도 (rad/s)
        public readonly double RoverChassisSpeedM_S;      // 비전 계측 차체 이동 속도 (m/s)
        public readonly double NormalLoadN;               // 바퀴 수직 하중 (N)
        public readonly double SlopePitchAngleRad;        // 경사면 피치각 (라디안)

        public RoverWheelKinematics(double omega, double v, double wLoad, double pitch)
        {
            WheelAngularVelocityRadS = omega;
            RoverChassisSpeedM_S = v;
            NormalLoadN = wLoad;
            SlopePitchAngleRad = pitch;
        }
    }

    /// <summary>
    /// 인휠 모터 토크 및 슬립 제어 출력 명령
    /// </summary>
    public ref struct WheelTorqueCommand
    {
        public double CommandedTorqueNm;
        public double CurrentSlipRatio;
        public bool SinkageRiskAlert;
        public double EstimatedTractionForceN;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public WheelTorqueCommand(double torque, double slip, bool alert, double traction)
        {
            CommandedTorqueNm = torque;
            CurrentSlipRatio = slip;
            SinkageRiskAlert = alert;
            EstimatedTractionForceN = traction;
        }
    }

    /// <summary>
    /// 1,000Hz 무할당 화성 로버 테라메카닉스 트랙션 서보 엔진
    /// </summary>
    public ref struct RoverTerramechanicsEngine
    {
        private const double WheelRadiusM = 0.25; // 25cm 바퀴 반경
        private const double TargetOptimalSlip = 0.18; // 18% 최적 견인 슬립율
        private const double CohesionPa = 1200.0; // 화성 레골리스 점착력 (Pa)
        private const double FrictionAngleRad = 0.61; // 내부 마찰각 35도 (0.61 rad)

        private double _slipErrorIntegral;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ExecuteControlCycle(
            in RoverWheelKinematics sensor,
            double desiredDriveTorqueNm,
            out WheelTorqueCommand command)
        {
            // 1. 슬립율(Slip Ratio) 계산 (s = (r*omega - v) / (r*omega))
            double wheelLinearSpeed = WheelRadiusM * sensor.WheelAngularVelocityRadS;
            double roverSpeed = Math.Max(0.001, sensor.RoverChassisSpeedM_S);

            double slipRatio = 0.0;
            if (wheelLinearSpeed > roverSpeed)
            {
                slipRatio = (wheelLinearSpeed - roverSpeed) / Math.Max(wheelLinearSpeed, 0.01);
            }
            else
            {
                slipRatio = (wheelLinearSpeed - roverSpeed) / roverSpeed; // 제동 슬립
            }
            slipRatio = Math.Clamp(slipRatio, -1.0, 1.0);

            // 2. 위험 슬립 감지 (슬립율 > 0.5 이면 모래 파고들기 위험 발생)
            bool alert = slipRatio > 0.50;

            // 3. PI 슬립 트랙션 제어 루프
            double slipError = TargetOptimalSlip - slipRatio;
            _slipErrorIntegral += slipError * 0.001;

            double kp = 45.0;
            double ki = 120.0;

            double torqueModifier = kp * slipError + ki * _slipErrorIntegral;
            double finalTorque = desiredDriveTorqueNm + torqueModifier;

            // 과도한 슬립 시 급격한 토크 컷오프로 고립 방지
            if (alert)
            {
                finalTorque = Math.Min(finalTorque, desiredDriveTorqueNm * 0.35);
            }
            finalTorque = Math.Clamp(finalTorque, 0.0, 120.0); // 최대 120 Nm

            // 4. 추정 순 견인력 (Drawbar Pull) 산출
            double maxTraction = (CohesionPa * 0.05 + sensor.NormalLoadN * Math.Tan(FrictionAngleRad));
            double estimatedTraction = maxTraction * (1.0 - Math.Exp(-Math.Max(0.0, slipRatio) / 0.1));

            command = new WheelTorqueCommand(finalTorque, slipRatio, alert, estimatedTraction);
        }
    }
}