拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,509 @@
|
||||
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));
|
||||
|
||||
EnsureFinitePositive(
|
||||
maximumLinearSpeedMetersPerSecond,
|
||||
nameof(maximumLinearSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
maximumAngularSpeedRadiansPerSecond,
|
||||
nameof(maximumAngularSpeedRadiansPerSecond));
|
||||
EnsureFiniteNonNegative(
|
||||
positionJumpMarginMeters,
|
||||
nameof(positionJumpMarginMeters));
|
||||
EnsureFiniteNonNegative(
|
||||
headingJumpMarginRadians,
|
||||
nameof(headingJumpMarginRadians));
|
||||
EnsureFinitePositive(
|
||||
velocityPositionResidualMeters,
|
||||
nameof(velocityPositionResidualMeters));
|
||||
EnsureFinitePositive(
|
||||
velocityHeadingResidualRadians,
|
||||
nameof(velocityHeadingResidualRadians));
|
||||
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 = AcceptPoseAfterVelocityRebase(
|
||||
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();
|
||||
|
||||
EnsureFinite(location.x, "DetourX");
|
||||
EnsureFinite(location.y, "DetourY");
|
||||
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 AcceptPoseAfterVelocityRebase(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator
|
||||
.RebasePreservingVelocity(
|
||||
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 (!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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数和Detour位姿必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于状态估计。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。
|
||||
/// </summary>
|
||||
public sealed class FirstOrderLowPassFilter
|
||||
{
|
||||
private readonly double _timeConstantSeconds;
|
||||
private bool _isInitialized;
|
||||
private double _value;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定时间常数的一阶低通滤波器。
|
||||
/// </summary>
|
||||
public FirstOrderLowPassFilter(
|
||||
double timeConstantSeconds)
|
||||
{
|
||||
EnsureFinitePositive(
|
||||
timeConstantSeconds,
|
||||
nameof(timeConstantSeconds));
|
||||
|
||||
_timeConstantSeconds =
|
||||
timeConstantSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
|
||||
/// </summary>
|
||||
public double TimeConstantSeconds =>
|
||||
_timeConstantSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// 获取滤波器是否已经接收过有效初值。
|
||||
/// </summary>
|
||||
public bool IsInitialized =>
|
||||
_isInitialized;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前滤波输出；尚未初始化时读取会抛出异常。
|
||||
/// </summary>
|
||||
public double Value
|
||||
{
|
||||
get
|
||||
{
|
||||
if (!_isInitialized)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"一阶低通滤波器尚未初始化。");
|
||||
}
|
||||
|
||||
return _value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前输入和真实采样间隔更新滤波结果。
|
||||
/// </summary>
|
||||
public double Update(
|
||||
double input,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
EnsureFinite(
|
||||
input,
|
||||
nameof(input));
|
||||
EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
if (!_isInitialized)
|
||||
{
|
||||
_value = input;
|
||||
_isInitialized = true;
|
||||
return _value;
|
||||
}
|
||||
|
||||
var alpha =
|
||||
deltaTimeSeconds /
|
||||
(_timeConstantSeconds +
|
||||
deltaTimeSeconds);
|
||||
|
||||
_value += alpha * (input - _value);
|
||||
return _value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除历史输出，使下一次有效输入直接成为新的初值。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_value = 0.0;
|
||||
_isInitialized = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将滤波器立即重置到指定的有限初值。
|
||||
/// </summary>
|
||||
public void Reset(double initialValue)
|
||||
{
|
||||
EnsureFinite(
|
||||
initialValue,
|
||||
nameof(initialValue));
|
||||
|
||||
_value = initialValue;
|
||||
_isInitialized = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"滤波时间常数和采样间隔必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"滤波输入必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。
|
||||
/// </summary>
|
||||
public interface IVehicleStateProvider
|
||||
{
|
||||
/// <summary>
|
||||
/// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。
|
||||
/// </summary>
|
||||
bool TryGetState(out VehicleState state);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,138 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
|
||||
/// </summary>
|
||||
public readonly struct VehicleState
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
|
||||
/// </summary>
|
||||
public VehicleState(
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D poseInWorld,
|
||||
Twist2D twistInWorld,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteTwist(
|
||||
twistInWorld,
|
||||
nameof(twistInWorld));
|
||||
|
||||
SampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
|
||||
// 第一帧或定位重置后的速度不可用于闭环控制，
|
||||
// 此时显式置零，避免调用方误用残留速度。
|
||||
TwistInWorld = hasValidVelocityEstimate
|
||||
? twistInWorld
|
||||
: Twist2D.Zero;
|
||||
|
||||
var worldPoseInBody =
|
||||
FrameTransform2D.Inverse(
|
||||
PoseInWorld);
|
||||
TwistInBody =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
worldPoseInBody,
|
||||
TwistInWorld);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态源单调时钟中的采样时刻，单位为s。
|
||||
/// </summary>
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
|
||||
/// </summary>
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
|
||||
/// </summary>
|
||||
public Twist2D TwistInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
|
||||
/// </summary>
|
||||
public Twist2D TwistInBody { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
|
||||
/// </summary>
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查位姿是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(pose.XMeters) ||
|
||||
!IsFinite(pose.YMeters) ||
|
||||
!IsFinite(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"车辆位姿必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查速度是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteTwist(
|
||||
Twist2D twist,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(twist.VxMetersPerSecond) ||
|
||||
!IsFinite(twist.VyMetersPerSecond) ||
|
||||
!IsFinite(twist.OmegaRadiansPerSecond))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"车辆速度必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"采样时刻必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于车辆状态计算。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,275 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
|
||||
/// </summary>
|
||||
public sealed class VelocityEstimator2D
|
||||
{
|
||||
public const double DefaultLinearFilterTimeConstantSeconds =
|
||||
0.15;
|
||||
public const double DefaultAngularFilterTimeConstantSeconds =
|
||||
0.20;
|
||||
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_worldVelocityXFilter;
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_worldVelocityYFilter;
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_angularVelocityFilter;
|
||||
|
||||
private bool _hasPreviousSample;
|
||||
private Pose2D _previousPoseInWorld;
|
||||
private double _previousTimestampSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
|
||||
/// </summary>
|
||||
public VelocityEstimator2D()
|
||||
: this(
|
||||
DefaultLinearFilterTimeConstantSeconds,
|
||||
DefaultAngularFilterTimeConstantSeconds)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定线速度和角速度滤波时间常数的估计器。
|
||||
/// </summary>
|
||||
public VelocityEstimator2D(
|
||||
double linearFilterTimeConstantSeconds,
|
||||
double angularFilterTimeConstantSeconds)
|
||||
{
|
||||
_worldVelocityXFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
linearFilterTimeConstantSeconds);
|
||||
_worldVelocityYFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
linearFilterTimeConstantSeconds);
|
||||
_angularVelocityFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
angularFilterTimeConstantSeconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否已经保存了可用于下一次差分的位姿基准。
|
||||
/// </summary>
|
||||
public bool HasPreviousSample =>
|
||||
_hasPreviousSample;
|
||||
|
||||
/// <summary>
|
||||
/// 使用一个新的有效定位样本更新并返回车辆状态。
|
||||
/// </summary>
|
||||
public VehicleState Update(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
var normalizedPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
|
||||
if (!_hasPreviousSample)
|
||||
{
|
||||
return Reset(
|
||||
normalizedPoseInWorld,
|
||||
sampleTimestampSeconds);
|
||||
}
|
||||
|
||||
var deltaTimeSeconds =
|
||||
sampleTimestampSeconds -
|
||||
_previousTimestampSeconds;
|
||||
|
||||
if (deltaTimeSeconds <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(sampleTimestampSeconds),
|
||||
"新定位样本的单调时间戳必须严格大于上一帧。");
|
||||
}
|
||||
|
||||
var rawVelocityXInWorld =
|
||||
(normalizedPoseInWorld.XMeters -
|
||||
_previousPoseInWorld.XMeters) /
|
||||
deltaTimeSeconds;
|
||||
var rawVelocityYInWorld =
|
||||
(normalizedPoseInWorld.YMeters -
|
||||
_previousPoseInWorld.YMeters) /
|
||||
deltaTimeSeconds;
|
||||
var rawAngularVelocity =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
normalizedPoseInWorld.YawRadians,
|
||||
_previousPoseInWorld.YawRadians) /
|
||||
deltaTimeSeconds;
|
||||
|
||||
var filteredTwistInWorld =
|
||||
new Twist2D(
|
||||
_worldVelocityXFilter.Update(
|
||||
rawVelocityXInWorld,
|
||||
deltaTimeSeconds),
|
||||
_worldVelocityYFilter.Update(
|
||||
rawVelocityYInWorld,
|
||||
deltaTimeSeconds),
|
||||
_angularVelocityFilter.Update(
|
||||
rawAngularVelocity,
|
||||
deltaTimeSeconds));
|
||||
|
||||
_previousPoseInWorld =
|
||||
normalizedPoseInWorld;
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
normalizedPoseInWorld,
|
||||
filteredTwistInWorld,
|
||||
true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
|
||||
/// </summary>
|
||||
public VehicleState RebasePreservingVelocity(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
var normalizedPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
|
||||
_previousPoseInWorld =
|
||||
normalizedPoseInWorld;
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_hasPreviousSample = true;
|
||||
|
||||
var hasValidVelocityEstimate =
|
||||
_worldVelocityXFilter.IsInitialized &&
|
||||
_worldVelocityYFilter.IsInitialized &&
|
||||
_angularVelocityFilter.IsInitialized;
|
||||
|
||||
var retainedTwistInWorld =
|
||||
hasValidVelocityEstimate
|
||||
? new Twist2D(
|
||||
_worldVelocityXFilter.Value,
|
||||
_worldVelocityYFilter.Value,
|
||||
_angularVelocityFilter.Value)
|
||||
: Twist2D.Zero;
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
normalizedPoseInWorld,
|
||||
retainedTwistInWorld,
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
|
||||
/// </summary>
|
||||
public VehicleState Reset(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
_previousPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_hasPreviousSample = true;
|
||||
|
||||
_worldVelocityXFilter.Reset();
|
||||
_worldVelocityYFilter.Reset();
|
||||
_angularVelocityFilter.Reset();
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
_previousPoseInWorld,
|
||||
Twist2D.Zero,
|
||||
false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_hasPreviousSample = false;
|
||||
_previousPoseInWorld = Pose2D.Identity;
|
||||
_previousTimestampSeconds = 0.0;
|
||||
|
||||
_worldVelocityXFilter.Reset();
|
||||
_worldVelocityYFilter.Reset();
|
||||
_angularVelocityFilter.Reset();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查位姿是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(pose.XMeters) ||
|
||||
!IsFinite(pose.YMeters) ||
|
||||
!IsFinite(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"速度估计使用的车辆位姿必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"速度估计使用的采样时刻必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于速度估计。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user