ActiveProtect/Models/LaserBeamRiderGuidanceSystem.cs

using System;

namespace ActiveProtect.Models
{
    public class LaserBeamRiderGuidanceSystem : BasicGuidanceSystem
    {
        private Vector3D LaserSourcePosition { get; set; }
        private Vector3D LaserDirection { get; set; }
        public double LaserPower { get; set; }
        private const double MinDetectablePower = 1e-3; // 假设最小可探测功率为1毫瓦
        private const double DetectorDiameter = 0.1; // 假设探测器直径为10厘米
        private const double ControlFieldDiameter = 6.0; // 控制场直径（米）

        private Vector3D LastError = Vector3D.Zero;

        public Vector3D LastGuidanceCommand { get; private set; }

        private Vector3D IntegralError = Vector3D.Zero;


        public LaserBeamRiderGuidanceSystem(double proportionalNavigationCoefficient) 
            : base(proportionalNavigationCoefficient)
        {
            LaserSourcePosition = Vector3D.Zero;
            LaserDirection = Vector3D.Zero;
            LaserPower = 0;
            HasGuidance = false;
            LastGuidanceCommand = Vector3D.Zero;
        }

        public void UpdateLaserBeamRider(Vector3D sourcePosition, Vector3D direction)
        {
            LaserSourcePosition = sourcePosition;
            LaserDirection = direction.Normalize();
        }

        public void ActivateLaserBeam()
        {
            HasGuidance = true;
        }

        public void DeactivateLaserBeam()
        {
            HasGuidance = false;
        }

        public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            base.Update(deltaTime, missilePosition, missileVelocity);
            
            UpdateGuidanceStatus();

            if (HasGuidance)
            {
                CalculateGuidanceCommand(deltaTime);
            }
            else
            {
                GuidanceCommand = Vector3D.Zero;
            }

            Console.WriteLine($"Missile Position: {Position}, Velocity: {Velocity}, Has Guidance: {HasGuidance}");
        }

        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");
            }
        }

        protected override void CalculateGuidanceCommand(double deltaTime)
        {
            if (!HasGuidance)
            {
                GuidanceCommand = Vector3D.Zero;
                return;
            }

            // 计算导弹到激光束的最短距离

            Vector3D shortestDistanceVector = CalculateShortestDistanceToLaserBeam();

            
            // PID控制
            double Kp = 30; // 增加比例系数,使系统对误差更敏感，反应更快
            double Ki = 0.05; // 减小积分系数,减少长期误差累积的影响
            double Kd = 5; // 增加微分系数,减少系统的超调量，提高稳定性    
            double Kc = 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 = 30; // 稍微增加最大加速度
            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;

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

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

            // 更新上一次的误差和制导命令
            LastError = error;
            LastGuidanceCommand = GuidanceCommand;

            Console.WriteLine($"Guidance Command: {GuidanceCommand.Magnitude()}, Lateral Error: {shortestDistanceVector.Magnitude()}, Lateral Acceleration: {lateralAcceleration.Magnitude()}, Forward Acceleration: {forwardAcceleration.Magnitude()}");
        }

        public override string ToString()
        {
            return $"LaserBeamRiderGuidanceSystem: HasGuidance={HasGuidance}, LaserSourcePosition={LaserSourcePosition}, LaserDirection={LaserDirection}, GuidanceCommand={GuidanceCommand}";
        }

        private Vector3D CalculateShortestDistanceToLaserBeam()
        {
            // 计算导弹到激光源的向量
            Vector3D missileToSource = LaserSourcePosition - Position;

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

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

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

            return shortestDistanceVector;
        }
    }
}