增加了红外指令制导导弹,红外测角仪,红外指令制导系统,但制导效果不佳,需要进一步优化

This commit is contained in:
Tian jianyong 2024-10-26 01:42:49 +08:00
parent 14220062f4
commit dbdb16fc8e
9 changed files with 676 additions and 14 deletions

View File

@ -1,4 +1,5 @@
using System;
using System.Numerics;
namespace ActiveProtect.Models
{
@ -218,6 +219,35 @@ namespace ActiveProtect.Models
return a + unitDirection * distance;
}
internal Vector3D Rotate(Orientation orientation, double misalignmentAngle)
{
// 首先,根据方向和失调角计算出旋转矩阵
Matrix4x4 rotationMatrix = CalculateRotationMatrix(orientation, misalignmentAngle);
// 然后,使用旋转矩阵对当前向量进行旋转
Vector3D rotatedVector = ApplyRotationMatrix(this, rotationMatrix);
return rotatedVector;
}
private static Matrix4x4 CalculateRotationMatrix(Orientation orientation, double misalignmentAngle)
{
// 创建旋转矩阵
Matrix4x4 yawRotation = Matrix4x4.CreateRotationY((float)orientation.Yaw);
Matrix4x4 pitchRotation = Matrix4x4.CreateRotationX((float)orientation.Pitch);
Matrix4x4 rollRotation = Matrix4x4.CreateRotationZ((float)orientation.Roll);
Matrix4x4 misalignmentRotation = Matrix4x4.CreateRotationY((float)misalignmentAngle);
// 组合旋转矩阵
return misalignmentRotation * yawRotation * pitchRotation * rollRotation;
}
private static Vector3D ApplyRotationMatrix(Vector3D vector, Matrix4x4 matrix)
{
double x = vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31 + matrix.M41;
double y = vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32 + matrix.M42;
double z = vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33 + matrix.M43;
return new Vector3D(x, y, z);
}
}
/// <summary>
@ -281,8 +311,13 @@ namespace ActiveProtect.Models
return angle;
}
internal Vector3D Rotate(Vector3D vector, double misalignmentAngle)
{
throw new NotImplementedException();
}
/// <summary>
/// 根据给定的方向向量创建方向
/// 根<EFBFBD><EFBFBD><EFBFBD>给定的方向向量创建方向
/// </summary>
/// <param name="direction">方向向量</param>
/// <returns>对应的方向</returns>

View File

