ActiveProtect/Models/LaserBeamRiderGuidanceSystem.cs

using System;

namespace ActiveProtect.Models
{
    /// <summary>
    /// 激光驾束制导系统
    /// </summary>
    public class LaserBeamRiderGuidanceSystem : BasicGuidanceSystem
    {
        /// <summary>
        /// 激光源位置
        /// </summary>
        private Vector3D LaserSourcePosition { get; set; }

        /// <summary>
        /// 激光方向
        /// </summary> 
        private Vector3D LaserDirection { get; set; }

        /// <summary>
        /// 激光功率
        /// </summary>
        public double LaserPower { get; set; }

        private const double MinDetectablePower = 1e-3; // 假设最小可探测功率为1毫瓦
        private const double DetectorDiameter = 0.1; // 假设探测器直径为10厘米
        private const double ControlFieldDiameter = 20.0; // 控制场直径（米）

        /// <summary>
        /// 上一次的误差
        /// </summary>
        private Vector3D LastError = Vector3D.Zero;

        /// <summary>
        /// 上一次的制导加速度
        /// </summary>
        public Vector3D LastGuidanceAcceleration { get; private set; }

        /// <summary>
        /// 积分误差
        /// </summary>
        private Vector3D IntegralError = Vector3D.Zero;

        /// <summary>
        /// 激光照射是否开启
        /// </summary>
        private bool LaserIlluminationOn { get; set; }


        /// <summary>
        /// 构造函数
        /// </summary>
        public LaserBeamRiderGuidanceSystem(double maxAcceleration, double guidanceCoefficient) : base(maxAcceleration, guidanceCoefficient)
        {
            LaserSourcePosition = Vector3D.Zero;
            LaserDirection = Vector3D.Zero;
            LaserPower = 0;
            HasGuidance = false;
            LastGuidanceAcceleration = Vector3D.Zero;
        }

        /// <summary>
        /// 更新激光驾束仪
        /// </summary>
        /// <param name="sourcePosition">激光源位置</param>
        /// <param name="direction">激光方向</param>
        /// <param name="laserPower">激光功率</param>
        public void UpdateLaserBeamRider(Vector3D sourcePosition, Vector3D direction, double laserPower)
        {
            LaserIlluminationOn = true;
            LaserSourcePosition = sourcePosition;
            LaserDirection = direction.Normalize();
            LaserPower = laserPower;
        }

        /// <summary>
        /// 关闭激光照射
        /// </summary>
        public void DeactivateLaserBeam()
        {
            LaserSourcePosition = Vector3D.Zero;
            LaserDirection = Vector3D.Zero;
            LaserPower = 0;
            HasGuidance = false;
        }

        /// <summary>
        /// 更新制导系统
        /// </summary>
        /// <param name="deltaTime">时间步长</param>
        /// <param name="missilePosition">导弹位置</param>
        /// <param name="missileVelocity">导弹速度</param>
        public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            base.Update(deltaTime, missilePosition, missileVelocity);
            
            if (LaserIlluminationOn)
            {
                UpdateGuidanceStatus();
                if (HasGuidance)
                {
                    CalculateGuidanceAcceleration(deltaTime);
                }
                else
                {
                    GuidanceAcceleration = Vector3D.Zero;
                }
            }
            else
            {
                GuidanceAcceleration = Vector3D.Zero;
            }
        }

        /// <summary>
        /// 更新制导状态
        /// </summary>
        private void UpdateGuidanceStatus()
        {
            // 计算导弹到激光束的最短距离
            Vector3D shortestDistanceVector = CalculateShortestDistanceToLaserBeam();
            double shortestDistance = shortestDistanceVector.Magnitude();

            // 检查导弹是否在控制场内
            if (shortestDistance > ControlFieldDiameter / 2)
            {
                HasGuidance = false;
                Console.WriteLine($"激光驾束引导系统: 失去引导, 原因: 超出控制场范围, 距离: {shortestDistance}");
                return;
            }

            Console.WriteLine($"激光驾束引导系统: 在控制场内, 距离: {shortestDistance}");

            // 计算接收到的激光功率
            double beamArea = Math.PI * Math.Pow(ControlFieldDiameter / 2, 2);
            double powerDensity = LaserPower / beamArea;
            double receivedPower = powerDensity * (Math.PI * Math.Pow(DetectorDiameter / 2, 2));

            if(HasGuidance = receivedPower >= MinDetectablePower)
            {
                HasGuidance = true;
            }
            else
            {
                HasGuidance = false;
                Console.WriteLine($"激光驾束引导系统: 失去引导, 原因: 接收到的激光功率低于最小可探测功率,{LaserPower:E} W/{receivedPower:E} W");
            }
        }

