三点法计算红外指令制导导弹的制导

2024-10-26 15:11:05 +08:00 · 2024-10-26 15:11:05 +08:00 · 1b8bba70a7
commit 1b8bba70a7
parent dbdb16fc8e
10 changed files with 82 additions and 85 deletions
--- a/Docs/DesignMissile.md
+++ b/Docs/DesignMissile.md
@ -92,3 +92,10 @@ public class Missile : IProperties, IState
 4. 符合单一职责原则：每个类都只负责一种类型的参数。
 5. 便于测试：可以轻松地为每种参数类型创建模拟对象，便于单元测试。
 6. 灵活性：可以为不同类型的导弹创建不同的参数实现，而保持相同的接口。
+
+## 红外指令制导导弹
+
+### 制导系统
+
+采用三点法进行制导计算，并增加了提前量和加速度平滑因子，以提高制导精度。
+因为三点法的缺点，导致导弹在接近目标时，加速度会突然增大，导致导弹过载，容易导致脱靶。因此，限制了最大加速度，这也导致导弹在接近目标时，会出现转向不足的情况，出现一定制导误差。
--- a/Models/BasicGuidanceSystem.cs
+++ b/Models/BasicGuidanceSystem.cs
@ -135,7 +135,7 @@ namespace ActiveProtect.Models
        /// <returns>制导系统状态</returns>
        public virtual string GetStatus()
        {
-            return $"BasicGuidanceSystem: HasGuidance={HasGuidance}, Position={Position}, Velocity={Velocity}, GuidanceAcceleration={GuidanceAcceleration}";
+            return $"  导引头状态: 有制导={HasGuidance}, 位置={Position}, 速度={Velocity}, 制导加速度={GuidanceAcceleration}";
        }
    }
 }
--- a/Models/InfraredCommandGuidanceSystem.cs
+++ b/Models/InfraredCommandGuidanceSystem.cs
@ -8,17 +8,17 @@ namespace ActiveProtect.Models
    /// </summary>
    public class InfraredCommandGuidanceSystem : BasicGuidanceSystem
    {
-        private Vector3D lastTrackerToTargetVector;
        private Vector3D lastTrackerToMissileVector;
-        private double timeSinceLastCommand;
-        private Vector3D previousLineOfSight;
-        private double timeSinceLastCalculation;
-        private Vector3D lastGuidanceAcceleration;
+        private Vector3D lastTrackerToTargetVector;
+        private Vector3D lastDesiredDirection;
+        private double turnRate;
+        private const double TurnRateSmoothingFactor = 0.5; // 转向速率平滑因子，越大越平滑，0.1~0.5
+        private const double LeadTimeFactor = 0.3; // 提前量因子，单位：秒，越大提前量越大，一般取0.5秒

        /// <summary>
-        /// 比例导航系数
+        /// 制导系数
        /// </summary>
-        public double ProportionalNavigationCoefficient { get; set; }
+        public double GuidanceCoefficient { get; set; }

        /// <summary>
        /// 最大加速度
@ -28,17 +28,15 @@ namespace ActiveProtect.Models
        /// <summary>
        /// 构造函数
        /// </summary>
-        public InfraredCommandGuidanceSystem(double maxAcceleration, double proportionalNavigationCoefficient)
+        public InfraredCommandGuidanceSystem(double maxAcceleration, double guidanceCoefficient)
            : base()
        {
-            lastTrackerToTargetVector = Vector3D.Zero;
            lastTrackerToMissileVector = Vector3D.Zero;
-            timeSinceLastCommand = 0;
+            lastTrackerToTargetVector = Vector3D.Zero;
+            lastDesiredDirection = Vector3D.Zero;
            MaxAcceleration = maxAcceleration;
-            ProportionalNavigationCoefficient = proportionalNavigationCoefficient;
-            timeSinceLastCalculation = 0;
-            previousLineOfSight = Vector3D.Zero;
-            lastGuidanceAcceleration = Vector3D.Zero;
+            GuidanceCoefficient = guidanceCoefficient;
+            turnRate = 0;
        }

        /// <summary>
@ -47,48 +45,52 @@ namespace ActiveProtect.Models
        public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            base.Update(deltaTime, missilePosition, missileVelocity);
-            timeSinceLastCommand += deltaTime;
            CalculateGuidanceAcceleration(deltaTime);
        }

        /// <summary>
        /// 接收制导指令
        /// </summary>
