372 lines
14 KiB
C#
372 lines
14 KiB
C#
using ThreatSource.Utils;
|
||
using System.Diagnostics;
|
||
namespace ThreatSource.Sensor
|
||
{
|
||
/// <summary>
|
||
/// 四象限探测器类，实现了激光半主动导弹光学导引头中的核心组件
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 该类提供了四象限探测器的核心功能：
|
||
/// - 四个独立光敏元件的信号处理
|
||
/// - 光斑中心位置计算
|
||
/// - 目标方向误差计算
|
||
/// - 实时状态更新
|
||
/// 用于激光半主动导弹的精确制导
|
||
/// </remarks>
|
||
public class QuadrantDetector
|
||
{
|
||
/// <summary>
|
||
/// 四个象限的光敏元件接收到的信号强度
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 按照右上、左上、左下、右下的顺序排列
|
||
/// 用于计算光斑中心位置
|
||
/// </remarks>
|
||
private double[] quadrantSignals = new double[4];
|
||
|
||
/// <summary>
|
||
/// 获取或设置探测器直径，单位：米
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 定义了四象限传感器的物理尺寸
|
||
/// 影响系统的探测精度
|
||
/// 典型值为0.03米
|
||
/// </remarks>
|
||
public double DetectorDiameter { get; set; }
|
||
|
||
/// <summary>
|
||
/// 获取或设置聚焦后光斑直径，单位：米
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 定义了光学系统聚焦后的光斑大小
|
||
/// 影响系统的探测灵敏度
|
||
/// 典型值为0.006米
|
||
/// </remarks>
|
||
public double FocusedSpotDiameter { get; set; }
|
||
|
||
/// <summary>
|
||
/// 获取或设置最小可检测功率，单位：瓦特
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 定义了探测器的灵敏度阈值
|
||
/// 低于此值时无法有效探测
|
||
/// 典型值为1e-12瓦特
|
||
/// </remarks>
|
||
public double MinDetectablePower { get; set; }
|
||
|
||
/// <summary>
|
||
/// 获取水平方向误差
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 范围：[-1, 1]
|
||
/// 正值表示光斑中心在探测器中心右侧
|
||
/// 负值表示光斑中心在探测器中心左侧
|
||
/// </remarks>
|
||
public double HorizontalError { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取垂直方向误差
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 范围：[-1, 1]
|
||
/// 正值表示光斑中心在探测器中心上方
|
||
/// 负值表示光斑中心在探测器中心下方
|
||
/// </remarks>
|
||
public double VerticalError { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 滤波后的水平方向误差
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 对原始水平误差进行滤波后的值
|
||
/// 用于减少误差信号的噪声和快速变化
|
||
/// </remarks>
|
||
private double filteredHorizontalError = 0;
|
||
|
||
/// <summary>
|
||
/// 滤波后的垂直方向误差
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 对原始垂直误差进行滤波后的值
|
||
/// 用于减少误差信号的噪声和快速变化
|
||
/// </remarks>
|
||
private double filteredVerticalError = 0;
|
||
|
||
/// <summary>
|
||
/// 误差滤波系数
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 控制滤波的强度，值越小滤波越强
|
||
/// 范围：(0,1]
|
||
/// 值为1时无滤波效果
|
||
/// </remarks>
|
||
private const double errorFilterFactor = 0.15;
|
||
|
||
/// <summary>
|
||
/// 误差死区阈值
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 小于此值的误差将被视为零
|
||
/// 用于避免对微小误差的过度响应
|
||
/// </remarks>
|
||
private const double errorDeadZone = 0.01;
|
||
|
||
/// <summary>
|
||
/// 获取总接收功率，单位：瓦特
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 四个象限接收到的总功率
|
||
/// 用于判断是否锁定目标
|
||
/// </remarks>
|
||
public double TotalReceivedPower { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取是否锁定目标
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// true表示接收到足够的信号强度，可以锁定目标
|
||
/// false表示信号强度不足，无法锁定目标
|
||
/// </remarks>
|
||
public bool IsTargetLocked { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 初始化四象限探测器的新实例
|
||
/// </summary>
|
||
/// <param name="detectorDiameter">传感器直径，单位：米</param>
|
||
/// <param name="focusedSpotDiameter">聚焦后光斑直径，单位：米</param>
|
||
/// <param name="minDetectablePower">最小可检测功率，单位：瓦特</param>
|
||
/// <remarks>
|
||
/// 构造过程：
|
||
/// - 设置传感器参数
|
||
/// - 初始化信号强度
|
||
/// - 初始化误差值
|
||
/// </remarks>
|
||
public QuadrantDetector(double detectorDiameter, double focusedSpotDiameter, double minDetectablePower)
|
||
{
|
||
DetectorDiameter = detectorDiameter;
|
||
FocusedSpotDiameter = focusedSpotDiameter;
|
||
MinDetectablePower = minDetectablePower;
|
||
|
||
// 初始化信号强度和误差值
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
quadrantSignals[i] = 0;
|
||
}
|
||
|
||
HorizontalError = 0;
|
||
VerticalError = 0;
|
||
filteredHorizontalError = 0;
|
||
filteredVerticalError = 0;
|
||
TotalReceivedPower = 0;
|
||
IsTargetLocked = false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 处理接收到的激光信号
|
||
/// </summary>
|
||
/// <param name="receivedPower">接收到的总功率，单位：瓦特</param>
|
||
/// <param name="spotOffset">光斑中心相对于探测器中心的偏移量，单位：米</param>
|
||
/// <remarks>
|
||
/// 处理过程：
|
||
/// - 计算每个象限接收到的功率
|
||
/// - 更新误差值
|
||
/// - 判断是否锁定目标
|
||
/// </remarks>
|
||
public void ProcessLaserSignal(double receivedPower, Vector2D spotOffset)
|
||
{
|
||
// 更新总接收功率
|
||
TotalReceivedPower = receivedPower;
|
||
|
||
// 判断是否锁定目标
|
||
IsTargetLocked = receivedPower >= MinDetectablePower;
|
||
|
||
// 如果功率不足，清空信号并返回
|
||
if (!IsTargetLocked)
|
||
{
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
quadrantSignals[i] = 0;
|
||
}
|
||
HorizontalError = 0;
|
||
VerticalError = 0;
|
||
filteredHorizontalError = 0;
|
||
filteredVerticalError = 0;
|
||
return;
|
||
}
|
||
|
||
// 计算光斑在每个象限的覆盖情况
|
||
CalculateQuadrantSignals(spotOffset);
|
||
|
||
// 计算误差值
|
||
CalculateErrors();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算每个象限接收到的信号强度
|
||
/// </summary>
|
||
/// <param name="spotOffset">光斑中心相对于探测器中心的偏移量，单位：米</param>
|
||
/// <remarks>
|
||
/// 计算过程：
|
||
/// - 根据光斑位置和大小计算与每个象限的重叠面积
|
||
/// - 根据重叠面积分配总功率
|
||
/// </remarks>
|
||
private void CalculateQuadrantSignals(Vector2D spotOffset)
|
||
{
|
||
// 探测器半径
|
||
double detectorRadius = DetectorDiameter / 2;
|
||
// 光斑半径
|
||
double spotRadius = FocusedSpotDiameter / 2;
|
||
|
||
// 光斑中心坐标
|
||
double spotX = spotOffset.X;
|
||
double spotY = spotOffset.Y;
|
||
|
||
// 简化模型：根据光斑中心到各象限中心的距离计算信号强度
|
||
// 象限中心坐标
|
||
double quadrantRadius = detectorRadius / 2;
|
||
Vector2D[] quadrantCenters = new Vector2D[4]
|
||
{
|
||
new Vector2D(quadrantRadius, quadrantRadius), // 右上
|
||
new Vector2D(-quadrantRadius, quadrantRadius), // 左上
|
||
new Vector2D(-quadrantRadius, -quadrantRadius), // 左下
|
||
new Vector2D(quadrantRadius, -quadrantRadius) // 右下
|
||
};
|
||
|
||
// 计算每个象限接收到的信号强度
|
||
double totalWeight = 0;
|
||
double[] weights = new double[4];
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
// 计算光斑中心到象限中心的距离
|
||
double distance = Math.Sqrt(
|
||
Math.Pow(spotX - quadrantCenters[i].X, 2) +
|
||
Math.Pow(spotY - quadrantCenters[i].Y, 2)
|
||
);
|
||
|
||
// 使用高斯分布模型计算权重
|
||
weights[i] = Math.Exp(-Math.Pow(distance, 2) / (2 * Math.Pow(spotRadius, 2)));
|
||
totalWeight += weights[i];
|
||
}
|
||
|
||
// 分配总功率
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
quadrantSignals[i] = totalWeight > 0 ?
|
||
TotalReceivedPower * weights[i] / totalWeight : 0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算水平和垂直方向误差
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 计算公式：
|
||
/// - 水平误差 = ((A+D) - (B+C)) / (A+B+C+D)
|
||
/// - 垂直误差 = ((A+B) - (C+D)) / (A+B+C+D)
|
||
/// 其中A、B、C、D分别为右上、左上、左下、右下象限的信号强度
|
||
/// </remarks>
|
||
private void CalculateErrors()
|
||
{
|
||
// 确保总功率不为零
|
||
if (TotalReceivedPower > 0)
|
||
{
|
||
// 计算水平误差
|
||
double rawHorizontalError = ((quadrantSignals[0] + quadrantSignals[3]) -
|
||
(quadrantSignals[1] + quadrantSignals[2])) /
|
||
TotalReceivedPower;
|
||
|
||
// 计算垂直误差
|
||
double rawVerticalError = ((quadrantSignals[0] + quadrantSignals[1]) -
|
||
(quadrantSignals[2] + quadrantSignals[3])) /
|
||
TotalReceivedPower;
|
||
|
||
// 限制误差范围在[-1, 1]之间
|
||
HorizontalError = Math.Max(-1, Math.Min(1, rawHorizontalError));
|
||
VerticalError = Math.Max(-1, Math.Min(1, rawVerticalError));
|
||
|
||
// 应用误差滤波
|
||
filteredHorizontalError = filteredHorizontalError * (1 - errorFilterFactor) +
|
||
HorizontalError * errorFilterFactor;
|
||
filteredVerticalError = filteredVerticalError * (1 - errorFilterFactor) +
|
||
VerticalError * errorFilterFactor;
|
||
|
||
// 应用误差死区
|
||
if (Math.Abs(filteredHorizontalError) < errorDeadZone)
|
||
filteredHorizontalError = 0;
|
||
if (Math.Abs(filteredVerticalError) < errorDeadZone)
|
||
filteredVerticalError = 0;
|
||
|
||
// 使用滤波后的误差替代原始误差
|
||
HorizontalError = filteredHorizontalError;
|
||
VerticalError = filteredVerticalError;
|
||
|
||
// 增强调试输出，同时显示水平和垂直误差
|
||
Debug.WriteLine($"原始误差: 水平={rawHorizontalError:F6}, 垂直={rawVerticalError:F6}, 滤波后: 水平={HorizontalError:F6}, 垂直={VerticalError:F6}");
|
||
}
|
||
else
|
||
{
|
||
HorizontalError = 0;
|
||
VerticalError = 0;
|
||
filteredHorizontalError = 0;
|
||
filteredVerticalError = 0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取目标方向向量
|
||
/// </summary>
|
||
/// <param name="currentDirection">当前指向方向</param>
|
||
/// <param name="sensitivity">灵敏度系数</param>
|
||
/// <returns>修正后的目标方向向量</returns>
|
||
/// <remarks>
|
||
/// 使用通用的正交基构建方法
|
||
/// 不依赖于特定的坐标系
|
||
/// 适用于任何飞行姿态
|
||
/// </remarks>
|
||
public Vector3D GetTargetDirection(Vector3D currentDirection, double sensitivity)
|
||
{
|
||
if (!IsTargetLocked)
|
||
{
|
||
return currentDirection;
|
||
}
|
||
|
||
// 1. 获取当前方向的单位向量
|
||
Vector3D forward = currentDirection.Normalize();
|
||
|
||
// 2. 构建任意正交基
|
||
// 选择一个非平行于forward的向量来构建正交基
|
||
Vector3D temp = Math.Abs(forward.X) < 0.9 ? Vector3D.UnitX : Vector3D.UnitY;
|
||
Vector3D right = Vector3D.CrossProduct(temp, forward).Normalize();
|
||
Vector3D up = Vector3D.CrossProduct(forward, right).Normalize();
|
||
|
||
// 3. 在这个局部坐标系中应用误差修正
|
||
// HorizontalError 对应 right 方向
|
||
// VerticalError 对应 up 方向
|
||
Vector3D correction = right * (HorizontalError * sensitivity) +
|
||
up * (VerticalError * sensitivity);
|
||
|
||
// 4. 计算新的方向向量并归一化
|
||
return (forward + correction).Normalize();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取探测器状态信息
|
||
/// </summary>
|
||
/// <returns>包含探测器状态的详细描述</returns>
|
||
/// <remarks>
|
||
/// 返回信息包括：
|
||
/// - 锁定状态
|
||
/// - 总接收功率
|
||
/// - 最小可探测功率
|
||
/// - 水平和垂直误差
|
||
/// - 各象限信号强度
|
||
/// </remarks>
|
||
public string GetStatus()
|
||
{
|
||
return $"锁定={IsTargetLocked}, 总功率={TotalReceivedPower:E} W, 最小可探测功率={MinDetectablePower:E} W," +
|
||
$" 水平误差={HorizontalError:F4}, 垂直误差={VerticalError:F4}," +
|
||
$" 象限信号=[{quadrantSignals[0]:E}, {quadrantSignals[1]:E}, {quadrantSignals[2]:E}, {quadrantSignals[3]:E}]";
|
||
}
|
||
}
|
||
} |