590 lines
21 KiB
C#
590 lines
21 KiB
C#
using ThreatSource.Simulation;
|
||
using ThreatSource.Utils;
|
||
using ThreatSource.Target;
|
||
using System.Diagnostics;
|
||
namespace ThreatSource.Guidance
|
||
{
|
||
/// <summary>
|
||
/// 毫米波导引头系统类，实现了基于毫米波雷达的目标探测和跟踪功能
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 该类提供了毫米波导引头系统的核心功能：
|
||
/// - 目标探测和识别
|
||
/// - 信噪比计算
|
||
/// - 抗干扰处理
|
||
/// - 比例导引控制
|
||
/// 用于实现全天候制导打击
|
||
/// </remarks>
|
||
public class MillimeterWaveGuidanceSystem : BasicGuidanceSystem
|
||
{
|
||
/// <summary>
|
||
/// 工作模式枚举
|
||
/// </summary>
|
||
private enum WorkMode
|
||
{
|
||
Search, // 搜索模式
|
||
Track, // 跟踪模式
|
||
Lock // 锁定模式
|
||
}
|
||
|
||
/// <summary>
|
||
/// 毫米波导引系统配置
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 存储系统的所有可配置参数
|
||
/// 包括探测范围、视场角等
|
||
/// </remarks>
|
||
private readonly MillimeterWaveGuidanceConfig config;
|
||
|
||
/// <summary>
|
||
/// 随机数生成器实例
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 用于生成随机扰动
|
||
/// 模拟系统噪声和识别概率
|
||
/// </remarks>
|
||
private readonly Random random = new();
|
||
|
||
/// <summary>
|
||
/// 上一次探测到的目标位置
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录目标的历史位置
|
||
/// 用于计算目标速度
|
||
/// </remarks>
|
||
private Vector3D lastTargetPosition;
|
||
|
||
/// <summary>
|
||
/// 上一次探测到的目标速度
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录目标的历史速度
|
||
/// 用于计算目标速度
|
||
/// </remarks>
|
||
private Vector3D lastTargetVelocity;
|
||
|
||
/// <summary>
|
||
/// 目标丢失计时器
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录目标丢失的时间
|
||
/// 用于切换到搜索模式
|
||
/// </remarks>
|
||
private double targetLostTimer = 0;
|
||
|
||
/// <summary>
|
||
/// 锁定确认计时器
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录锁定确认的时间
|
||
/// 用于切换到锁定模式
|
||
/// </remarks>
|
||
private double lockConfirmationTimer = 0;
|
||
|
||
/// <summary>
|
||
/// 当前工作模式
|
||
/// </summary>
|
||
private WorkMode currentMode = WorkMode.Search;
|
||
|
||
/// <summary>
|
||
/// 是否受到干扰
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 指示当前是否受到电子干扰
|
||
/// 影响系统的工作状态
|
||
/// </remarks>
|
||
private bool isJammed = false;
|
||
|
||
/// <summary>
|
||
/// 干扰功率，单位：瓦特
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录接收到的干扰信号功率
|
||
/// 用于评估干扰效果
|
||
/// </remarks>
|
||
private double jammingPower = 0;
|
||
|
||
/// <summary>
|
||
/// 是否有目标
|
||
/// </summary>
|
||
public bool HasTarget { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 是否在锁定模式
|
||
/// </summary>
|
||
public bool IsInLockMode => currentMode == WorkMode.Lock;
|
||
|
||
/// <summary>
|
||
/// 当前扫描角度，单位：弧度
|
||
/// </summary>
|
||
private double currentScanAngle = 0;
|
||
|
||
/// <summary>
|
||
/// 当前扫描半径，单位：弧度
|
||
/// </summary>
|
||
private double currentScanRadius = 0;
|
||
|
||
/// <summary>
|
||
/// 最大扫描半径，单位：弧度
|
||
/// </summary>
|
||
private double maxScanRadius => config.FieldOfViewAngle * Math.PI / 180.0 / 2;
|
||
|
||
/// <summary>
|
||
/// 激活制导系统
|
||
/// </summary>
|
||
public override void Activate()
|
||
{
|
||
base.Activate();
|
||
SwitchToSearchMode();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 停用制导系统
|
||
/// </summary>
|
||
public override void Deactivate()
|
||
{
|
||
base.Deactivate();
|
||
HasTarget = false;
|
||
HasGuidance = false;
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 初始化毫米波制导系统的新实例
|
||
/// </summary>
|
||
/// <param name="id">制导系统ID</param>
|
||
/// <param name="maxAcceleration">最大加速度，单位：米/平方秒</param>
|
||
/// <param name="guidanceCoefficient">制导系数</param>
|
||
/// <param name="simulationManager">仿真管理器实例</param>
|
||
/// <param name="config">毫米波导引系统配置</param>
|
||
/// <remarks>
|
||
/// 构造过程：
|
||
/// - 初始化基类参数
|
||
/// - 设置仿真管理器
|
||
/// - 初始化目标位置
|
||
/// - 注册干扰事件处理
|
||
/// </remarks>
|
||
public MillimeterWaveGuidanceSystem(
|
||
string id,
|
||
double maxAcceleration,
|
||
double guidanceCoefficient,
|
||
MillimeterWaveGuidanceConfig config,
|
||
ISimulationManager simulationManager)
|
||
: base(id, maxAcceleration, guidanceCoefficient, simulationManager)
|
||
{
|
||
this.config = config;
|
||
lastTargetPosition = Vector3D.Zero;
|
||
lastTargetVelocity = Vector3D.Zero;
|
||
|
||
// 订阅干扰事件
|
||
simulationManager.SubscribeToEvent<MillimeterWaveJammingEvent>(HandleJammingEvent);
|
||
|
||
SwitchToSearchMode(); // 初始化为搜索模式
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换到搜索模式
|
||
/// </summary>
|
||
public void SwitchToSearchMode()
|
||
{
|
||
currentMode = WorkMode.Search;
|
||
HasTarget = false;
|
||
HasGuidance = false;
|
||
targetLostTimer = 0;
|
||
lockConfirmationTimer = 0;
|
||
currentScanAngle = 0;
|
||
currentScanRadius = config.SearchBeamWidth * Math.PI / 720.0; // 从半个波束宽度的一半开始
|
||
Trace.WriteLine($"切换到搜索模式，波束宽度: {config.SearchBeamWidth}度");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换到跟踪模式
|
||
/// </summary>
|
||
private void SwitchToTrackMode()
|
||
{
|
||
currentMode = WorkMode.Track;
|
||
lockConfirmationTimer = 0;
|
||
HasGuidance = true;
|
||
Trace.WriteLine($"切换到跟踪模式，波束宽度: {config.TrackBeamWidth}度");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换到锁定模式
|
||
/// </summary>
|
||
private void SwitchToLockMode()
|
||
{
|
||
currentMode = WorkMode.Lock;
|
||
HasGuidance = true;
|
||
Trace.WriteLine("切换到锁定模式 - 目标锁定成功");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 处理毫米波干扰事件
|
||
/// </summary>
|
||
/// <param name="evt">干扰事件参数</param>
|
||
/// <remarks>
|
||
/// 处理过程：
|
||
/// - 更新干扰状态
|
||
/// - 记录干扰功率
|
||
/// </remarks>
|
||
private void HandleJammingEvent(MillimeterWaveJammingEvent evt)
|
||
{
|
||
isJammed = true;
|
||
jammingPower = evt.JammingPower;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新圆锥扫描参数
|
||
/// </summary>
|
||
private void UpdateConicalScan(double deltaTime)
|
||
{
|
||
if (currentMode == WorkMode.Search)
|
||
{
|
||
// 使用配置参数
|
||
currentScanAngle += config.ScanAngularSpeedDeg * Math.PI / 180.0 * deltaTime;
|
||
currentScanRadius += config.ScanRadiusGrowthRateDeg * Math.PI / 180.0 * deltaTime;
|
||
currentScanRadius = Math.Min(currentScanRadius, maxScanRadius);
|
||
|
||
if (currentScanAngle >= 2 * Math.PI)
|
||
{
|
||
currentScanAngle -= 2 * Math.PI;
|
||
}
|
||
|
||
Console.WriteLine($"扫描参数 - 半径: {currentScanRadius * 180/Math.PI:F2}度, 角度: {currentScanAngle * 180/Math.PI:F2}度");
|
||
}
|
||
else
|
||
{
|
||
currentScanAngle = 0;
|
||
// 非搜索模式时重置扫描参数
|
||
currentScanRadius = config.SearchBeamWidth * Math.PI / 720.0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算目标是否在当前扫描波束内
|
||
/// </summary>
|
||
private bool IsTargetInBeam(Vector3D missileVelocity, Vector3D toTarget)
|
||
{
|
||
// 使用导弹速度方向作为X轴（前进方向）
|
||
Vector3D baseDirection = missileVelocity.Normalize();
|
||
|
||
// 根据当前模式选择波束宽度
|
||
double currentBeamWidth = currentMode switch
|
||
{
|
||
WorkMode.Search => config.SearchBeamWidth * Math.PI / 180.0,
|
||
WorkMode.Track => config.TrackBeamWidth * Math.PI / 180.0,
|
||
WorkMode.Lock => config.LockBeamWidth * Math.PI / 180.0,
|
||
_ => config.SearchBeamWidth * Math.PI / 180.0
|
||
};
|
||
|
||
if (currentMode == WorkMode.Search)
|
||
{
|
||
// 建立以前进方向为X轴的右手坐标系
|
||
Vector3D xAxis = baseDirection;
|
||
Vector3D yAxis;
|
||
Vector3D referenceAxis = Vector3D.UnitY; // 优先使用垂直轴作为参考
|
||
|
||
// 处理近乎垂直飞行的情况
|
||
if (Math.Abs(xAxis.Y) > 0.9)
|
||
{
|
||
referenceAxis = Vector3D.UnitZ; // 改用Z轴作为参考
|
||
}
|
||
|
||
// 构建正交坐标系
|
||
yAxis = Vector3D.CrossProduct(Vector3D.CrossProduct(xAxis, referenceAxis), xAxis).Normalize();
|
||
Vector3D zAxis = Vector3D.CrossProduct(xAxis, yAxis).Normalize();
|
||
|
||
// 计算圆锥扫描方向
|
||
double offsetAngle = currentScanRadius;
|
||
// X轴为主轴的圆锥扫描参数
|
||
double x = Math.Cos(offsetAngle); // 主轴分量
|
||
double y = Math.Sin(offsetAngle) * Math.Sin(currentScanAngle); // 垂直分量
|
||
double z = Math.Sin(offsetAngle) * Math.Cos(currentScanAngle); // 水平分量
|
||
|
||
// 合成扫描方向向量
|
||
Vector3D scanDirection = (
|
||
xAxis * x +
|
||
yAxis * y +
|
||
zAxis * z
|
||
).Normalize();
|
||
|
||
// 调试输出
|
||
Console.WriteLine($"导弹前进方向: {baseDirection}");
|
||
Console.WriteLine($"目标方向: {toTarget.Normalize()}");
|
||
Console.WriteLine($"扫描方向: {scanDirection}");
|
||
|
||
// 计算目标夹角
|
||
double angle = Vector3D.AngleBetween(scanDirection, toTarget.Normalize());
|
||
Console.WriteLine($"目标夹角: {angle * 180/Math.PI:F2}度");
|
||
Console.WriteLine($"波束宽度: {currentBeamWidth * 180/Math.PI:F2}度，SNR阈值: {config.RecognitionSNRThreshold}dB");
|
||
|
||
return angle <= currentBeamWidth;
|
||
}
|
||
else
|
||
{
|
||
// 跟踪和锁定模式：直接使用基准方向
|
||
double angle = Math.Acos(Vector3D.DotProduct(baseDirection, toTarget.Normalize()));
|
||
|
||
string mode = currentMode == WorkMode.Track ? "跟踪" : "锁定";
|
||
Console.WriteLine($"{mode}模式 - 目标夹角: {angle * 180.0 / Math.PI:F2}度, 波束宽度: {currentBeamWidth * 180.0 / Math.PI:F2}度");
|
||
|
||
return angle <= currentBeamWidth;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新制导系统的状态
|
||
/// </summary>
|
||
/// <param name="deltaTime">自上次更新以来的时间间隔，单位：秒</param>
|
||
/// <param name="missilePosition">导弹当前位置，单位：米</param>
|
||
/// <param name="missileVelocity">导弹当前速度，单位：米/秒</param>
|
||
/// <remarks>
|
||
/// 更新过程：
|
||
/// - 探测目标位置
|
||
/// - 计算目标速度
|
||
/// - 生成制导指令
|
||
/// - 限制最大加速度
|
||
/// </remarks>
|
||
public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
|
||
{
|
||
// 更新扫描参数
|
||
UpdateConicalScan(deltaTime);
|
||
|
||
base.Update(deltaTime, missilePosition, missileVelocity);
|
||
|
||
if (TryDetectAndTrackTarget(missilePosition, missileVelocity, deltaTime, out Vector3D targetPosition))
|
||
{
|
||
targetLostTimer = 0;
|
||
Vector3D targetVelocity = (targetPosition - lastTargetPosition) / deltaTime;
|
||
lastTargetPosition = targetPosition;
|
||
lastTargetVelocity = targetVelocity;
|
||
|
||
// 使用比例导引控制算法
|
||
GuidanceAcceleration = MotionAlgorithm.CalculateProportionalNavigation(
|
||
ProportionalNavigationCoefficient,
|
||
missilePosition,
|
||
missileVelocity,
|
||
targetPosition,
|
||
targetVelocity);
|
||
|
||
if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
|
||
{
|
||
GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
|
||
}
|
||
|
||
HasGuidance = true;
|
||
}
|
||
else
|
||
{
|
||
if (currentMode != WorkMode.Search)
|
||
{
|
||
targetLostTimer += deltaTime;
|
||
// 如果目标丢失时间达到阈值，切换回搜索模式
|
||
if (targetLostTimer >= config.TargetLostTolerance)
|
||
{
|
||
HasGuidance = false;
|
||
Trace.WriteLine($"目标丢失 {targetLostTimer:F3}秒，切换回搜索模式");
|
||
SwitchToSearchMode();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
HasGuidance = false;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 尝试探测和跟踪目标
|
||
/// </summary>
|
||
/// <param name="missilePosition">导弹当前位置，单位：米</param>
|
||
/// <param name="missileVelocity">导弹当前速度，单位：米/秒</param>
|
||
/// <param name="deltaTime">自上次更新以来的时间间隔，单位：秒</param>
|
||
/// <param name="targetPosition">输出探测到的目标位置，单位：米</param>
|
||
/// <returns>是否成功探测到目标</returns>
|
||
/// <remarks>
|
||
/// 探测过程：
|
||
/// - 检查干扰状态
|
||
/// - 遍历场景中的目标
|
||
/// - 检查距离和角度
|
||
/// - 计算信噪比
|
||
/// - 判断目标有效性
|
||
/// </remarks>
|
||
private bool TryDetectAndTrackTarget(Vector3D missilePosition, Vector3D missileVelocity, double deltaTime, out Vector3D targetPosition)
|
||
{
|
||
targetPosition = Vector3D.Zero;
|
||
double minDistance = double.MaxValue;
|
||
bool foundTarget = false;
|
||
double currentSNR = double.MinValue;
|
||
|
||
// 如果被干扰，直接返回false
|
||
if (isJammed)
|
||
{
|
||
Trace.WriteLine($"毫米波导引头受到干扰，干扰功率: {jammingPower:F2}W");
|
||
HasGuidance = false;
|
||
return false;
|
||
}
|
||
|
||
foreach (var element in SimulationManager.GetEntitiesByType<SimulationElement>())
|
||
{
|
||
if (element is ITarget target)
|
||
{
|
||
Vector3D toTarget = target.Position - missilePosition;
|
||
double distance = toTarget.Magnitude();
|
||
|
||
if (distance <= config.MaxDetectionRange)
|
||
{
|
||
// 计算信噪比
|
||
double snr = CalculateSNR(distance, target.RadarCrossSection);
|
||
Console.WriteLine($"信噪比: {snr:F2}dB");
|
||
|
||
if (currentMode == WorkMode.Search)
|
||
{
|
||
// 在搜索模式下进行波束判断
|
||
if (IsTargetInBeam(missileVelocity, toTarget) && snr >= config.RecognitionSNRThreshold)
|
||
{
|
||
targetPosition = target.Position;
|
||
minDistance = distance;
|
||
foundTarget = true;
|
||
currentSNR = snr;
|
||
}
|
||
}
|
||
else if (currentMode == WorkMode.Track || currentMode == WorkMode.Lock)
|
||
{
|
||
// 在跟踪和锁定模式下直接使用搜索阶段的目标位置
|
||
if (Vector3D.Distance(target.Position, lastTargetPosition) < 100.0) // 目标识别容差
|
||
{
|
||
targetPosition = target.Position;
|
||
foundTarget = true;
|
||
currentSNR = snr;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 根据当前模式和探测结果更新系统状态
|
||
UpdateSystemState(foundTarget, currentSNR, deltaTime);
|
||
|
||
HasTarget = foundTarget;
|
||
return foundTarget;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新系统状态
|
||
/// </summary>
|
||
private void UpdateSystemState(bool hasTarget, double snr, double deltaTime)
|
||
{
|
||
if (!hasTarget)
|
||
{
|
||
targetLostTimer += deltaTime;
|
||
if (targetLostTimer >= config.TargetLostTolerance)
|
||
{
|
||
SwitchToSearchMode();
|
||
}
|
||
return;
|
||
}
|
||
|
||
targetLostTimer = 0;
|
||
|
||
switch (currentMode)
|
||
{
|
||
case WorkMode.Search:
|
||
if (snr >= config.RecognitionSNRThreshold)
|
||
{
|
||
SwitchToTrackMode();
|
||
}
|
||
break;
|
||
|
||
case WorkMode.Track:
|
||
if (snr >= config.LockSNRThreshold)
|
||
{
|
||
lockConfirmationTimer += deltaTime;
|
||
if (lockConfirmationTimer >= config.LockConfirmationTime)
|
||
{
|
||
SwitchToLockMode();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
lockConfirmationTimer = 0;
|
||
}
|
||
break;
|
||
|
||
case WorkMode.Lock:
|
||
if (snr < config.LockSNRThreshold)
|
||
{
|
||
SwitchToTrackMode();
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取制导系统的详细状态信息
|
||
/// </summary>
|
||
/// <returns>包含完整状态参数的字符串</returns>
|
||
/// <remarks>
|
||
/// 返回信息：
|
||
/// - 基本状态信息
|
||
/// - 目标位置
|
||
/// 用于系统监控和调试
|
||
/// </remarks>
|
||
public override string GetStatus()
|
||
{
|
||
return base.GetStatus() +
|
||
$"\n当前模式: {currentMode}" +
|
||
$"\n目标位置: {lastTargetPosition}" +
|
||
$"\n目标速度: {lastTargetVelocity}" +
|
||
$"\n是否有目标: {HasTarget}";
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算目标的信噪比
|
||
/// </summary>
|
||
/// <param name="distance">到目标的距离，单位：米</param>
|
||
/// <param name="radarCrossSection">目标雷达散射截面积，单位：平方米</param>
|
||
/// <returns>信噪比，单位：分贝</returns>
|
||
/// <remarks>
|
||
/// 计算过程：
|
||
/// - 设置雷达参数
|
||
/// - 计算信号功率
|
||
/// - 计算噪声功率
|
||
/// - 计算信噪比
|
||
/// - 转换为分贝值
|
||
/// </remarks>
|
||
private double CalculateSNR(double distance, double radarCrossSection)
|
||
{
|
||
// 雷达参数
|
||
double transmitPower = config.TransmitPower; // 发射功率(W)，典型值0.3W
|
||
double antennaGain = Math.Pow(10, config.AntennaGainDB/10); // 天线增益(线性值)
|
||
double wavelength = 3e8 / config.WaveFrequency; // 波长(m)，94GHz -> 3.19mm
|
||
double bandwidth = 1.0 / config.PulseDuration; // 带宽(Hz)，由脉冲持续时间决定
|
||
double noiseFigure = Math.Pow(10, config.NoiseFigureDB/10); // 噪声系数(线性值)
|
||
|
||
// 系统损耗，单位：dB
|
||
double atmosphericLoss = 0.4 * distance / 1000.0; // 大气衰减，0.4dB/km
|
||
double totalLoss = Math.Pow(10, (atmosphericLoss + config.SystemLossDB) / 10); // 转换为线性值
|
||
|
||
// 常量
|
||
double k = 1.38e-23; // 玻尔兹曼常数
|
||
double T0 = 290; // 标准噪声温度(K)
|
||
|
||
// 计算接收信号功率
|
||
double signalPower = (transmitPower * Math.Pow(antennaGain, 2) * Math.Pow(wavelength, 2) * radarCrossSection)
|
||
/ (Math.Pow(4 * Math.PI, 3) * Math.Pow(distance, 4) * totalLoss);
|
||
|
||
// 计算噪声功率
|
||
double noisePower = k * T0 * bandwidth * noiseFigure;
|
||
|
||
// 计算信噪比
|
||
double snr = signalPower / noisePower;
|
||
|
||
// 转换为dB
|
||
return 10 * Math.Log10(snr);
|
||
}
|
||
}
|
||
}
|
||
|