-        public void ReceiveGuidanceCommand(Vector3D trackerToTargetVector, Vector3D trackerToMissileVector)
+        public void ReceiveGuidanceCommand(Vector3D trackerToMissileVector, Vector3D trackerToTargetVector)
        {
            HasGuidance = true;
-            lastTrackerToTargetVector = trackerToTargetVector;
            lastTrackerToMissileVector = trackerToMissileVector;
-            timeSinceLastCommand = 0;
+            lastTrackerToTargetVector = trackerToTargetVector;
        }

        /// <summary>
        /// 计算制导加速度
        /// </summary>
- protected override void CalculateGuidanceAcceleration(double deltaTime)
+        protected override void CalculateGuidanceAcceleration(double deltaTime)
        {
-            if (HasGuidance && lastTrackerToTargetVector.Magnitude() > 0 && lastTrackerToMissileVector.Magnitude() > 0)
+            if (HasGuidance)
            {
-                // 计算当前视线向量（从导弹到目标）
-                Vector3D currentLineOfSight = (lastTrackerToTargetVector - lastTrackerToMissileVector).Normalize();
+                // 计算期望飞行方向（从导弹指向目标）
+                Vector3D currentDesiredDirection = (lastTrackerToTargetVector - lastTrackerToMissileVector).Normalize();

-                // 计算视线角速率
-                Vector3D lineOfSightRate;
-                if (previousLineOfSight != Vector3D.Zero && timeSinceLastCalculation > 0)
+                // 计算当前飞行方向
+                Vector3D currentDirection = Velocity.Normalize();
+
+                // 更新转向速率
+                if (lastDesiredDirection != Vector3D.Zero)
                {
-                    lineOfSightRate = Vector3D.CrossProduct(previousLineOfSight, currentLineOfSight) / timeSinceLastCalculation;
-                }
-                else
-                {
-                    lineOfSightRate = Vector3D.Zero;
+                    double instantTurnRate = Vector3D.CrossProduct(lastDesiredDirection, currentDesiredDirection).Magnitude() / deltaTime;
+                    turnRate = turnRate * (1 - TurnRateSmoothingFactor) + instantTurnRate * TurnRateSmoothingFactor;
                }

+                // 计算带有提前量的期望方向
+                Vector3D leadDirection = Vector3D.CrossProduct(currentDesiredDirection, Vector3D.CrossProduct(currentDesiredDirection, currentDirection).Normalize());
+                Vector3D desiredDirectionWithLead = (currentDesiredDirection + leadDirection * turnRate * LeadTimeFactor).Normalize();
+
+                // 计算转向轴
+                Vector3D turnAxis = Vector3D.CrossProduct(currentDirection, desiredDirectionWithLead).Normalize();
+
+                // 计算所需转向角度
+                double turnAngle = Math.Acos(Vector3D.DotProduct(currentDirection, desiredDirectionWithLead));
+
                // 计算制导加速度
-                Vector3D newGuidanceAcceleration = Vector3D.CrossProduct(lineOfSightRate, Velocity) * ProportionalNavigationCoefficient;
-
-                // 应用低通滤波器来平滑加速度变化，alpha越大，平滑效果越强
-                double alpha = 0.2;
-                GuidanceAcceleration = lastGuidanceAcceleration * (1 - alpha) + newGuidanceAcceleration * alpha;
+                double accelerationMagnitude = GuidanceCoefficient * turnAngle * Velocity.Magnitude();
+                GuidanceAcceleration = Vector3D.CrossProduct(turnAxis, currentDirection) * accelerationMagnitude;

                // 限制最大加速度
                if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
@ -96,15 +98,11 @@ namespace ActiveProtect.Models
                    GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
                }

-                // 更新上一次的视线向量和计算时间
-                previousLineOfSight = currentLineOfSight;
-                timeSinceLastCalculation = deltaTime;
-                lastGuidanceAcceleration = GuidanceAcceleration;
+                lastDesiredDirection = currentDesiredDirection;

-                Console.WriteLine($"Missile Guidance: Current Position: {Position}, Velocity: {Velocity}, " +
-                                  $"Acceleration: {GuidanceAcceleration}, " +
-                                  $"LOS Rate: {lineOfSightRate.Magnitude()}, LOS: {currentLineOfSight}, " +
-                                  $"Distance to Target: {(lastTrackerToTargetVector - lastTrackerToMissileVector).Magnitude()}\n");
+                // Console.WriteLine($"Missile Guidance: Current Position: {Position}, Velocity: {Velocity}, " +
+                //                   $"Acceleration: {GuidanceAcceleration}, Current Direction: {currentDirection}, " +
+                //                   $"Desired Direction: {desiredDirectionWithLead}, Turn Angle: {turnAngle}, Turn Rate: {turnRate}\n");
            }
            else
            {
@ -118,7 +116,8 @@ namespace ActiveProtect.Models
        public override string GetStatus()
        {
            return base.GetStatus() + $", GuidanceAcceleration: {GuidanceAcceleration}, " +
-                   $"Tracker to Target Vector: {lastTrackerToTargetVector}, Tracker to Missile Vector: {lastTrackerToMissileVector}";
+                   $"Tracker to Target Vector: {lastTrackerToTargetVector}, Tracker to Missile Vector: {lastTrackerToMissileVector}, " +
+                   $"Turn Rate: {turnRate}";
        }
    }
 }