@ -0,0 +1,124 @@
using System;
using ActiveProtect.SimulationEnvironment;
namespace ActiveProtect.Models
{
/// <summary>
/// 红外指令导引系统类
/// </summary>
public class InfraredCommandGuidanceSystem : BasicGuidanceSystem
{
private Vector3D lastTrackerToTargetVector;
private Vector3D lastTrackerToMissileVector;
private double timeSinceLastCommand;
private Vector3D previousLineOfSight;
private double timeSinceLastCalculation;
private Vector3D lastGuidanceAcceleration;
/// <summary>
/// 比例导航系数
/// </summary>
public double ProportionalNavigationCoefficient { get; set; }
/// <summary>
/// 最大加速度
/// </summary>
public double MaxAcceleration { get; set; }
/// <summary>
/// 构造函数
/// </summary>
public InfraredCommandGuidanceSystem(double maxAcceleration, double proportionalNavigationCoefficient)
: base()
{
lastTrackerToTargetVector = Vector3D.Zero;
lastTrackerToMissileVector = Vector3D.Zero;
timeSinceLastCommand = 0;
MaxAcceleration = maxAcceleration;
ProportionalNavigationCoefficient = proportionalNavigationCoefficient;
timeSinceLastCalculation = 0;
previousLineOfSight = Vector3D.Zero;
lastGuidanceAcceleration = Vector3D.Zero;
}
/// <summary>
/// 更新制导系统
/// </summary>
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)
{
HasGuidance = true;
lastTrackerToTargetVector = trackerToTargetVector;
lastTrackerToMissileVector = trackerToMissileVector;
timeSinceLastCommand = 0;
}
/// <summary>
/// 计算制导加速度
/// </summary>
protected override void CalculateGuidanceAcceleration(double deltaTime)
{
if (HasGuidance && lastTrackerToTargetVector.Magnitude() > 0 && lastTrackerToMissileVector.Magnitude() > 0)
{
// 计算当前视线向量(从导弹到目标)
Vector3D currentLineOfSight = (lastTrackerToTargetVector - lastTrackerToMissileVector).Normalize();
// 计算视线角速率
Vector3D lineOfSightRate;
if (previousLineOfSight != Vector3D.Zero && timeSinceLastCalculation > 0)
{
lineOfSightRate = Vector3D.CrossProduct(previousLineOfSight, currentLineOfSight) / timeSinceLastCalculation;
}
else
{
lineOfSightRate = Vector3D.Zero;
}
// 计算制导加速度
Vector3D newGuidanceAcceleration = Vector3D.CrossProduct(lineOfSightRate, Velocity) * ProportionalNavigationCoefficient;
// 应用低通滤波器来平滑加速度变化alpha越大平滑效果越强
double alpha = 0.2;
GuidanceAcceleration = lastGuidanceAcceleration * (1 - alpha) + newGuidanceAcceleration * alpha;
// 限制最大加速度
if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
{
GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
}
// 更新上一次的视线向量和计算时间
previousLineOfSight = currentLineOfSight;
timeSinceLastCalculation = deltaTime;
lastGuidanceAcceleration = GuidanceAcceleration;
Console.WriteLine($"Missile Guidance: Current Position: {Position}, Velocity: {Velocity}, " +
$"Acceleration: {GuidanceAcceleration}, " +
$"LOS Rate: {lineOfSightRate.Magnitude()}, LOS: {currentLineOfSight}, " +
$"Distance to Target: {(lastTrackerToTargetVector - lastTrackerToMissileVector).Magnitude()}\n");
}
else
{
GuidanceAcceleration = Vector3D.Zero;
}
}
/// <summary>
/// 获取制导系统状态
/// </summary>
public override string GetStatus()
{
return base.GetStatus() + $", GuidanceAcceleration: {GuidanceAcceleration}, " +
$"Tracker to Target Vector: {lastTrackerToTargetVector}, Tracker to Missile Vector: {lastTrackerToMissileVector}";
}
}
}

View File