        /// <summary>
        /// 计算制导加速度
        /// </summary>
        /// <param name="deltaTime">时间步长</param>
        protected override void CalculateGuidanceAcceleration(double deltaTime)
        {
            if (!HasGuidance)
            {
                GuidanceAcceleration = Vector3D.Zero;
                return;
            }

            // 计算导弹到激光束的最短距离
            Vector3D shortestDistanceVector = CalculateShortestDistanceToLaserBeam();
            
            // PID控制
            double Kp = 30; // 增加比例系数,使系统对误差更敏感，反应更快 30
            double Ki = 0.05; // 减小积分系数,减少长期误差累积的影响 0.05
            double Kd = 5; // 增加微分系数,减少系统的超调量，提高稳定性    5
            double Kc = 0.5; // 减小非线性增益系数, 控制偏移量, 使得在更小的误差范围内有更大的修正 0.5

            // 计算误差
            Vector3D error = shortestDistanceVector;
    
            // 积分项
            IntegralError += error * deltaTime;
    
            // 微分项
            Vector3D derivativeError = (error - LastError) / deltaTime;

            // 计算PID输出
            Vector3D pidOutput = error * Kp + IntegralError * Ki + derivativeError * Kd;

            // 非线性增益
            double distance = shortestDistanceVector.Magnitude();
            double nonLinearGain = Math.Tanh(distance / Kc); 

            // 计算横向加速度
            Vector3D lateralAcceleration = pidOutput * nonLinearGain;

            // 限制最大加速度
            double maxAcceleration = 50; // 稍微增加最大加速度 50
            if (lateralAcceleration.Magnitude() > maxAcceleration)
            {
                lateralAcceleration = lateralAcceleration.Normalize() * maxAcceleration;
            }

            // 计算前向加速度
            Vector3D forwardDirection = LaserDirection;
            Vector3D currentDirection = Velocity.Normalize();
            Vector3D rotationAxis = Vector3D.CrossProduct(currentDirection, forwardDirection);
            double rotationAngle = Math.Acos(Vector3D.DotProduct(currentDirection, forwardDirection));
            Vector3D forwardAcceleration = Vector3D.CrossProduct(rotationAxis, Velocity) * rotationAngle * ProportionalNavigationCoefficient;

            // 合并横向和前向加速度
            GuidanceAcceleration = lateralAcceleration + forwardAcceleration;

            // 低通滤波
            const double alpha = 0.2;
            GuidanceAcceleration = GuidanceAcceleration * alpha + LastGuidanceAcceleration * (1 - alpha);

            // 更新上一次的误差和制导加速度
            LastError = error;
            LastGuidanceAcceleration = GuidanceAcceleration;

            //Console.WriteLine($"Shortest Distance Vector: {shortestDistanceVector}");
        }

        /// <summary>
        /// 获取制导系统状态
        /// </summary>
        /// <returns>制导系统状态</returns>
        public override string GetStatus()
        {
            return base.GetStatus();
        }

        /// <summary>
        /// 计算导弹到激光束的最短距离
        /// </summary>
        /// <returns>最短距离向量</returns>
        private Vector3D CalculateShortestDistanceToLaserBeam()
        {
            // 计算导弹到激光源的向量
            Vector3D missileToSource = LaserSourcePosition - Position;

            // 计算导弹在激光方向上的投影长度
            double projectionLength = Vector3D.DotProduct(missileToSource, LaserDirection);

            // 计算激光束上最接近导弹的点
            Vector3D closestPointOnBeam = LaserSourcePosition - LaserDirection * projectionLength;

            // 计算导弹到激光束最近点的向量（即最短距离向量）
            Vector3D shortestDistanceVector = closestPointOnBeam - Position;

            return shortestDistanceVector;
        }
    }
}