--- a/Models/InfraredCommandGuidedMissile.cs
+++ b/Models/InfraredCommandGuidedMissile.cs
@ -52,7 +52,7 @@ namespace ActiveProtect.Models
            // 初始化红外指令导引系统，包括自适应 PID 参数
            guidanceSystem = new InfraredCommandGuidanceSystem(
                maxAcceleration: config.MaxAcceleration,
-                proportionalNavigationCoefficient: config.ProportionalNavigationCoefficient
+                guidanceCoefficient: config.ProportionalNavigationCoefficient
            );

        }
@ -146,13 +146,13 @@ namespace ActiveProtect.Models
            {
                if (infraredTracker == null && evt.SenderId != null)
                {
-                    if (SimulationManager.GetEntityById(evt.SenderId) is InfraredTracker infraredTracker)
+                    if (SimulationManager.GetEntityById(evt.SenderId) is InfraredTracker tracker)
                    {
-                        this.infraredTracker = infraredTracker;
+                        infraredTracker = tracker;
                    }
                }
                
-                guidanceSystem.ReceiveGuidanceCommand(evt.TrackerToTargetVector, evt.TrackerToMissileVector);
+                guidanceSystem.ReceiveGuidanceCommand(evt.TrackerToMissileVector, evt.TrackerToTargetVector);
            }
        }

@ -183,7 +183,7 @@ namespace ActiveProtect.Models
        /// </summary>
        public override string GetStatus()
        {
-            return base.GetStatus() + $"\n  当前阶段：{currentStage}\n  红外指令导引系统状态: {guidanceSystem.GetStatus()}";
+            return base.GetStatus() + $"\n  当前阶段：{currentStage}\n  导引系统状态: {guidanceSystem.GetStatus()}";
        }
    }
 }
--- a/Models/InfraredTracker.cs
+++ b/Models/InfraredTracker.cs
@ -19,11 +19,6 @@ namespace ActiveProtect.Models
        public string? TargetId { get; private set; }


-        /// <summary>
-        /// 上次更新时间
-        /// </summary>
-        private double timeSinceLastUpdate;
-
        private readonly InfraredTrackerConfig config;

        /// <summary>
@ -32,7 +27,6 @@ namespace ActiveProtect.Models
        public InfraredTracker(string id, string targetId, InfraredTrackerConfig config, ISimulationManager manager) 
            : base(id, config.InitialPosition, config.InitialOrientation, 0, manager)
        {
-            timeSinceLastUpdate = 0;
            TargetId = targetId;
            this.config = config;
        }
@ -44,14 +38,7 @@ namespace ActiveProtect.Models
        {
            if (!IsActive) return;

-            timeSinceLastUpdate += deltaTime;
-
-            // 检查是否需要更新
-            if (timeSinceLastUpdate >= 1 / config.UpdateFrequency)
-            {
-                UpdateTracking();
-                timeSinceLastUpdate = 0;
-            }
+            UpdateTracking();
        }

        /// <summary>