@ -0,0 +1,189 @@
using System;
using ActiveProtect.SimulationEnvironment;
namespace ActiveProtect.Models
{
/// <summary>
/// 红外指令制导导弹类
/// </summary>
public class InfraredCommandGuidedMissile : MissileBase
{
/// <summary>
/// 红外指令制导导弹阶段
/// </summary>
private enum ICGM_Stage
{
Launch, // 发射阶段
Cruise, // 巡航阶段
Explode, // 爆炸阶段
SelfDestruct // 自毁阶段
}
/// <summary>
/// 当前阶段
/// </summary>
private ICGM_Stage currentStage;
/// <summary>
/// 红外热源辐射功率(瓦特)
/// </summary>
public double RadiationPower { get; set; }
/// <summary>
/// 红外指令导引系统
/// </summary>
private readonly InfraredCommandGuidanceSystem guidanceSystem;
/// <summary>
/// 红外测角仪
/// </summary>
private InfraredTracker infraredTracker;
/// <summary>
/// 构造函数
/// </summary>
public InfraredCommandGuidedMissile(MissileConfig config, ISimulationManager manager) : base(config, manager)
{
infraredTracker = null!;
RadiationPower = 1000;
// 初始化红外指令导引系统,包括自适应 PID 参数
guidanceSystem = new InfraredCommandGuidanceSystem(
maxAcceleration: config.MaxAcceleration,
proportionalNavigationCoefficient: config.ProportionalNavigationCoefficient
);
}
/// <summary>
/// 更新导弹状态
/// </summary>
protected override void UpdateMotionState(double deltaTime)
{
// 点亮红外热源
LightInfraredSource();
switch (currentStage)
{
case ICGM_Stage.Launch:
UpdateLaunchStage(deltaTime);
break;
case ICGM_Stage.Cruise:
UpdateCruiseStage(deltaTime);
break;
case ICGM_Stage.Explode:
UpdateExplodeStage(deltaTime);
break;
case ICGM_Stage.SelfDestruct:
UpdateSelfDestructStage(deltaTime);
break;
}
base.UpdateMotionState(deltaTime);
}
/// <summary>
/// 更新发射阶段
/// </summary>
/// <param name="deltaTime">时间步长</param>
private void UpdateLaunchStage(double deltaTime)
{
// 发射阶段
GuidanceAcceleration = Vector3D.Zero;
if (FlightTime >= 0.1)
{
currentStage = ICGM_Stage.Cruise;
}
}
/// <summary>
/// 更新巡航阶段
/// </summary>
/// <param name="deltaTime">时间步长</param>
private void UpdateCruiseStage(double deltaTime)
{
guidanceSystem.Update(deltaTime, Position, Velocity);
GuidanceAcceleration = guidanceSystem.GetGuidanceAcceleration();
if (DistanceToTarget <= ExplosionRadius)
{
currentStage = ICGM_Stage.Explode;
}
if(ShouldSelfDestruct())
{
currentStage = ICGM_Stage.SelfDestruct;
}
}
/// <summary>
/// 更新爆炸阶段
/// </summary>
/// <param name="deltaTime">时间步长</param>
private void UpdateExplodeStage(double deltaTime)
{
// 爆炸阶段
Explode();
}
/// <summary>
/// 更新自毁阶段
/// </summary>
/// <param name="deltaTime">时间步长</param>
private void UpdateSelfDestructStage(double deltaTime)
{
// 自毁阶段
SelfDestruct();
}
/// <summary>
/// 处理制导指令事件
/// </summary>
private void HandleGuidanceCommand(InfraredGuidanceCommandEvent evt)
{
if (evt.TargetMissileId == Id)
{
if (infraredTracker == null && evt.SenderId != null)
{
if (SimulationManager.GetEntityById(evt.SenderId) is InfraredTracker infraredTracker)
{
this.infraredTracker = infraredTracker;
}
}
guidanceSystem.ReceiveGuidanceCommand(evt.TrackerToTargetVector, evt.TrackerToMissileVector);
}
}
/// <summary>
/// 点亮红外热源
/// </summary>
public void LightInfraredSource()
{
SimulationManager.PublishEvent(new InfraredGuidanceMissileLightEvent { RadiationPower = this.RadiationPower, SenderId = this.Id });
}
public override void Activate()
{
base.Activate();
// 订阅红外指令制导事件
SimulationManager.SubscribeToEvent<InfraredGuidanceCommandEvent>(HandleGuidanceCommand);
}
public override void Deactivate()
{
base.Deactivate();
// 取消订阅红外指令制导事件
SimulationManager.UnsubscribeFromEvent<InfraredGuidanceCommandEvent>(HandleGuidanceCommand);
}
/// <summary>
/// 获取导弹状态
/// </summary>
public override string GetStatus()
{
return base.GetStatus() + $"\n 当前阶段:{currentStage}\n 红外指令导引系统状态: {guidanceSystem.GetStatus()}";
}
}
}

152
Models/InfraredTracker.cs Normal file
View File

