1019 lines
44 KiB
C#
1019 lines
44 KiB
C#
using ThreatSource.Utils;
|
||
using ThreatSource.Equipment;
|
||
using System.Diagnostics;
|
||
using ThreatSource.Jammer;
|
||
using ThreatSource.Simulation;
|
||
using AirTransmission;
|
||
|
||
namespace ThreatSource.Guidance
|
||
{
|
||
/// <summary>
|
||
/// 毫米波导引头系统类，实现了基于毫米波雷达的目标探测和跟踪功能
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 该类提供了毫米波导引头系统的核心功能：
|
||
/// - 目标探测和识别
|
||
/// - 信噪比计算
|
||
/// - 抗干扰处理
|
||
/// - 比例导引控制
|
||
/// </remarks>
|
||
public class MillimeterWaveGuidanceSystem : BaseGuidanceSystem
|
||
{
|
||
/// <summary>
|
||
/// 工作模式枚举
|
||
/// </summary>
|
||
private enum WorkMode
|
||
{
|
||
Search, // 搜索模式
|
||
Track, // 跟踪模式
|
||
Lock // 锁定模式
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取设备支持的阻塞干扰类型
|
||
/// </summary>
|
||
public override IEnumerable<JammingType> SupportedBlockingJammingTypes => [JammingType.MillimeterWave];
|
||
|
||
/// <summary>
|
||
/// 获取设备支持的干扰类型
|
||
/// </summary>
|
||
public override IEnumerable<JammingType> SupportedJammingTypes => [JammingType.MillimeterWave, JammingType.SmokeGrenade];
|
||
|
||
/// <summary>
|
||
/// 毫米波导引系统配置
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 存储系统的所有可配置参数
|
||
/// 包括探测范围、视场角等
|
||
/// </remarks>
|
||
private readonly MillimeterWaveGuidanceConfig config;
|
||
|
||
/// <summary>
|
||
/// 上一次探测到的目标位置 (使用可空类型)
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录目标的历史位置
|
||
/// 用于计算目标速度
|
||
/// </remarks>
|
||
private Vector3D? LastTargetPosition { get; set; }
|
||
|
||
/// <summary>
|
||
/// 上一次探测到的目标速度 (使用可空类型)
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录目标的历史速度
|
||
/// 用于计算目标速度
|
||
/// </remarks>
|
||
private Vector3D? LastTargetVelocity { get; set; }
|
||
|
||
/// <summary>
|
||
/// 锁定确认计时器
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录锁定确认的时间
|
||
/// 用于切换到锁定模式
|
||
/// </remarks>
|
||
private double lockConfirmationTimer = 0;
|
||
|
||
/// <summary>
|
||
/// 扫描周期计时器
|
||
/// </summary>
|
||
private double scanCycleTimer = 0;
|
||
|
||
/// <summary>
|
||
/// 当前工作模式
|
||
/// </summary>
|
||
private WorkMode currentMode = WorkMode.Search;
|
||
|
||
/// <summary>
|
||
/// 当前正在跟踪或已锁定的目标ID (由Search模式切换时设置)
|
||
/// </summary>
|
||
private string? currentlyTrackedTargetId;
|
||
|
||
/// <summary>
|
||
/// 当前活动模式 (Search焦点、Track、Lock) 的探测历史缓冲区
|
||
/// </summary>
|
||
private CircularBuffer<bool> activeDetectionHistory = new(TRACK_DETECTION_WINDOW_SIZE);
|
||
|
||
/// <summary>
|
||
/// Lock模式的SNR历史缓冲区，用于计算平均SNR
|
||
/// </summary>
|
||
private CircularBuffer<double> lockSnrHistory = new(LOCK_SNR_WINDOW_SIZE);
|
||
|
||
/// <summary>
|
||
/// Track模式探测历史滑动窗口大小
|
||
/// </summary>
|
||
private const int TRACK_DETECTION_WINDOW_SIZE = 8;
|
||
|
||
/// <summary>
|
||
/// Lock模式SNR滑动窗口大小
|
||
/// </summary>
|
||
private const int LOCK_SNR_WINDOW_SIZE = 8;
|
||
|
||
/// <summary>
|
||
/// Track模式切换到Lock模式的成功率阈值
|
||
/// </summary>
|
||
private const double SUCCESS_RATE_THRESHOLD = 0.5;
|
||
|
||
/// <summary>
|
||
/// 是否有目标
|
||
/// </summary>
|
||
public bool HasTarget { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 是否在锁定模式
|
||
/// </summary>
|
||
public bool IsInLockMode => currentMode == WorkMode.Lock;
|
||
|
||
/// <summary>
|
||
/// 当前扫描半径，单位：弧度
|
||
/// </summary>
|
||
private double currentScanRadius = 0;
|
||
|
||
/// <summary>
|
||
/// 螺旋扫描角度，单位：弧度
|
||
/// </summary>
|
||
private double spiralAngle = 0;
|
||
|
||
/// <summary>
|
||
/// 螺旋扫描半径，单位：弧度
|
||
/// </summary>
|
||
private double spiralRadius = 0;
|
||
|
||
/// <summary>
|
||
/// 角度误差滤波器 - 方位角
|
||
/// </summary>
|
||
private double filteredAzimuthError = 0;
|
||
|
||
/// <summary>
|
||
/// 角度误差滤波器 - 俯仰角
|
||
/// </summary>
|
||
private double filteredElevationError = 0;
|
||
|
||
/// <summary>
|
||
/// 滤波器系数 (0-1, 越小越平滑)
|
||
/// </summary>
|
||
private const double FILTER_COEFFICIENT = 0.3;
|
||
|
||
/// <summary>
|
||
/// 最大扫描半径，单位：弧度
|
||
/// </summary>
|
||
private double MaxScanRadius => config.FieldOfViewAngle * Math.PI / 180.0 / 2;
|
||
|
||
/// <summary>
|
||
/// 光速，单位：m/s
|
||
/// </summary>
|
||
private const double LIGHT_SPEED = 299792458.0; // 光速 m/s
|
||
|
||
/// <summary>
|
||
/// Swerling模型实例
|
||
/// </summary>
|
||
private readonly SwerlingRcsModel swerlingRcsModel;
|
||
|
||
/// <summary>
|
||
/// 初始化毫米波制导系统的新实例
|
||
/// </summary>
|
||
/// <param name="id">制导系统ID</param>
|
||
/// <param name="missileId">导弹ID</param>
|
||
/// <param name="maxAcceleration">最大加速度，单位：米/平方秒</param>
|
||
/// <param name="guidanceCoefficient">制导系数</param>
|
||
/// <param name="simulationManager">仿真管理器实例</param>
|
||
/// <param name="config">毫米波导引系统配置</param>
|
||
/// <remarks>
|
||
/// 构造过程：
|
||
/// - 初始化基类参数
|
||
/// - 设置仿真管理器
|
||
/// - 初始化目标位置
|
||
/// - 注册干扰事件处理
|
||
/// - 加载干扰阈值配置
|
||
/// </remarks>
|
||
public MillimeterWaveGuidanceSystem(
|
||
string id,
|
||
string missileId,
|
||
double maxAcceleration,
|
||
double guidanceCoefficient,
|
||
MillimeterWaveGuidanceConfig config,
|
||
ISimulationManager simulationManager)
|
||
: base(id, missileId, maxAcceleration, guidanceCoefficient, simulationManager)
|
||
{
|
||
this.config = config;
|
||
InitializeJamming(config.JammingResistanceThreshold, SupportedJammingTypes, SupportedBlockingJammingTypes);
|
||
SwitchToSearchMode(); // 初始化为搜索模式
|
||
swerlingRcsModel = new SwerlingRcsModel();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换到搜索模式
|
||
/// </summary>
|
||
public void SwitchToSearchMode()
|
||
{
|
||
currentMode = WorkMode.Search;
|
||
HasTarget = false;
|
||
HasGuidance = false;
|
||
lockConfirmationTimer = 0;
|
||
currentScanRadius = 0;
|
||
spiralAngle = 0;
|
||
spiralRadius = 0;
|
||
LastTargetPosition = null;
|
||
LastTargetVelocity = null;
|
||
|
||
activeDetectionHistory.Clear();
|
||
lockSnrHistory.Clear(); // 清理SNR历史
|
||
currentlyTrackedTargetId = null;
|
||
|
||
Trace.TraceInformation($"切换到搜索模式，波束宽度: {config.SearchBeamWidth}度");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换到跟踪模式
|
||
/// </summary>
|
||
private void SwitchToTrackMode()
|
||
{
|
||
currentMode = WorkMode.Track;
|
||
lockConfirmationTimer = 0;
|
||
HasGuidance = true;
|
||
|
||
activeDetectionHistory.Clear();
|
||
|
||
// 调整导引头朝向目标
|
||
UpdateSeekerOrientation();
|
||
|
||
Trace.TraceInformation($"切换到跟踪模式，波束宽度: {config.TrackBeamWidth}度");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换到锁定模式
|
||
/// </summary>
|
||
private void SwitchToLockMode()
|
||
{
|
||
currentMode = WorkMode.Lock;
|
||
HasGuidance = true;
|
||
|
||
// 调整导引头朝向目标
|
||
UpdateSeekerOrientation();
|
||
|
||
Trace.TraceInformation("切换到锁定模式 - 目标锁定成功");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新导引头朝向，使其指向当前跟踪的目标
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 在Track和Lock模式下，导引头需要持续指向目标以保持跟踪
|
||
/// </remarks>
|
||
private void UpdateSeekerOrientation()
|
||
{
|
||
if (currentlyTrackedTargetId != null && LastTargetPosition.HasValue)
|
||
{
|
||
// 计算从导弹到目标的方向向量
|
||
Vector3D toTarget = (LastTargetPosition.Value - KState.Position).Normalize();
|
||
|
||
// 将方向向量转换为朝向
|
||
Orientation targetOrientation = Orientation.FromVector(toTarget);
|
||
|
||
// 更新导引头朝向
|
||
KState.Orientation = targetOrientation;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 激活制导系统
|
||
/// </summary>
|
||
public override void Activate()
|
||
{
|
||
if (!IsActive)
|
||
{
|
||
IsActive = true;
|
||
SimulationManager.SubscribeToEvent<JammingEvent>(HandleJammingEvent);
|
||
}
|
||
base.Activate();
|
||
SwitchToSearchMode();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 停用制导系统
|
||
/// </summary>
|
||
public override void Deactivate()
|
||
{
|
||
if (IsActive)
|
||
{
|
||
IsActive = false;
|
||
SimulationManager.UnsubscribeFromEvent<JammingEvent>(HandleJammingEvent);
|
||
}
|
||
base.Deactivate();
|
||
HasTarget = false;
|
||
HasGuidance = false;
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
LastTargetPosition = null;
|
||
LastTargetVelocity = null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 处理系统被干扰的事件
|
||
/// </summary>
|
||
/// <param name="parameters">干扰参数</param>
|
||
protected override void HandleJammingApplied(JammingParameters parameters)
|
||
{
|
||
base.HandleJammingApplied(parameters);
|
||
|
||
if (parameters.Type == JammingType.MillimeterWave)
|
||
{
|
||
Debug.WriteLine($"[MMW_GUIDANCE] 受到毫米波干扰，功率：{parameters.Power} W。切换到搜索模式。", "Jamming");
|
||
if (currentMode != WorkMode.Search)
|
||
{
|
||
SwitchToSearchMode();
|
||
}
|
||
}
|
||
else if (parameters.Type == JammingType.SmokeGrenade)
|
||
{
|
||
Debug.WriteLine($"[MMW_GUIDANCE] 受到烟幕干扰影响。Jammer ID: {parameters.JammerId}", "Jamming");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 处理系统干扰被清除的事件
|
||
/// </summary>
|
||
/// <param name="parameters">干扰参数</param>
|
||
protected override void HandleJammingCleared(JammingParameters parameters)
|
||
{
|
||
base.HandleJammingCleared(parameters);
|
||
|
||
if (parameters.Type == JammingType.MillimeterWave)
|
||
{
|
||
Debug.WriteLine($"[MMW_GUIDANCE] 毫米波干扰已清除。", "Jamming");
|
||
}
|
||
else if (parameters.Type == JammingType.SmokeGrenade)
|
||
{
|
||
Debug.WriteLine($"[MMW_GUIDANCE] 烟幕干扰解除。Jammer ID: {parameters.JammerId}", "Jamming");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断是否应该处理传入的干扰参数（毫米波干扰需要额外检查波长）
|
||
/// </summary>
|
||
/// <param name="parameters">干扰参数</param>
|
||
/// <returns>如果应该处理则返回 true，否则返回 false</returns>
|
||
protected override bool ShouldHandleJamming(JammingParameters parameters)
|
||
{
|
||
if (!base.ShouldHandleJamming(parameters))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (parameters.Type == JammingType.MillimeterWave)
|
||
{
|
||
const double speedOfLight = 3e8;
|
||
double configWavelength_mm = speedOfLight / config.WaveFrequency * 1000.0; // m/s / Hz * 1000 = mm
|
||
|
||
if (Math.Abs((parameters.Wavelength ?? 0) - configWavelength_mm) > 1e-1) // 允许 0.1mm 偏差
|
||
{
|
||
Debug.WriteLine($"[MMW_GUIDANCE] {Id} 忽略毫米波干扰：干扰波长 {parameters.Wavelength}mm 与系统工作波长 {configWavelength_mm:F2}mm (频率 {config.WaveFrequency/1e9:F1}GHz) 不匹配。", "Jamming");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新扫描模式参数
|
||
/// </summary>
|
||
private void UpdateScanPattern(double deltaTime)
|
||
{
|
||
if (currentMode == WorkMode.Search)
|
||
{
|
||
// 螺旋扫描算法
|
||
spiralAngle += config.ScanAngularSpeedDeg * Math.PI / 180.0 * deltaTime;
|
||
|
||
// 计算螺旋半径（阿基米德螺旋）
|
||
double maxRadius = MaxScanRadius;
|
||
double totalAngle = config.SpiralTurns * 2 * Math.PI;
|
||
|
||
// 自适应螺旋密度：根据扫描进度动态调整
|
||
double adaptiveTightness = config.SpiralTightness;
|
||
double scanProgress = spiralAngle / totalAngle;
|
||
|
||
// 早期扫描时增加密度，后期适当放宽
|
||
if (scanProgress < 0.3) // 前30%扫描时间
|
||
{
|
||
adaptiveTightness *= 1.3; // 增加30%密度
|
||
}
|
||
else if (scanProgress > 0.7) // 后30%扫描时间
|
||
{
|
||
adaptiveTightness *= 0.9; // 减少10%密度，加快覆盖
|
||
}
|
||
|
||
// 修正螺旋半径计算 - 确保能覆盖整个视场
|
||
spiralRadius = (spiralAngle / totalAngle) * maxRadius * adaptiveTightness;
|
||
|
||
// 检查是否完成一次完整扫描
|
||
if (spiralAngle >= totalAngle)
|
||
{
|
||
Debug.WriteLine($"[螺旋扫描] 完成一次完整扫描，重新开始。总角度: {totalAngle * 180/Math.PI:F1}度");
|
||
spiralAngle = 0;
|
||
spiralRadius = 0;
|
||
}
|
||
|
||
// 更新传统扫描参数以保持兼容性
|
||
currentScanRadius = spiralRadius;
|
||
|
||
Debug.WriteLine($"螺旋扫描参数 - 半径: {spiralRadius * 180/Math.PI:F2}度, 角度: {spiralAngle * 180/Math.PI:F2}度, 最大半径: {maxRadius * 180/Math.PI:F2}度, 自适应系数: {adaptiveTightness:F2}");
|
||
}
|
||
else
|
||
{
|
||
currentScanRadius = config.SearchBeamWidth * Math.PI / 720.0;
|
||
spiralRadius = currentScanRadius;
|
||
}
|
||
|
||
// 更新扫描周期计时器
|
||
if (scanCycleTimer < config.FieldOfViewAngle / config.ScanAngularSpeedDeg)
|
||
{
|
||
scanCycleTimer += deltaTime;
|
||
}
|
||
else
|
||
{
|
||
scanCycleTimer = 0;
|
||
swerlingRcsModel.ClearAllCachedRcs();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单脉冲测角算法
|
||
/// </summary>
|
||
private (double azimuthError, double elevationError) CalculateMonopulseAngularError(Vector3D targetDirection, Vector3D beamDirection)
|
||
{
|
||
// 计算目标相对于波束中心的角度误差
|
||
Vector3D errorVector = targetDirection - beamDirection;
|
||
|
||
// 建立波束坐标系
|
||
Vector3D beamAxis = beamDirection.Normalize();
|
||
Vector3D upVector = Vector3D.UnitY;
|
||
|
||
// 处理垂直情况
|
||
if (Math.Abs(beamAxis.Y) > 0.9)
|
||
{
|
||
upVector = Vector3D.UnitZ;
|
||
}
|
||
|
||
Vector3D rightVector = Vector3D.CrossProduct(beamAxis, upVector).Normalize();
|
||
Vector3D elevationVector = Vector3D.CrossProduct(rightVector, beamAxis).Normalize();
|
||
|
||
// 计算角度误差（弧度）
|
||
double azimuthErrorRaw = Vector3D.DotProduct(errorVector, rightVector) * config.MonopulseSensitivity;
|
||
double elevationErrorRaw = Vector3D.DotProduct(errorVector, elevationVector) * config.MonopulseSensitivity;
|
||
|
||
// 应用低通滤波器减少噪声
|
||
filteredAzimuthError = FILTER_COEFFICIENT * azimuthErrorRaw + (1 - FILTER_COEFFICIENT) * filteredAzimuthError;
|
||
filteredElevationError = FILTER_COEFFICIENT * elevationErrorRaw + (1 - FILTER_COEFFICIENT) * filteredElevationError;
|
||
|
||
// 转换为度数
|
||
double azimuthErrorDeg = filteredAzimuthError * 180.0 / Math.PI;
|
||
double elevationErrorDeg = filteredElevationError * 180.0 / Math.PI;
|
||
|
||
Debug.WriteLine($"[单脉冲测角] 原始误差: 方位{azimuthErrorRaw * 180/Math.PI:F3}°, 俯仰{elevationErrorRaw * 180/Math.PI:F3}°");
|
||
Debug.WriteLine($"[单脉冲测角] 滤波误差: 方位{azimuthErrorDeg:F3}°, 俯仰{elevationErrorDeg:F3}°");
|
||
|
||
return (azimuthErrorDeg, elevationErrorDeg);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算Lock模式的平均SNR
|
||
/// </summary>
|
||
/// <returns>平均SNR值，如果历史数据不足则返回当前SNR</returns>
|
||
private double CalculateAverageLockSnr()
|
||
{
|
||
if (lockSnrHistory.Count == 0)
|
||
{
|
||
return double.MinValue; // 无历史数据
|
||
}
|
||
|
||
double averageSnr = lockSnrHistory.CalculateAverage();
|
||
Debug.WriteLine($"[SNR平均] 窗口大小: {lockSnrHistory.Count}/{LOCK_SNR_WINDOW_SIZE}, 平均SNR: {averageSnr:F2}dB");
|
||
return averageSnr;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算目标是否在当前扫描波束内
|
||
/// </summary>
|
||
private bool IsTargetInBeam(Vector3D missileVelocity, Vector3D toTarget)
|
||
{
|
||
// 根据当前模式选择波束宽度
|
||
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 = missileVelocity.Normalize();
|
||
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 = spiralRadius; // 从中心向外的偏移角度
|
||
double rotationAngle = spiralAngle; // 绕轴的旋转角度
|
||
|
||
// 使用球坐标系计算扫描方向
|
||
double x = Math.Cos(offsetAngle); // 主轴分量（前进方向）
|
||
double y = Math.Sin(offsetAngle) * Math.Cos(rotationAngle); // Y轴分量
|
||
double z = Math.Sin(offsetAngle) * Math.Sin(rotationAngle); // Z轴分量
|
||
|
||
// 合成扫描方向向量
|
||
Vector3D scanDirection = (
|
||
xAxis * x +
|
||
yAxis * y +
|
||
zAxis * z
|
||
).Normalize();
|
||
|
||
// 调试输出
|
||
Debug.WriteLine($"导弹前进方向: {xAxis}");
|
||
Debug.WriteLine($"目标方向: {toTarget.Normalize()}");
|
||
Debug.WriteLine($"螺旋扫描方向: {scanDirection}");
|
||
Debug.WriteLine($"螺旋参数 - 偏移角: {offsetAngle * 180/Math.PI:F2}度, 旋转角: {rotationAngle * 180/Math.PI:F2}度");
|
||
|
||
// 计算目标夹角
|
||
double angle = Vector3D.AngleBetween(scanDirection, toTarget.Normalize());
|
||
Debug.WriteLine($"目标夹角: {angle * 180/Math.PI:F2}度");
|
||
Debug.WriteLine($"波束宽度: {currentBeamWidth * 180/Math.PI:F2}度，SNR阈值: {config.RecognitionSNRThreshold}dB");
|
||
|
||
return angle <= (currentBeamWidth / 2.0);
|
||
}
|
||
else
|
||
{
|
||
// 跟踪/锁定模式使用单脉冲测角
|
||
var (azError, elError) = CalculateMonopulseAngularError(toTarget.Normalize(), missileVelocity);
|
||
Debug.WriteLine($"[单脉冲测角] 方位误差: {azError:F3}度, 俯仰误差: {elError:F3}度");
|
||
|
||
// 将度数转换为弧度进行比较
|
||
double azErrorRad = azError * Math.PI / 180.0;
|
||
double elErrorRad = elError * Math.PI / 180.0;
|
||
double totalErrorRad = Math.Sqrt(azErrorRad * azErrorRad + elErrorRad * elErrorRad);
|
||
|
||
Debug.WriteLine($"[波束宽度检查] 当前波束宽度的一半: {(currentBeamWidth / 2.0) * 180/Math.PI:F3}度，实际误差合成: {totalErrorRad * 180/Math.PI:F3}度");
|
||
return totalErrorRad <= (currentBeamWidth / 2.0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新制导系统的状态
|
||
/// </summary>
|
||
/// <param name="deltaTime">自上次更新以来的时间间隔，单位：秒</param>
|
||
/// <remarks>
|
||
/// 更新过程：
|
||
/// - 探测目标位置
|
||
/// - 计算目标速度
|
||
/// - 生成制导指令
|
||
/// - 更新导引头朝向（Track和Lock模式）
|
||
/// - 限制最大加速度
|
||
/// </remarks>
|
||
public override void Update(double deltaTime)
|
||
{
|
||
base.Update(deltaTime);
|
||
|
||
if (IsBlockingJammed)
|
||
{
|
||
if (currentMode != WorkMode.Search) SwitchToSearchMode();
|
||
HasTarget = false;
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
HasGuidance = false;
|
||
return;
|
||
}
|
||
|
||
UpdateScanPattern(deltaTime);
|
||
|
||
TryDetectAndTrackTarget(KState.Position, KState.Velocity, deltaTime, out Vector3D currentTargetPositionFromDetector);
|
||
|
||
HasGuidance = HasTarget;
|
||
|
||
if (HasGuidance)
|
||
{
|
||
Vector3D? currentTargetVelocity = null;
|
||
if (LastTargetPosition.HasValue && deltaTime > 0 && currentTargetPositionFromDetector != Vector3D.Zero)
|
||
{
|
||
currentTargetVelocity = (currentTargetPositionFromDetector - LastTargetPosition.Value) / deltaTime;
|
||
}
|
||
LastTargetVelocity = currentTargetVelocity;
|
||
|
||
GuidanceAcceleration = MotionAlgorithm.CalculateProportionalNavigation(
|
||
ProportionalNavigationCoefficient,
|
||
KState.Position,
|
||
KState.Velocity,
|
||
currentTargetPositionFromDetector,
|
||
LastTargetVelocity ?? Vector3D.Zero
|
||
);
|
||
|
||
if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
|
||
{
|
||
GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
|
||
}
|
||
|
||
// 在Track和Lock模式下持续调整导引头朝向
|
||
if (currentMode == WorkMode.Track || currentMode == WorkMode.Lock)
|
||
{
|
||
UpdateSeekerOrientation();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
LastTargetVelocity = null;
|
||
}
|
||
HasGuidance = HasTarget;
|
||
}
|
||
|
||
/// <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;
|
||
|
||
foreach (var element in SimulationManager.GetEntitiesByType<SimulationElement>())
|
||
{
|
||
if (!(element is BaseEquipment target && target.Id != MissileId && target.IsActive))
|
||
{
|
||
continue; // 卫语句: 跳过自身或无效目标
|
||
}
|
||
|
||
Vector3D toTarget = target.KState.Position - missilePosition;
|
||
double distance = toTarget.Magnitude();
|
||
|
||
// 卫语句: 超出最大探测距离或距离无效
|
||
if (!(distance <= config.MaxDetectionRange && distance > 0))
|
||
{
|
||
if ((currentMode == WorkMode.Track || currentMode == WorkMode.Lock) && target.Id == currentlyTrackedTargetId)
|
||
{
|
||
Debug.WriteLine($"[MMW {currentMode.ToString().ToUpper()}] 当前跟踪目标 {currentlyTrackedTargetId} 超出最大探测距离 ({distance:F0}m > {config.MaxDetectionRange:F0}m) 或距离无效。");
|
||
}
|
||
else
|
||
{
|
||
continue;
|
||
}
|
||
}
|
||
|
||
double liveSmokeTransmittance = CalculateLiveSmokeTransmittance(missilePosition, target.KState.Position);
|
||
double rcsDbSm = RcsPattern.DEFAULT_RCS_DBSM;
|
||
double rcsLinear;
|
||
Vector3D targetForward = Vector3D.Zero;
|
||
Vector3D targetRight = Vector3D.Zero;
|
||
Vector3D targetUp = Vector3D.Zero;
|
||
bool isTargetOrientationValid = false;
|
||
MotionStateType motionState = MotionStateType.Static;
|
||
|
||
if (target.KState != null)
|
||
{
|
||
targetForward = target.KState.Orientation.ToVector().Normalize();
|
||
if (targetForward.MagnitudeSquared() > 1e-10)
|
||
{
|
||
Vector3D worldUpRef = Vector3D.UnitY;
|
||
if (Math.Abs(Vector3D.DotProduct(targetForward, worldUpRef)) > 0.99) worldUpRef = Vector3D.UnitZ;
|
||
targetRight = Vector3D.CrossProduct(targetForward, worldUpRef).Normalize();
|
||
if (targetRight.MagnitudeSquared() > 1e-10)
|
||
{
|
||
targetUp = Vector3D.CrossProduct(targetRight, targetForward).Normalize();
|
||
if (targetUp.MagnitudeSquared() > 1e-10) isTargetOrientationValid = true;
|
||
}
|
||
}
|
||
if (target.KState.Speed > 0.1) motionState = MotionStateType.ConstantVelocity;
|
||
}
|
||
if (!isTargetOrientationValid) Debug.WriteLine($"[MMW_GUIDANCE] Target {target.Id}: 方向无效或 KState 为 null. 使用默认RCS.");
|
||
|
||
if (target.Properties.RcsPattern != null)
|
||
{
|
||
if (isTargetOrientationValid) rcsDbSm = target.Properties.RcsPattern.GetRcsValueDbSm(missilePosition - target.KState!.Position, targetForward, targetUp, targetRight);
|
||
}
|
||
else Debug.WriteLine($"[MMW_GUIDANCE] Target {target.Id}: 没有 RcsPattern 对象. 使用默认RCS.");
|
||
|
||
rcsLinear = Math.Pow(10, rcsDbSm / 10.0);
|
||
Debug.WriteLine($"[MMW_GUIDANCE] 目标 {target.Id}: 原始RCS: {rcsDbSm:F2} dBsm ({rcsLinear:F4} m²).");
|
||
bool isNewScanPeriodForRcs = currentMode == WorkMode.Track || currentMode == WorkMode.Lock;
|
||
double originalRcsLinear = rcsLinear;
|
||
rcsLinear = swerlingRcsModel.GetRealtimeRcs(target.Id, target.Properties.Type, motionState, rcsLinear, isNewScanPeriodForRcs);
|
||
double rcsDbSmAfterSwerling = 10.0 * Math.Log10(rcsLinear);
|
||
double rcsVariationDb = rcsDbSmAfterSwerling - rcsDbSm;
|
||
Debug.WriteLine($"[MMW_GUIDANCE] 目标 {target.Id}: Swerling后RCS: {rcsDbSmAfterSwerling:F2} dBsm ({rcsLinear:F4} m²), 波动: {rcsVariationDb:F2} dB.");
|
||
double localSnrDb = CalculateSNR(distance, rcsLinear, liveSmokeTransmittance);
|
||
|
||
switch (currentMode)
|
||
{
|
||
case WorkMode.Search:
|
||
if (IsTargetInBeam(missileVelocity, toTarget) && localSnrDb >= config.RecognitionSNRThreshold)
|
||
{
|
||
Debug.WriteLine($"[MMW SEARCH] 发现目标: {target.Id}, SNR: {localSnrDb:F2} dB.");
|
||
targetPosition = target.KState!.Position;
|
||
UpdateStateAndPotentiallySwitchMode(true, localSnrDb, target.Id, targetPosition, deltaTime);
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case WorkMode.Track:
|
||
if (currentlyTrackedTargetId == null || target.Id != currentlyTrackedTargetId)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
bool currentFrameSuccessForTrack = IsTargetInBeam(missileVelocity, toTarget) && localSnrDb >= config.RecognitionSNRThreshold;
|
||
activeDetectionHistory.Add(currentFrameSuccessForTrack);
|
||
|
||
Debug.WriteLine($"[MMW TRACK] 目标 {currentlyTrackedTargetId} 本帧探测: {currentFrameSuccessForTrack}, SNR: {localSnrDb:F2} dB. 窗口 ({activeDetectionHistory.Count}/{TRACK_DETECTION_WINDOW_SIZE})");
|
||
|
||
bool trackOverallSuccess;
|
||
if (activeDetectionHistory.Count == TRACK_DETECTION_WINDOW_SIZE)
|
||
{
|
||
double successRate = activeDetectionHistory.CalculateBooleanSuccessRate();
|
||
trackOverallSuccess = successRate >= SUCCESS_RATE_THRESHOLD;
|
||
if(trackOverallSuccess) Debug.WriteLine($"[MMW TRACK] 目标 {currentlyTrackedTargetId} 滑动窗口稳定 (成功率: {successRate:P2}).");
|
||
else Debug.WriteLine($"[MMW TRACK] 目标 {currentlyTrackedTargetId} 滑动窗口不稳定 (成功率: {successRate:P2}).");
|
||
}
|
||
else
|
||
{
|
||
trackOverallSuccess = currentFrameSuccessForTrack;
|
||
Debug.WriteLine($"[MMW TRACK] 目标 {currentlyTrackedTargetId} 窗口未满. 当前帧成功: {currentFrameSuccessForTrack}.");
|
||
}
|
||
|
||
targetPosition = trackOverallSuccess ?
|
||
(currentFrameSuccessForTrack ? target.KState!.Position : (LastTargetPosition ?? Vector3D.Zero)) :
|
||
Vector3D.Zero;
|
||
UpdateStateAndPotentiallySwitchMode(trackOverallSuccess, localSnrDb, currentlyTrackedTargetId, targetPosition, deltaTime);
|
||
return trackOverallSuccess;
|
||
|
||
case WorkMode.Lock:
|
||
if (currentlyTrackedTargetId == null || target.Id != currentlyTrackedTargetId)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
// 添加SNR到历史缓冲区
|
||
lockSnrHistory.Add(localSnrDb);
|
||
|
||
// 计算平均SNR
|
||
double averageSnr = CalculateAverageLockSnr();
|
||
|
||
// 使用平均SNR进行判断（如果历史数据不足，则使用瞬时SNR）
|
||
double snrForDecision = lockSnrHistory.Count >= LOCK_SNR_WINDOW_SIZE ? averageSnr : localSnrDb;
|
||
bool currentFrameSuccessForLock = IsTargetInBeam(missileVelocity, toTarget) && snrForDecision >= config.LockSNRThreshold;
|
||
|
||
activeDetectionHistory.Add(currentFrameSuccessForLock);
|
||
|
||
Debug.WriteLine($"[MMW LOCK] 目标 {currentlyTrackedTargetId} 本帧探测: {currentFrameSuccessForLock}, 瞬时SNR: {localSnrDb:F2}dB, 平均SNR: {averageSnr:F2}dB, 决策SNR: {snrForDecision:F2}dB. 窗口 ({activeDetectionHistory.Count}/{TRACK_DETECTION_WINDOW_SIZE}).");
|
||
|
||
bool lockOverallSuccess;
|
||
if (activeDetectionHistory.Count == TRACK_DETECTION_WINDOW_SIZE)
|
||
{
|
||
double successRate = activeDetectionHistory.CalculateBooleanSuccessRate();
|
||
lockOverallSuccess = successRate >= SUCCESS_RATE_THRESHOLD;
|
||
if(lockOverallSuccess) Debug.WriteLine($"[MMW LOCK] 目标 {currentlyTrackedTargetId} 滑动窗口稳定 (成功率: {successRate:P2}).");
|
||
else Debug.WriteLine($"[MMW LOCK] 目标 {currentlyTrackedTargetId} 滑动窗口不稳定 (成功率: {successRate:P2}).");
|
||
}
|
||
else
|
||
{
|
||
lockOverallSuccess = currentFrameSuccessForLock;
|
||
Debug.WriteLine($"[MMW LOCK] 目标 {currentlyTrackedTargetId} 窗口未满. 当前帧成功: {currentFrameSuccessForLock}.");
|
||
}
|
||
|
||
targetPosition = lockOverallSuccess ?
|
||
(currentFrameSuccessForLock ? target.KState!.Position : (LastTargetPosition ?? Vector3D.Zero)) :
|
||
Vector3D.Zero;
|
||
UpdateStateAndPotentiallySwitchMode(lockOverallSuccess, snrForDecision, currentlyTrackedTargetId, targetPosition, deltaTime);
|
||
return lockOverallSuccess;
|
||
}
|
||
}
|
||
|
||
UpdateStateAndPotentiallySwitchMode(false, double.MinValue, currentlyTrackedTargetId, Vector3D.Zero, deltaTime);
|
||
targetPosition = Vector3D.Zero;
|
||
return false;
|
||
}
|
||
|
||
private void UpdateStateAndPotentiallySwitchMode(bool frameSuccess, double snr, string? newOrCurrentTargetId, Vector3D newOrCurrentPosition, double deltaTime)
|
||
{
|
||
HasTarget = frameSuccess;
|
||
|
||
if (frameSuccess && newOrCurrentPosition != Vector3D.Zero)
|
||
{
|
||
LastTargetPosition = newOrCurrentPosition;
|
||
}
|
||
|
||
if (!frameSuccess)
|
||
{
|
||
if (currentMode == WorkMode.Track || currentMode == WorkMode.Lock)
|
||
{
|
||
Debug.WriteLine($"[MMW {currentMode.ToString().ToUpper()}] 判定目标丢失/不稳定 (frameSuccess=false)，切换到搜索模式。");
|
||
SwitchToSearchMode();
|
||
}
|
||
return;
|
||
}
|
||
|
||
switch (currentMode)
|
||
{
|
||
case WorkMode.Search:
|
||
if (newOrCurrentTargetId != null)
|
||
{
|
||
currentlyTrackedTargetId = newOrCurrentTargetId;
|
||
Debug.WriteLine($"[MMW SEARCH] UpdateState确认目标 {newOrCurrentTargetId}，准备切换到Track。");
|
||
SwitchToTrackMode();
|
||
}
|
||
else
|
||
{
|
||
Debug.WriteLine("[MMW SEARCH] Warning: UpdateState收到Search成功但无TargetId。");
|
||
}
|
||
break;
|
||
|
||
case WorkMode.Track:
|
||
lockConfirmationTimer += deltaTime;
|
||
Debug.WriteLine($"[MMW TRACK] UpdateState目标 {currentlyTrackedTargetId} 跟踪稳定. Lock确认计时: {lockConfirmationTimer:F2}s / {config.LockConfirmationTime:F2}s. SNR决策值: {snr:F2}dB");
|
||
if (lockConfirmationTimer >= config.LockConfirmationTime)
|
||
{
|
||
Debug.WriteLine($"[MMW TRACK] UpdateState达到锁定持续时间. 切换到Lock模式.");
|
||
SwitchToLockMode();
|
||
}
|
||
break;
|
||
|
||
case WorkMode.Lock:
|
||
Debug.WriteLine($"[MMW LOCK] UpdateState目标 {currentlyTrackedTargetId} 锁定稳定. SNR决策值: {snr:F2}dB");
|
||
break;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取制导系统的详细状态信息
|
||
/// </summary>
|
||
/// <returns>包含完整状态参数的ElementStatusInfo对象</returns>
|
||
/// <remarks>
|
||
/// 特有的返回信息：
|
||
/// - 当前模式
|
||
/// - 目标位置
|
||
/// - 目标速度
|
||
/// - 是否有目标
|
||
/// 用于系统监控和调试
|
||
/// </remarks>
|
||
public override ElementStatusInfo GetStatusInfo()
|
||
{
|
||
var statusInfo = base.GetStatusInfo();
|
||
|
||
string lastPosStr = LastTargetPosition.HasValue ? LastTargetPosition.Value.ToString() : "null";
|
||
string lastVelStr = LastTargetVelocity.HasValue ? LastTargetVelocity.Value.ToString() : "null";
|
||
|
||
statusInfo.ExtendedProperties["Mode"] = currentMode.ToString();
|
||
statusInfo.ExtendedProperties["LastTargetPosition"] = lastPosStr;
|
||
statusInfo.ExtendedProperties["LastTargetVelocity"] = lastVelStr;
|
||
statusInfo.ExtendedProperties["HasTarget"] = HasTarget.ToString();
|
||
|
||
return statusInfo;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算目标的信噪比
|
||
/// </summary>
|
||
/// <param name="distance">到目标的距离，单位：米</param>
|
||
/// <param name="radarCrossSection">目标雷达散射截面积，单位：平方米</param>
|
||
/// <param name="smokeTransmittanceLinear">当前视线的烟幕总线性透过率 (0.0 到 1.0)</param>
|
||
/// <returns>信噪比，单位：分贝</returns>
|
||
/// <remarks>
|
||
/// 计算过程：
|
||
/// - 设置雷达参数
|
||
/// - 计算信号功率
|
||
/// - 计算噪声功率
|
||
/// - 计算信噪比
|
||
/// - 转换为分贝值
|
||
/// - 应用烟幕衰减
|
||
/// </remarks>
|
||
private double CalculateSNR(double distance, double radarCrossSection, double smokeTransmittanceLinear)
|
||
{
|
||
// 雷达参数
|
||
double transmitPower = config.TransmitPower;
|
||
double antennaGain = Math.Pow(10, config.AntennaGainDB/10);
|
||
|
||
// 波长，单位：米 (m)，用于雷达方程核心计算
|
||
double wavelength_m = LIGHT_SPEED / config.WaveFrequency;
|
||
// 波长，单位：微米 (µm)，用于需要微米单位的函数调用
|
||
double wavelength_um = LIGHT_SPEED / config.WaveFrequency * 1e6;
|
||
|
||
double bandwidth = 1.0 / config.PulseDuration;
|
||
double noiseFigure = Math.Pow(10, config.NoiseFigureDB/10);
|
||
|
||
double atmosphericLoss_dB = 0.4 * distance / 1000.0;
|
||
double totalSystemLoss_linear = Math.Pow(10, (atmosphericLoss_dB + config.SystemLossDB) / 10.0);
|
||
|
||
double k = 1.38e-23;
|
||
double T0 = 290;
|
||
|
||
double signalPowerNumerator = transmitPower * Math.Pow(antennaGain, 2) * Math.Pow(wavelength_m, 2) * radarCrossSection; // Use wavelength_m
|
||
double signalPowerDenominator = Math.Pow(4 * Math.PI, 3) * Math.Pow(distance, 4) * totalSystemLoss_linear;
|
||
|
||
if (signalPowerDenominator == 0) return -100.0;
|
||
double signalPower = signalPowerNumerator / signalPowerDenominator;
|
||
|
||
double weatherSpecificAtmosphericTransmittance = 1.0;
|
||
if(SimulationManager?.CurrentWeather != null)
|
||
{
|
||
weatherSpecificAtmosphericTransmittance = AtmosphereDllWrapper.CalculateTransmittance(
|
||
distance,
|
||
RadiationType.MillimeterWave,
|
||
wavelength_um,
|
||
SimulationManager.CurrentWeather);
|
||
}
|
||
signalPower *= weatherSpecificAtmosphericTransmittance;
|
||
|
||
double noisePower = k * T0 * bandwidth * noiseFigure;
|
||
if (noisePower == 0) return 100.0;
|
||
|
||
double snr_linear = signalPower / noisePower;
|
||
|
||
if (snr_linear <= 0) return -100.0;
|
||
|
||
double snr_dB_no_smoke = 10 * Math.Log10(snr_linear);
|
||
Debug.WriteLine($"[SNR计算] SNR_dB (烟幕前): {snr_dB_no_smoke:F2} dB. Incoming SmokeTransmittance_Linear: {smokeTransmittanceLinear:F6}");
|
||
|
||
// 应用烟幕透过率
|
||
if (smokeTransmittanceLinear <= 1e-6) // 防止log(0)或非常大的负数
|
||
{
|
||
Debug.WriteLine($"[SNR计算] 烟幕透过率太低. Final_SNR_dB: -100.0 dB");
|
||
return -100.0; // 由于烟幕影响，信号实际上为零
|
||
}
|
||
|
||
double smokeEffect_dB = 10 * Math.Log10(smokeTransmittanceLinear);
|
||
double final_snr_dB = snr_dB_no_smoke + smokeEffect_dB;
|
||
Debug.WriteLine($"[SNR计算] 烟幕影响: {smokeEffect_dB:F2} dB. 最终SNR_dB (烟幕后): {final_snr_dB:F2} dB");
|
||
|
||
return final_snr_dB;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算给定观察点和目标点之间的总烟幕透过率。
|
||
/// </summary>
|
||
/// <param name="observerPosition">观察者位置</param>
|
||
/// <param name="targetEndPosition">目标位置</param>
|
||
/// <returns>总透过率 (0.0 到 1.0)。</returns>
|
||
private double CalculateLiveSmokeTransmittance(Vector3D observerPosition, Vector3D targetEndPosition)
|
||
{
|
||
double totalTransmittance = 1.0;
|
||
if (SimulationManager == null)
|
||
{
|
||
Trace.TraceWarning("[毫米波制导] 仿真管理器是 null, 假设没有烟幕.");
|
||
return 1.0;
|
||
}
|
||
|
||
var activeSmokeGrenades = SimulationManager.GetEntitiesByType<SmokeGrenade>()
|
||
.Where(sg => sg.IsActive && sg.IsJamming && sg.config != null)
|
||
.ToList();
|
||
|
||
if (activeSmokeGrenades.Count == 0)
|
||
{
|
||
return 1.0;
|
||
}
|
||
|
||
double wavelength_um = LIGHT_SPEED / config.WaveFrequency * 1e6;
|
||
|
||
if (wavelength_um <= 0) {
|
||
Trace.TraceInformation("[毫米波制导] 计算波长为米时无效. 假设没有烟幕影响.");
|
||
return 1.0;
|
||
}
|
||
|
||
foreach (var smokeGrenade in activeSmokeGrenades)
|
||
{
|
||
double transmittanceForThisSmoke = smokeGrenade.GetSmokeTransmittanceOnLine(observerPosition, targetEndPosition, wavelength_um);
|
||
totalTransmittance *= transmittanceForThisSmoke;
|
||
|
||
if (totalTransmittance < 1e-6) // 使用更小的阈值提前退出，如果透过率几乎为零
|
||
{
|
||
return 0.0;
|
||
}
|
||
}
|
||
return Math.Max(0.0, totalTransmittance);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|