434 lines
16 KiB
C#
434 lines
16 KiB
C#
using System;
|
|
using System.Diagnostics;
|
|
using ClumsyCore.Interfaces;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.StateEstimation
|
|
{
|
|
/// <summary>
|
|
/// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。
|
|
/// </summary>
|
|
public sealed class DetourVehicleStateProvider
|
|
: IVehicleStateProvider
|
|
{
|
|
public const double DefaultMaximumLinearSpeedMetersPerSecond =
|
|
1.20;
|
|
public const double DefaultMaximumAngularSpeedRadiansPerSecond =
|
|
Math.PI / 4.0;
|
|
public const double DefaultPositionJumpMarginMeters =
|
|
0.03;
|
|
public const double DefaultHeadingJumpMarginRadians =
|
|
5.0 * Math.PI / 180.0;
|
|
public const double DefaultVelocityPositionResidualMeters =
|
|
0.04;
|
|
public const double DefaultVelocityHeadingResidualRadians =
|
|
5.0 * Math.PI / 180.0;
|
|
public const double DefaultStationaryConfirmationSeconds =
|
|
0.35;
|
|
|
|
private const double MillimetersPerMeter = 1000.0;
|
|
private const double PositionEqualityToleranceMeters = 1e-9;
|
|
private const double HeadingEqualityToleranceRadians = 1e-8;
|
|
|
|
private readonly object _syncRoot = new object();
|
|
private readonly Stopwatch _clock = Stopwatch.StartNew();
|
|
private readonly VelocityEstimator2D _velocityEstimator;
|
|
private readonly double _maximumLinearSpeedMetersPerSecond;
|
|
private readonly double _maximumAngularSpeedRadiansPerSecond;
|
|
private readonly double _positionJumpMarginMeters;
|
|
private readonly double _headingJumpMarginRadians;
|
|
private readonly double _velocityPositionResidualMeters;
|
|
private readonly double _velocityHeadingResidualRadians;
|
|
private readonly double _stationaryConfirmationSeconds;
|
|
|
|
private bool _hasAcceptedPose;
|
|
private Pose2D _acceptedPoseInWorld;
|
|
private double _acceptedTimestampSeconds;
|
|
private VehicleState _latestState;
|
|
private bool _stationaryHoldActive;
|
|
|
|
/// <summary>
|
|
/// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。
|
|
/// </summary>
|
|
public DetourVehicleStateProvider()
|
|
: this(
|
|
new VelocityEstimator2D(),
|
|
DefaultMaximumLinearSpeedMetersPerSecond,
|
|
DefaultMaximumAngularSpeedRadiansPerSecond,
|
|
DefaultPositionJumpMarginMeters,
|
|
DefaultHeadingJumpMarginRadians,
|
|
DefaultVelocityPositionResidualMeters,
|
|
DefaultVelocityHeadingResidualRadians,
|
|
DefaultStationaryConfirmationSeconds)
|
|
{
|
|
}
|
|
|
|
/// <summary>
|
|
/// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。
|
|
/// </summary>
|
|
public DetourVehicleStateProvider(
|
|
VelocityEstimator2D velocityEstimator,
|
|
double maximumLinearSpeedMetersPerSecond,
|
|
double maximumAngularSpeedRadiansPerSecond,
|
|
double positionJumpMarginMeters,
|
|
double headingJumpMarginRadians,
|
|
double velocityPositionResidualMeters,
|
|
double velocityHeadingResidualRadians,
|
|
double stationaryConfirmationSeconds)
|
|
{
|
|
_velocityEstimator = velocityEstimator ??
|
|
throw new ArgumentNullException(
|
|
nameof(velocityEstimator));
|
|
|
|
NumericGuard.EnsureFinitePositive(
|
|
maximumLinearSpeedMetersPerSecond,
|
|
nameof(maximumLinearSpeedMetersPerSecond));
|
|
NumericGuard.EnsureFinitePositive(
|
|
maximumAngularSpeedRadiansPerSecond,
|
|
nameof(maximumAngularSpeedRadiansPerSecond));
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
positionJumpMarginMeters,
|
|
nameof(positionJumpMarginMeters));
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
headingJumpMarginRadians,
|
|
nameof(headingJumpMarginRadians));
|
|
NumericGuard.EnsureFinitePositive(
|
|
velocityPositionResidualMeters,
|
|
nameof(velocityPositionResidualMeters));
|
|
NumericGuard.EnsureFinitePositive(
|
|
velocityHeadingResidualRadians,
|
|
nameof(velocityHeadingResidualRadians));
|
|
NumericGuard.EnsureFinitePositive(
|
|
stationaryConfirmationSeconds,
|
|
nameof(stationaryConfirmationSeconds));
|
|
|
|
_maximumLinearSpeedMetersPerSecond =
|
|
maximumLinearSpeedMetersPerSecond;
|
|
_maximumAngularSpeedRadiansPerSecond =
|
|
maximumAngularSpeedRadiansPerSecond;
|
|
_positionJumpMarginMeters =
|
|
positionJumpMarginMeters;
|
|
_headingJumpMarginRadians =
|
|
headingJumpMarginRadians;
|
|
_velocityPositionResidualMeters =
|
|
velocityPositionResidualMeters;
|
|
_velocityHeadingResidualRadians =
|
|
velocityHeadingResidualRadians;
|
|
_stationaryConfirmationSeconds =
|
|
stationaryConfirmationSeconds;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。
|
|
/// </summary>
|
|
public string LastFailureReason { get; private set; } = "";
|
|
|
|
/// <summary>
|
|
/// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。
|
|
/// </summary>
|
|
public bool TryGetState(out VehicleState state)
|
|
{
|
|
lock (_syncRoot)
|
|
{
|
|
try
|
|
{
|
|
var poseInWorld =
|
|
ReadDetourPoseInWorld();
|
|
var timestampSeconds =
|
|
_clock.Elapsed.TotalSeconds;
|
|
|
|
if (!_hasAcceptedPose)
|
|
{
|
|
state = AcceptPoseAfterReset(
|
|
poseInWorld,
|
|
timestampSeconds);
|
|
LastFailureReason = "";
|
|
return true;
|
|
}
|
|
|
|
if (ArePosesEquivalent(
|
|
poseInWorld,
|
|
_acceptedPoseInWorld))
|
|
{
|
|
state = HandleRepeatedPose(
|
|
timestampSeconds);
|
|
LastFailureReason = "";
|
|
return true;
|
|
}
|
|
|
|
// 静止保持后出现新定位时重新建立差分基准，
|
|
// 避免用很长的静止时间稀释第一次运动速度。
|
|
if (_stationaryHoldActive)
|
|
{
|
|
state = AcceptPoseAfterReset(
|
|
poseInWorld,
|
|
timestampSeconds);
|
|
LastFailureReason = "";
|
|
return true;
|
|
}
|
|
|
|
var elapsedSeconds =
|
|
timestampSeconds -
|
|
_acceptedTimestampSeconds;
|
|
|
|
if (!IsMotionPlausible(
|
|
_acceptedPoseInWorld,
|
|
poseInWorld,
|
|
elapsedSeconds))
|
|
{
|
|
// 单帧异常不进入差分器，也不中断调用方；下一次
|
|
// 正常观测仍相对最近有效位姿和真实时间差计算。
|
|
state = _latestState;
|
|
LastFailureReason =
|
|
"Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。";
|
|
return true;
|
|
}
|
|
|
|
if (IsVelocityInnovationAbnormal(
|
|
poseInWorld,
|
|
elapsedSeconds))
|
|
{
|
|
state = AcceptPoseAfterReset(
|
|
poseInWorld,
|
|
timestampSeconds);
|
|
LastFailureReason =
|
|
"Detour位姿偏离速度预测，已重新建立速度估计基准。";
|
|
return true;
|
|
}
|
|
|
|
state = AcceptContinuousPose(
|
|
poseInWorld,
|
|
timestampSeconds);
|
|
LastFailureReason = "";
|
|
return true;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
state = default;
|
|
LastFailureReason =
|
|
"Detour车辆状态读取失败：" +
|
|
exception.Message;
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 清除Detour位姿历史和速度估计状态。
|
|
/// </summary>
|
|
public void Reset()
|
|
{
|
|
lock (_syncRoot)
|
|
{
|
|
_velocityEstimator.Reset();
|
|
_hasAcceptedPose = false;
|
|
_acceptedPoseInWorld = Pose2D.Identity;
|
|
_acceptedTimestampSeconds = 0.0;
|
|
_latestState = default;
|
|
_stationaryHoldActive = false;
|
|
LastFailureReason = "";
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。
|
|
/// </summary>
|
|
private static Pose2D ReadDetourPoseInWorld()
|
|
{
|
|
var location =
|
|
DetourInterface.getCartLocation();
|
|
|
|
NumericGuard.EnsureFinite(location.x, "DetourX");
|
|
NumericGuard.EnsureFinite(location.y, "DetourY");
|
|
NumericGuard.EnsureFinite(location.th, "DetourTheta");
|
|
|
|
return new Pose2D(
|
|
location.x / MillimetersPerMeter,
|
|
location.y / MillimetersPerMeter,
|
|
AngleMath.NormalizeRadians(
|
|
AngleMath.DegreesToRadians(
|
|
location.th)));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 接受连续有效定位并更新速度估计和差分基准。
|
|
/// </summary>
|
|
private VehicleState AcceptContinuousPose(
|
|
Pose2D poseInWorld,
|
|
double timestampSeconds)
|
|
{
|
|
_latestState =
|
|
_velocityEstimator.Update(
|
|
poseInWorld,
|
|
timestampSeconds);
|
|
_acceptedPoseInWorld = poseInWorld;
|
|
_acceptedTimestampSeconds =
|
|
timestampSeconds;
|
|
_stationaryHoldActive = false;
|
|
return _latestState;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
|
|
/// </summary>
|
|
private VehicleState AcceptPoseAfterReset(
|
|
Pose2D poseInWorld,
|
|
double timestampSeconds)
|
|
{
|
|
_latestState =
|
|
_velocityEstimator.Reset(
|
|
poseInWorld,
|
|
timestampSeconds);
|
|
_acceptedPoseInWorld = poseInWorld;
|
|
_acceptedTimestampSeconds =
|
|
timestampSeconds;
|
|
_hasAcceptedPose = true;
|
|
_stationaryHoldActive = false;
|
|
return _latestState;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。
|
|
/// </summary>
|
|
private VehicleState HandleRepeatedPose(
|
|
double timestampSeconds)
|
|
{
|
|
var unchangedSeconds =
|
|
timestampSeconds -
|
|
_acceptedTimestampSeconds;
|
|
|
|
if (!_stationaryHoldActive &&
|
|
unchangedSeconds >=
|
|
_stationaryConfirmationSeconds)
|
|
{
|
|
_latestState =
|
|
new VehicleState(
|
|
timestampSeconds,
|
|
_acceptedPoseInWorld,
|
|
Twist2D.Zero,
|
|
true);
|
|
_stationaryHoldActive = true;
|
|
}
|
|
|
|
return _latestState;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。
|
|
/// </summary>
|
|
private bool IsMotionPlausible(
|
|
Pose2D startPoseInWorld,
|
|
Pose2D endPoseInWorld,
|
|
double deltaTimeSeconds)
|
|
{
|
|
if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
|
|
deltaTimeSeconds <= 0.0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
var deltaX =
|
|
endPoseInWorld.XMeters -
|
|
startPoseInWorld.XMeters;
|
|
var deltaY =
|
|
endPoseInWorld.YMeters -
|
|
startPoseInWorld.YMeters;
|
|
var displacementMeters =
|
|
Math.Sqrt(
|
|
deltaX * deltaX +
|
|
deltaY * deltaY);
|
|
var headingChangeRadians =
|
|
Math.Abs(
|
|
AngleMath.ShortestDifferenceRadians(
|
|
endPoseInWorld.YawRadians,
|
|
startPoseInWorld.YawRadians));
|
|
|
|
var maximumDisplacementMeters =
|
|
_maximumLinearSpeedMetersPerSecond *
|
|
deltaTimeSeconds +
|
|
_positionJumpMarginMeters;
|
|
var maximumHeadingChangeRadians =
|
|
_maximumAngularSpeedRadiansPerSecond *
|
|
deltaTimeSeconds +
|
|
_headingJumpMarginRadians;
|
|
|
|
return displacementMeters <=
|
|
maximumDisplacementMeters &&
|
|
headingChangeRadians <=
|
|
maximumHeadingChangeRadians;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。
|
|
/// </summary>
|
|
private bool IsVelocityInnovationAbnormal(
|
|
Pose2D poseInWorld,
|
|
double deltaTimeSeconds)
|
|
{
|
|
if (!_latestState.HasValidVelocityEstimate)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
var predictedX =
|
|
_acceptedPoseInWorld.XMeters +
|
|
_latestState.TwistInWorld
|
|
.VxMetersPerSecond *
|
|
deltaTimeSeconds;
|
|
var predictedY =
|
|
_acceptedPoseInWorld.YMeters +
|
|
_latestState.TwistInWorld
|
|
.VyMetersPerSecond *
|
|
deltaTimeSeconds;
|
|
var predictedYaw =
|
|
AngleMath.NormalizeRadians(
|
|
_acceptedPoseInWorld.YawRadians +
|
|
_latestState.TwistInWorld
|
|
.OmegaRadiansPerSecond *
|
|
deltaTimeSeconds);
|
|
|
|
var positionResidualX =
|
|
poseInWorld.XMeters - predictedX;
|
|
var positionResidualY =
|
|
poseInWorld.YMeters - predictedY;
|
|
var positionResidualMeters =
|
|
Math.Sqrt(
|
|
positionResidualX * positionResidualX +
|
|
positionResidualY * positionResidualY);
|
|
var headingResidualRadians =
|
|
Math.Abs(
|
|
AngleMath.ShortestDifferenceRadians(
|
|
poseInWorld.YawRadians,
|
|
predictedYaw));
|
|
|
|
return positionResidualMeters >
|
|
_velocityPositionResidualMeters ||
|
|
headingResidualRadians >
|
|
_velocityHeadingResidualRadians;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断两次读取是否为Detour保持输出的同一数值帧。
|
|
/// </summary>
|
|
private static bool ArePosesEquivalent(
|
|
Pose2D firstPose,
|
|
Pose2D secondPose)
|
|
{
|
|
return Math.Abs(
|
|
firstPose.XMeters -
|
|
secondPose.XMeters) <=
|
|
PositionEqualityToleranceMeters &&
|
|
Math.Abs(
|
|
firstPose.YMeters -
|
|
secondPose.YMeters) <=
|
|
PositionEqualityToleranceMeters &&
|
|
Math.Abs(
|
|
AngleMath.ShortestDifferenceRadians(
|
|
firstPose.YawRadians,
|
|
secondPose.YawRadians)) <=
|
|
HeadingEqualityToleranceRadians;
|
|
}
|
|
|
|
}
|
|
}
|