@ -0,0 +1,152 @@
using System;
using ActiveProtect.SimulationEnvironment;
namespace ActiveProtect.Models
{
/// <summary>
/// 红外测角仪类
/// </summary>
public class InfraredTracker : SimulationElement
{
/// <summary>
/// 当前跟踪的导弹ID
/// </summary>
public string? TrackedMissileId { get; private set; }
/// <summary>
/// 目标ID
/// </summary>
public string? TargetId { get; private set; }
/// <summary>
/// 上次更新时间
/// </summary>
private double timeSinceLastUpdate;
private readonly InfraredTrackerConfig config;
/// <summary>
/// 构造函数
/// </summary>
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;
}
/// <summary>
/// 更新红外测角仪状态
/// </summary>
public override void Update(double deltaTime)
{
if (!IsActive) return;
timeSinceLastUpdate += deltaTime;
// 检查是否需要更新
if (timeSinceLastUpdate >= 1 / config.UpdateFrequency)
{
UpdateTracking();
timeSinceLastUpdate = 0;
}
}
/// <summary>
/// 更新跟踪状态
/// </summary>
private void UpdateTracking()
{
if (TrackedMissileId == null || TargetId == null) return;
var target = SimulationManager.GetEntityById(TargetId);
var missile = SimulationManager.GetEntityById(TrackedMissileId) as InfraredCommandGuidedMissile;
if (missile == null || target == null) return;
// 计算导弹到测角仪的距离
double distanceToMissile = Vector3D.Distance(Position, missile.Position);
// 检查导弹是否在跟踪范围内
if (distanceToMissile <= config.MaxTrackingRange)
{
// 计算测角仪到导弹的向量
Vector3D trackerToMissile = (missile.Position - Position).Normalize();
// 计算测角仪到目标的向量
Vector3D trackerToTarget = (target.Position - Position).Normalize();
// 发送制导指令事件
PublishGuidanceCommandEvent(missile.Id, trackerToMissile, trackerToTarget, Id);
}
else
{
// 导弹超出跟踪范围,停止跟踪
StopTracking();
}
}
/// <summary>
/// 停止跟踪
/// </summary>
public void StopTracking()
{
TrackedMissileId = null;
TargetId = null;
}
/// <summary>
/// 发布制导指令事件
/// </summary>
private void PublishGuidanceCommandEvent(string missileId, Vector3D trackerToMissileVector, Vector3D trackerToTargetVector, string senderId)
{
var evt = new InfraredGuidanceCommandEvent
{
TargetMissileId = missileId,
TrackerToMissileVector = trackerToMissileVector,
TrackerToTargetVector = trackerToTargetVector,
SenderId = senderId
};
SimulationManager.PublishEvent(evt);
}
/// <summary>
/// 处理红外指令制导导弹点亮红外热源事件
/// </summary>
private void OnInfraredGuidanceMissileLight(InfraredGuidanceMissileLightEvent evt)
{
// 处理导弹点亮红外热源事件
TrackedMissileId ??= evt.SenderId;
}
/// <summary>
/// 激活红外测角仪
/// </summary>
public override void Activate()
{
base.Activate();
SimulationManager.SubscribeToEvent<InfraredGuidanceMissileLightEvent>(OnInfraredGuidanceMissileLight);
}
/// <summary>
/// 销毁红外测角仪
/// </summary>
public override void Deactivate()
{
base.Deactivate();
SimulationManager.UnsubscribeFromEvent<InfraredGuidanceMissileLightEvent>(OnInfraredGuidanceMissileLight);
}
/// <summary>
/// 获取红外测角仪状态
/// </summary>
public override string GetStatus()
{
return $"红外测角仪 {Id} 位置: {Position}, 朝向: {Orientation}, " +
$"跟踪导弹: {TrackedMissileId ?? ""}, 目标: {TargetId ?? ""}";
}
}
}

View File