@ -73,10 +60,10 @@ namespace ActiveProtect.Models
            if (distanceToMissile <= config.MaxTrackingRange)
            {
                // 计算测角仪到导弹的向量
-                Vector3D trackerToMissile = (missile.Position - Position).Normalize();
+                Vector3D trackerToMissile = missile.Position - Position;

                // 计算测角仪到目标的向量
-                Vector3D trackerToTarget = (target.Position - Position).Normalize();
+                Vector3D trackerToTarget = target.Position - Position;

                // 发送制导指令事件
                PublishGuidanceCommandEvent(missile.Id, trackerToMissile, trackerToTarget, Id);
@ -110,6 +97,7 @@ namespace ActiveProtect.Models
                SenderId = senderId
            };
            SimulationManager.PublishEvent(evt);
+            Console.WriteLine($"55发布制导指令事件: {evt}");
        }

        /// <summary>   
@ -144,8 +132,11 @@ namespace ActiveProtect.Models
        /// </summary>
        public override string GetStatus()
        {
-            return $"红外测角仪 {Id} 位置: {Position}, 朝向: {Orientation}, " +
-                   $"跟踪导弹: {TrackedMissileId ?? "无"}, 目标: {TargetId ?? "无"}";
+            return $"红外测角仪 {Id}:\n" +
+                   $"  位置: {Position}\n" +
+                   $"  跟踪导弹: {TrackedMissileId ?? "无"}\n" +
+                   $"  目标: {TargetId ?? "无"}\n" +
+                   $"  状态: {(IsActive ? "活动" : "已销毁")}\n";
        }

    }
--- a/Models/LaserSemiActiveGuidedMissile.cs
+++ b/Models/LaserSemiActiveGuidedMissile.cs
@ -140,7 +140,6 @@ namespace ActiveProtect.Models
        {
            LaserGuidanceSystem.Update(deltaTime, Position, Velocity);
            GuidanceAcceleration = LaserGuidanceSystem.GetGuidanceAcceleration();
-            Console.WriteLine($"导弹ID: {Id}, 距离目标: {DistanceToTarget}, 爆炸半径: {ExplosionRadius}");
            if (DistanceToTarget <= ExplosionRadius)
            {
                currentStage = LSAGM_Stage.Explode;
--- a/Models/TerminalSensitiveSubmunition.cs
+++ b/Models/TerminalSensitiveSubmunition.cs
@ -279,7 +279,7 @@ namespace ActiveProtect.Models
        /// <returns>子弹状态</returns>
        public override string GetStatus()
        {
-            return $"{base.GetStatus()}\nSubmunition Stage: {currentStage}\nDetection Angle: {detectionAngle * 180 / Math.PI:F2}°\nSpiral Angle: {spiralAngle * 180 / Math.PI:F2}°\nTarget Detected: {lastDetectionTime != null}";
+            return $"{base.GetStatus()}\n运行状态: {currentStage}\n检测角度: {detectionAngle * 180 / Math.PI:F2}°\n螺旋角度: {spiralAngle * 180 / Math.PI:F2}°\n目标检测: {lastDetectionTime != null}";
        }
    }
 }
