From 1b8bba70a7e00ef27c9266edb41761d497d83c72 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Sat, 26 Oct 2024 15:11:05 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=89=E7=82=B9=E6=B3=95=E8=AE=A1=E7=AE=97?= =?UTF-8?q?=E7=BA=A2=E5=A4=96=E6=8C=87=E4=BB=A4=E5=88=B6=E5=AF=BC=E5=AF=BC?= =?UTF-8?q?=E5=BC=B9=E7=9A=84=E5=88=B6=E5=AF=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Docs/DesignMissile.md | 7 ++ Models/BasicGuidanceSystem.cs | 2 +- Models/InfraredCommandGuidanceSystem.cs | 87 +++++++++++----------- Models/InfraredCommandGuidedMissile.cs | 10 +-- Models/InfraredTracker.cs | 27 +++---- Models/LaserSemiActiveGuidedMissile.cs | 1 - Models/TerminalSensitiveSubmunition.cs | 2 +- Program.cs | 22 +++--- SimulationEnvironment/SimulationEvents.cs | 1 + SimulationEnvironment/SimulationManager.cs | 8 +- 10 files changed, 82 insertions(+), 85 deletions(-) diff --git a/Docs/DesignMissile.md b/Docs/DesignMissile.md index 36b1590..0cefb89 100644 --- a/Docs/DesignMissile.md +++ b/Docs/DesignMissile.md @@ -92,3 +92,10 @@ public class Missile : IProperties, IState 4. 符合单一职责原则：每个类都只负责一种类型的参数。 5. 便于测试：可以轻松地为每种参数类型创建模拟对象，便于单元测试。 6. 灵活性：可以为不同类型的导弹创建不同的参数实现，而保持相同的接口。 + +## 红外指令制导导弹 + +### 制导系统 + +采用三点法进行制导计算，并增加了提前量和加速度平滑因子，以提高制导精度。 +因为三点法的缺点，导致导弹在接近目标时，加速度会突然增大，导致导弹过载，容易导致脱靶。因此，限制了最大加速度，这也导致导弹在接近目标时，会出现转向不足的情况，出现一定制导误差。 diff --git a/Models/BasicGuidanceSystem.cs b/Models/BasicGuidanceSystem.cs index 96d8d94..4762f1a 100644 --- a/Models/BasicGuidanceSystem.cs +++ b/Models/BasicGuidanceSystem.cs @@ -135,7 +135,7 @@ namespace ActiveProtect.Models /// 制导系统状态 public virtual string GetStatus() { - return $"BasicGuidanceSystem: HasGuidance={HasGuidance}, Position={Position}, Velocity={Velocity}, GuidanceAcceleration={GuidanceAcceleration}"; + return $" 导引头状态: 有制导={HasGuidance}, 位置={Position}, 速度={Velocity}, 制导加速度={GuidanceAcceleration}"; } } } diff --git a/Models/InfraredCommandGuidanceSystem.cs b/Models/InfraredCommandGuidanceSystem.cs index 943819e..6c0998d 100644 --- a/Models/InfraredCommandGuidanceSystem.cs +++ b/Models/InfraredCommandGuidanceSystem.cs @@ -8,17 +8,17 @@ namespace ActiveProtect.Models /// 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秒 /// - /// 比例导航系数 + /// 制导系数 /// - public double ProportionalNavigationCoefficient { get; set; } + public double GuidanceCoefficient { get; set; } /// /// 最大加速度 @@ -28,17 +28,15 @@ namespace ActiveProtect.Models /// /// 构造函数 /// - 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; } /// @@ -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); } /// /// 接收制导指令 /// - public void ReceiveGuidanceCommand(Vector3D trackerToTargetVector, Vector3D trackerToMissileVector) + public void ReceiveGuidanceCommand(Vector3D trackerToMissileVector, Vector3D trackerToTargetVector) { HasGuidance = true; - lastTrackerToTargetVector = trackerToTargetVector; lastTrackerToMissileVector = trackerToMissileVector; - timeSinceLastCommand = 0; + lastTrackerToTargetVector = trackerToTargetVector; } /// /// 计算制导加速度 /// - 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}"; } } } diff --git a/Models/InfraredCommandGuidedMissile.cs b/Models/InfraredCommandGuidedMissile.cs index 758b618..12e0da5 100644 --- 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 /// public override string GetStatus() { - return base.GetStatus() + $"\n 当前阶段：{currentStage}\n 红外指令导引系统状态: {guidanceSystem.GetStatus()}"; + return base.GetStatus() + $"\n 当前阶段：{currentStage}\n 导引系统状态: {guidanceSystem.GetStatus()}"; } } } diff --git a/Models/InfraredTracker.cs b/Models/InfraredTracker.cs index ea9d1fc..9a64f87 100644 --- a/Models/InfraredTracker.cs +++ b/Models/InfraredTracker.cs @@ -19,11 +19,6 @@ namespace ActiveProtect.Models public string? TargetId { get; private set; } - /// - /// 上次更新时间 - /// - private double timeSinceLastUpdate; - private readonly InfraredTrackerConfig config; /// @@ -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(); } /// @@ -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}"); } /// @@ -144,8 +132,11 @@ namespace ActiveProtect.Models /// public override string GetStatus() { - return $"红外测角仪 {Id} 位置: {Position}, 朝向: {Orientation}, " + - $"跟踪导弹: {TrackedMissileId ?? "无"}, 目标: {TargetId ?? "无"}"; + return $"红外测角仪 {Id}:\n" + + $" 位置: {Position}\n" + + $" 跟踪导弹: {TrackedMissileId ?? "无"}\n" + + $" 目标: {TargetId ?? "无"}\n" + + $" 状态: {(IsActive ? "活动" : "已销毁")}\n"; } } diff --git a/Models/LaserSemiActiveGuidedMissile.cs b/Models/LaserSemiActiveGuidedMissile.cs index cc74b0a..ba8633f 100644 --- 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; diff --git a/Models/TerminalSensitiveSubmunition.cs b/Models/TerminalSensitiveSubmunition.cs index e2f6e80..499cca1 100644 --- a/Models/TerminalSensitiveSubmunition.cs +++ b/Models/TerminalSensitiveSubmunition.cs @@ -279,7 +279,7 @@ namespace ActiveProtect.Models /// 子弹状态 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}"; } } } diff --git a/Program.cs b/Program.cs index 49362a6..3e33beb 100644 --- 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++; } diff --git a/SimulationEnvironment/SimulationEvents.cs b/SimulationEnvironment/SimulationEvents.cs index 53f9ba2..4fc54db 100644 --- a/SimulationEnvironment/SimulationEvents.cs +++ b/SimulationEnvironment/SimulationEvents.cs @@ -185,6 +185,7 @@ namespace ActiveProtect.SimulationEnvironment /// 视线向量 /// public Vector3D TrackerToMissileVector { get; set; } = Vector3D.Zero; + } /// diff --git a/SimulationEnvironment/SimulationManager.cs b/SimulationEnvironment/SimulationManager.cs index 48db31b..923917f 100644 --- 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());