@ -167,7 +167,7 @@ namespace ActiveProtect.Models
/// <param name="position">辐射计的位置</param>
/// <param name="orientation">辐射计的朝向</param>
/// <param name="frequency">工作频率</param>
/// <param name="detectionTemperatureDifferenceThreshold">辐射检测阈值</param>
/// <param name="detectionTemperatureDifferenceThreshold">辐射温差检测阈值</param>
public MillimeterWaveRadiometer(Vector3D position, Orientation orientation, double frequency, double detectionTemperatureDifferenceThreshold)
: base(position, orientation)
{
@ -202,7 +202,7 @@ namespace ActiveProtect.Models
{
private double currentAltitude;
private TerminalSensitiveSubmunition submunition;
private readonly TerminalSensitiveSubmunition submunition;
/// <summary>
/// 最大测量高度

View File

@ -125,10 +125,38 @@ namespace ActiveProtect
ProportionalNavigationCoefficient = 3,
Mass = 50,
Type = MissileType.TerminalSensitiveMissile
},
// 红外指令制导导弹配置
new() {
Id = "ICGM_1",
InitialPosition = new Vector3D(2000, 100, 100),
InitialOrientation = new Orientation(Math.PI, 0.0, 0),
InitialSpeed = 300,
MaxSpeed = 400,
TargetIndex = 0,
MaxFlightTime = 60,
MaxFlightDistance = 5000,
LaunchAcceleration = 50,
MaxEngineBurnTime = 10,
MaxAcceleration = 1000,
ExplosionRadius = 10,
ProportionalNavigationCoefficient = 1,
Mass = 50,
Type = MissileType.InfraredCommandGuidance
}
},
SimulationTimeStep = 0.025 // 仿真时间步长, 激光驾束制导导弹需要更小的步长,< 0.025s
// 配置红外测角仪
InfraredTrackerConfig = new InfraredTrackerConfig
{
Id = "IT_1",
InitialPosition = new Vector3D(2500, 0, 100),
InitialOrientation = new Orientation(Math.PI, 0, 0),
MaxTrackingRange = 10000, // 10公里
FieldOfView = Math.PI / 4, // 45度
AngleMeasurementAccuracy = 0.000005, // 0.0005弧度 (约0.029度)
UpdateFrequency = 10 // 20赫兹
},
SimulationTimeStep = 0.01 // 仿真时间步长, 激光驾束制导导弹需要更小的步长,< 0.025s
};
// 创建仿真管理器
@ -141,7 +169,7 @@ namespace ActiveProtect
{
simulationManager.Update();
simulationManager.PrintStatus();
Thread.Sleep(100); // 暂停100毫秒使输出更易读
Thread.Sleep(50); // 暂停100毫秒使输出更易读
iteration++;
}

View File

@ -1,5 +1,5 @@
using System;
using System.Collections.Generic;
using System.ComponentModel;
using ActiveProtect.Models;
using Model;
@ -45,6 +45,11 @@ namespace ActiveProtect.SimulationEnvironment
/// </summary>
public double SimulationTimeStep { get; set; }
/// <summary>
/// 红外测角仪配置
/// </summary>
public InfraredTrackerConfig InfraredTrackerConfig { get; set; }
/// <summary>
/// 构造函数,初始化默认配置
/// </summary>
@ -56,6 +61,7 @@ namespace ActiveProtect.SimulationEnvironment
LaserBeamRiderConfig = new LaserBeamRiderConfig();
LaserWarnerConfig = new LaserWarnerConfig();
LaserJammerConfig = new LaserJammerConfig();
InfraredTrackerConfig = new InfraredTrackerConfig();
SimulationTimeStep = 0.1; // 默认时间步长为0.1秒
}
}
@ -76,7 +82,7 @@ namespace ActiveProtect.SimulationEnvironment
public Vector3D InitialPosition { get; set; }
/// <summary>
/// 初始朝
/// 初<EFBFBD><EFBFBD><EFBFBD>
/// </summary>
public Orientation InitialOrientation { get; set; }
@ -223,6 +229,31 @@ namespace ActiveProtect.SimulationEnvironment
/// </summary>
public MissileType Type { get; set; }
/// <summary>
/// 比例增益
/// </summary>
public double Kp { get; set; }
/// <summary>
/// 积分增益
/// </summary>
public double Ki { get; set; }
/// <summary>
/// 微分增益
/// </summary>
public double Kd { get; set; }
/// <summary>
/// 自适应速率
/// </summary>
public double AdaptiveRate { get; set; }
/// <summary>
/// 期望性能指标
/// </summary>
public double DesiredPerformance { get; set; }
/// <summary>
/// 构造函数,设置默认值
/// </summary>
@ -426,4 +457,59 @@ namespace ActiveProtect.SimulationEnvironment
PowerIncreaseRate = 2000.0; // 每秒增加的功率
}
}
/// <summary>
/// 红外测角仪配置类
/// </summary>
public class InfraredTrackerConfig
{
/// <summary>
/// 红外测角仪ID
/// </summary>
public string Id { get; set; }
/// <summary>
/// 初始位置
/// </summary>
public Vector3D InitialPosition { get; set; }
/// <summary>
/// 初始朝向
/// </summary>
public Orientation InitialOrientation { get; set; }
/// <summary>
/// 最大跟踪距离(米)
/// </summary>
public double MaxTrackingRange { get; set; }
/// <summary>
/// 视场角(弧度)
/// </summary>
public double FieldOfView { get; set; }
/// <summary>
/// 角度测量精度(弧度)
/// </summary>
public double AngleMeasurementAccuracy { get; set; }
/// <summary>
/// 更新频率(赫兹)
/// </summary>
public double UpdateFrequency { get; set; }
/// <summary>
/// 构造函数,设置默认值
/// </summary>
public InfraredTrackerConfig()
{
Id = "";
InitialPosition = new Vector3D(0, 0, 0);
InitialOrientation = new Orientation(0, 0, 0);
MaxTrackingRange = 10000; // 10公里
FieldOfView = Math.PI / 3; // 60度
AngleMeasurementAccuracy = 0.001; // 0.001弧度 (约0.057度)
UpdateFrequency = 10; // 10赫兹
}
}
}