--- a/Program.cs
+++ b/Program.cs
@ -32,7 +32,7 @@ namespace ActiveProtect
                    {
                        Id = "Tank_1",
                        InitialOrientation = new Orientation(Math.PI/4, 0, 0),
-                        InitialSpeed = 10,
+                        InitialSpeed = 20,
                        MaxSpeed = 25,
                        MaxArmor = 100,
                        HasLaserWarner = false,
@ -121,7 +121,7 @@ namespace ActiveProtect
                        MaxFlightDistance = 5000, 
                        LaunchAcceleration = 0, 
                        MaxEngineBurnTime = 0, 
-                        MaxAcceleration = 1000,
+                        MaxAcceleration = 10,
                        ProportionalNavigationCoefficient = 3,
                        Mass = 50, 
                        Type = MissileType.TerminalSensitiveMissile
@ -129,7 +129,7 @@ namespace ActiveProtect
                    // 红外指令制导导弹配置
                    new() {
                        Id = "ICGM_1",
-                        InitialPosition = new Vector3D(2000, 100, 100),
+                        InitialPosition = new Vector3D(2000, 10, 100),
                        InitialOrientation = new Orientation(Math.PI, 0.0, 0),
                        InitialSpeed = 300,
                        MaxSpeed = 400,
@ -138,9 +138,9 @@ namespace ActiveProtect
                        MaxFlightDistance = 5000,
                        LaunchAcceleration = 50,
                        MaxEngineBurnTime = 10,
-                        MaxAcceleration = 1000,
-                        ExplosionRadius = 10,
-                        ProportionalNavigationCoefficient = 1,
+                        MaxAcceleration = 100,
+                        ExplosionRadius = 5,
+                        ProportionalNavigationCoefficient = 3,
                        Mass = 50,
                        Type = MissileType.InfraredCommandGuidance
                    }
@ -149,27 +149,27 @@ namespace ActiveProtect
                InfraredTrackerConfig = new InfraredTrackerConfig
                {
                    Id = "IT_1",
-                    InitialPosition = new Vector3D(2500, 0, 100),
+                    InitialPosition = new Vector3D(2000, 0, 100),
                    InitialOrientation = new Orientation(Math.PI, 0, 0),
                    MaxTrackingRange = 10000, // 10公里
                    FieldOfView = Math.PI / 4, // 45度
-                    AngleMeasurementAccuracy = 0.000005, // 0.0005弧度 (约0.029度)
+                    AngleMeasurementAccuracy = 0.0005, // 0.0005弧度 (约0.029度)
                    UpdateFrequency = 10 // 20赫兹
                },
-                SimulationTimeStep = 0.01 // 仿真时间步长, 激光驾束制导导弹需要更小的步长，< 0.025s
+                SimulationTimeStep = 0.005 // 仿真时间步长, 激光驾束制导导弹需要更小的步长，< 0.025s
            };

            // 创建仿真管理器
            var simulationManager = new SimulationManager(config);

            // 运行仿真
-            int maxIterations = 1000;
+            int maxIterations = 10000;
            int iteration = 0;
            while (!simulationManager.IsSimulationEnded && iteration < maxIterations)
            {
                simulationManager.Update();
                simulationManager.PrintStatus();
-                Thread.Sleep(50); // 暂停100毫秒，使输出更易读
+                Thread.Sleep(20); // 暂停100毫秒，使输出更易读
                iteration++;
            }

--- a/SimulationEnvironment/SimulationEvents.cs
+++ b/SimulationEnvironment/SimulationEvents.cs
@ -185,6 +185,7 @@ namespace ActiveProtect.SimulationEnvironment
        /// 视线向量
        /// </summary>
        public Vector3D TrackerToMissileVector { get; set; } = Vector3D.Zero;
+
    }
    
    /// <summary>
--- a/SimulationEnvironment/SimulationManager.cs
+++ b/SimulationEnvironment/SimulationManager.cs
@ -228,9 +228,9 @@ namespace ActiveProtect.SimulationEnvironment
            //Elements.FindAll(e => e.Id == "LJ_1").ForEach(e => e.Activate());

            // 激活激光半主动制导导弹
-            //Elements.FindAll(e => e.Id == "LSGM_1").ForEach(e => e.Activate());
+            Elements.FindAll(e => e.Id == "LSGM_1").ForEach(e => e.Activate());
            // 激活激光目标指示器
-            //Elements.FindAll(e => e.Id == "LD_1").ForEach(e => e.Activate());
+            Elements.FindAll(e => e.Id == "LD_1").ForEach(e => e.Activate());

            // 激活激光驾束制导导弹
            //Elements.FindAll(e => e.Id == "LBRM_1").ForEach(e => e.Activate());
@ -238,9 +238,9 @@ namespace ActiveProtect.SimulationEnvironment
            //Elements.FindAll(e => e.Id == "LBR_1").ForEach(e => e.Activate());

            // 激活红外指令制导导弹
-            Elements.FindAll(e => e.Id == "ICGM_1").ForEach(e => e.Activate());
+            //Elements.FindAll(e => e.Id == "ICGM_1").ForEach(e => e.Activate());
            // 激活红外测角仪
-            Elements.FindAll(e => e.Id == "IT_1").ForEach(e => e.Activate());
+            //Elements.FindAll(e => e.Id == "IT_1").ForEach(e => e.Activate());

            // 激活末敏弹
            //Elements.FindAll(e => e.Id == "TSM_1").ForEach(e => e.Activate());