View File

@ -45,7 +45,6 @@ namespace ActiveProtect.SimulationEnvironment
public SimulationElement? Target { get; set; }
}
/// <summary
/// <summary>
/// 激光照射更新事件
/// </summary>
@ -166,4 +165,36 @@ namespace ActiveProtect.SimulationEnvironment
{
public LaserBeamRider? LaserBeamRider { get; set; }
}
/// <summary>
/// 红外指令制导事件
/// </summary>
public class InfraredGuidanceCommandEvent : SimulationEvent
{
/// <summary>
/// 目标导弹ID
/// </summary>
public string? TargetMissileId { get; set; }
/// <summary>
/// 视线向量
/// </summary>
public Vector3D TrackerToTargetVector { get; set; } = Vector3D.Zero;
/// <summary>
/// 视线向量
/// </summary>
public Vector3D TrackerToMissileVector { get; set; } = Vector3D.Zero;
}
/// <summary>
/// 红外指令制导导弹点亮红外热源事件
/// </summary>
public class InfraredGuidanceMissileLightEvent : SimulationEvent
{
/// <summary>
/// 红外热源辐射功率(瓦特)
/// </summary>
public double RadiationPower { get; set; }
}
}

View File

@ -147,6 +147,9 @@ namespace ActiveProtect.SimulationEnvironment
case MissileType.TerminalSensitiveMissile:
missile = new TerminalSensitiveMissile(missileConfig, this);
break;
case MissileType.InfraredCommandGuidance:
missile = new InfraredCommandGuidedMissile(missileConfig, this);
break;
default:
missile = new MissileBase(missileConfig, this);
break;
@ -182,16 +185,25 @@ namespace ActiveProtect.SimulationEnvironment
this
);
Elements.Add(laserBeamRider);
// 创建红外测角仪
var infraredTrackerConfig = config.InfraredTrackerConfig;
var infraredTracker = new InfraredTracker(
"IT_1",
"Tank_1",
infraredTrackerConfig,
this
);
Elements.Add(infraredTracker);
// 激活所有元素
ActivateAllElements();
//ActivateAllElements();
// 激活部分元素
//ActivateSomeElements();
ActivateSomeElements();
// 激光驾束仪开始照射
laserBeamRider.StartBeamIllumination();
//laserBeamRider.StartBeamIllumination();
}
@ -225,8 +237,13 @@ 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 == "IT_1").ForEach(e => e.Activate());
// 激活末敏弹
Elements.FindAll(e => e.Id == "TSM_1").ForEach(e => e.Activate());
//Elements.FindAll(e => e.Id == "TSM_1").ForEach(e => e.Activate());
}
/// <summary>