288 lines
11 KiB
C#
288 lines
11 KiB
C#
using System;
|
|
using CommonUsage.Chassis;
|
|
using MyParking.Shared;
|
|
using System.Diagnostics;
|
|
|
|
namespace MultiWheelC.StateEstimation
|
|
{
|
|
/// <summary>
|
|
/// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
|
|
/// </summary>
|
|
public sealed class WheelFeedbackVehicleStateProvider
|
|
: IVehicleStateProvider
|
|
{
|
|
private readonly Stopwatch _wheelSpeedClock = Stopwatch.StartNew();
|
|
public const double DefaultVelocityFilterTimeConstantSeconds =
|
|
0.10;
|
|
|
|
private readonly object _syncRoot = new object();
|
|
private readonly IVehicleStateProvider _poseProvider;
|
|
private readonly MultiWheelChassis _chassis;
|
|
private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
|
|
private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
|
|
|
|
private bool _hasPreviousTimestamp;
|
|
private double _previousTimestampSeconds;
|
|
private bool _hasVelocityDiagnostics;
|
|
private double _latestDetourBodyVxMetersPerSecond;
|
|
private bool _latestDetourVelocityValid;
|
|
private double _latestRawWheelBodyVxMetersPerSecond;
|
|
private double _latestFilteredWheelBodyVxMetersPerSecond;
|
|
private double _latestRawWheelBodyVyMetersPerSecond;
|
|
private double _latestFilteredWheelBodyVyMetersPerSecond;
|
|
private bool _latestWheelVelocityValid;
|
|
|
|
/// <summary>
|
|
/// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
|
|
/// </summary>
|
|
public WheelFeedbackVehicleStateProvider(
|
|
IVehicleStateProvider poseProvider,
|
|
MultiWheelChassis chassis)
|
|
: this(
|
|
poseProvider,
|
|
chassis,
|
|
DefaultVelocityFilterTimeConstantSeconds)
|
|
{
|
|
}
|
|
|
|
/// <summary>
|
|
/// 创建使用指定低通时间常数的电机反馈平面速度状态源。
|
|
/// </summary>
|
|
public WheelFeedbackVehicleStateProvider(
|
|
IVehicleStateProvider poseProvider,
|
|
MultiWheelChassis chassis,
|
|
double velocityFilterTimeConstantSeconds)
|
|
{
|
|
_poseProvider = poseProvider ??
|
|
throw new ArgumentNullException(
|
|
nameof(poseProvider));
|
|
_chassis = chassis ??
|
|
throw new ArgumentNullException(
|
|
nameof(chassis));
|
|
_longitudinalSpeedFilter =
|
|
new FirstOrderLowPassFilter(
|
|
velocityFilterTimeConstantSeconds);
|
|
_lateralSpeedFilter =
|
|
new FirstOrderLowPassFilter(
|
|
velocityFilterTimeConstantSeconds);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取最近一次读取失败的原因，正常时为空字符串。
|
|
/// </summary>
|
|
public string LastFailureReason { get; private set; } =
|
|
string.Empty;
|
|
|
|
/// <summary>
|
|
/// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
|
|
/// </summary>
|
|
public bool TryGetState(out VehicleState state)
|
|
{
|
|
lock (_syncRoot)
|
|
{
|
|
if (!_poseProvider.TryGetState(
|
|
out var poseState))
|
|
{
|
|
state = default;
|
|
LastFailureReason =
|
|
"基础位姿状态源暂时不可用。";
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
var actualCarSpeed =
|
|
_chassis.GetCarSpeed(true);
|
|
var rawBodyVxMetersPerSecond =
|
|
(double)actualCarSpeed.Vx;
|
|
var rawBodyVyMetersPerSecond =
|
|
(double)actualCarSpeed.Vy;
|
|
|
|
NumericGuard.EnsureFinite(
|
|
rawBodyVxMetersPerSecond,
|
|
"电机反馈车体纵向速度");
|
|
NumericGuard.EnsureFinite(
|
|
rawBodyVyMetersPerSecond,
|
|
"电机反馈车体横向速度");
|
|
|
|
var wheelSpeedTimestampSeconds =
|
|
_wheelSpeedClock.Elapsed.TotalSeconds;
|
|
|
|
UpdateBodyVelocityFilters(
|
|
rawBodyVxMetersPerSecond,
|
|
rawBodyVyMetersPerSecond,
|
|
wheelSpeedTimestampSeconds,
|
|
out var filteredBodyVxMetersPerSecond,
|
|
out var filteredBodyVyMetersPerSecond,
|
|
out var hasValidWheelSpeedEstimate);
|
|
|
|
_latestDetourBodyVxMetersPerSecond =
|
|
poseState.TwistInBody.VxMetersPerSecond;
|
|
_latestDetourVelocityValid =
|
|
poseState.HasValidVelocityEstimate;
|
|
_latestRawWheelBodyVxMetersPerSecond =
|
|
rawBodyVxMetersPerSecond;
|
|
_latestFilteredWheelBodyVxMetersPerSecond =
|
|
filteredBodyVxMetersPerSecond;
|
|
_latestRawWheelBodyVyMetersPerSecond =
|
|
rawBodyVyMetersPerSecond;
|
|
_latestFilteredWheelBodyVyMetersPerSecond =
|
|
filteredBodyVyMetersPerSecond;
|
|
_latestWheelVelocityValid =
|
|
hasValidWheelSpeedEstimate;
|
|
_hasVelocityDiagnostics = true;
|
|
|
|
// 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
|
|
// 避免轮速差和舵角误差放大Omega噪声。
|
|
var twistInBody = new Twist2D(
|
|
filteredBodyVxMetersPerSecond,
|
|
filteredBodyVyMetersPerSecond,
|
|
poseState.TwistInBody
|
|
.OmegaRadiansPerSecond);
|
|
|
|
var twistInWorld =
|
|
FrameTransform2D
|
|
.TransformTwistAtSamePoint(
|
|
poseState.PoseInWorld,
|
|
twistInBody);
|
|
|
|
state = new VehicleState(
|
|
poseState.SampleTimestampSeconds,
|
|
poseState.PoseInWorld,
|
|
twistInWorld,
|
|
hasValidWheelSpeedEstimate);
|
|
|
|
LastFailureReason = string.Empty;
|
|
return true;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
state = default;
|
|
LastFailureReason =
|
|
"舵轮电机反馈车体速度解算失败：" +
|
|
exception.Message;
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
|
|
/// </summary>
|
|
public bool TryGetLatestVelocityDiagnostics(
|
|
out double detourBodyVxMetersPerSecond,
|
|
out bool detourVelocityValid,
|
|
out double rawWheelBodyVxMetersPerSecond,
|
|
out double filteredWheelBodyVxMetersPerSecond,
|
|
out double rawWheelBodyVyMetersPerSecond,
|
|
out double filteredWheelBodyVyMetersPerSecond,
|
|
out bool wheelVelocityValid)
|
|
{
|
|
lock (_syncRoot)
|
|
{
|
|
detourBodyVxMetersPerSecond =
|
|
_latestDetourBodyVxMetersPerSecond;
|
|
detourVelocityValid =
|
|
_latestDetourVelocityValid;
|
|
rawWheelBodyVxMetersPerSecond =
|
|
_latestRawWheelBodyVxMetersPerSecond;
|
|
filteredWheelBodyVxMetersPerSecond =
|
|
_latestFilteredWheelBodyVxMetersPerSecond;
|
|
rawWheelBodyVyMetersPerSecond =
|
|
_latestRawWheelBodyVyMetersPerSecond;
|
|
filteredWheelBodyVyMetersPerSecond =
|
|
_latestFilteredWheelBodyVyMetersPerSecond;
|
|
wheelVelocityValid =
|
|
_latestWheelVelocityValid;
|
|
return _hasVelocityDiagnostics;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 清除电机反馈速度的时间基准和低通滤波历史。
|
|
/// </summary>
|
|
public void Reset()
|
|
{
|
|
lock (_syncRoot)
|
|
{
|
|
_longitudinalSpeedFilter.Reset();
|
|
_lateralSpeedFilter.Reset();
|
|
_wheelSpeedClock.Restart();
|
|
_hasPreviousTimestamp = false;
|
|
_previousTimestampSeconds = 0.0;
|
|
_hasVelocityDiagnostics = false;
|
|
_latestDetourBodyVxMetersPerSecond = 0.0;
|
|
_latestDetourVelocityValid = false;
|
|
_latestRawWheelBodyVxMetersPerSecond = 0.0;
|
|
_latestFilteredWheelBodyVxMetersPerSecond = 0.0;
|
|
_latestRawWheelBodyVyMetersPerSecond = 0.0;
|
|
_latestFilteredWheelBodyVyMetersPerSecond = 0.0;
|
|
_latestWheelVelocityValid = false;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
|
|
/// </summary>
|
|
private void UpdateBodyVelocityFilters(
|
|
double rawBodyVxMetersPerSecond,
|
|
double rawBodyVyMetersPerSecond,
|
|
double timestampSeconds,
|
|
out double filteredBodyVxMetersPerSecond,
|
|
out double filteredBodyVyMetersPerSecond,
|
|
out bool hasValidWheelSpeedEstimate)
|
|
{
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
timestampSeconds,
|
|
nameof(timestampSeconds));
|
|
|
|
if (!_hasPreviousTimestamp)
|
|
{
|
|
_longitudinalSpeedFilter.Reset(
|
|
rawBodyVxMetersPerSecond);
|
|
_lateralSpeedFilter.Reset(
|
|
rawBodyVyMetersPerSecond);
|
|
_previousTimestampSeconds = timestampSeconds;
|
|
_hasPreviousTimestamp = true;
|
|
hasValidWheelSpeedEstimate = false;
|
|
filteredBodyVxMetersPerSecond =
|
|
rawBodyVxMetersPerSecond;
|
|
filteredBodyVyMetersPerSecond =
|
|
rawBodyVyMetersPerSecond;
|
|
return;
|
|
}
|
|
|
|
var deltaTimeSeconds =
|
|
timestampSeconds -
|
|
_previousTimestampSeconds;
|
|
_previousTimestampSeconds = timestampSeconds;
|
|
|
|
if (deltaTimeSeconds <= 0.0)
|
|
{
|
|
_longitudinalSpeedFilter.Reset(
|
|
rawBodyVxMetersPerSecond);
|
|
_lateralSpeedFilter.Reset(
|
|
rawBodyVyMetersPerSecond);
|
|
hasValidWheelSpeedEstimate = false;
|
|
filteredBodyVxMetersPerSecond =
|
|
rawBodyVxMetersPerSecond;
|
|
filteredBodyVyMetersPerSecond =
|
|
rawBodyVyMetersPerSecond;
|
|
return;
|
|
}
|
|
|
|
hasValidWheelSpeedEstimate = true;
|
|
filteredBodyVxMetersPerSecond =
|
|
_longitudinalSpeedFilter.Update(
|
|
rawBodyVxMetersPerSecond,
|
|
deltaTimeSeconds);
|
|
filteredBodyVyMetersPerSecond =
|
|
_lateralSpeedFilter.Update(
|
|
rawBodyVyMetersPerSecond,
|
|
deltaTimeSeconds);
|
|
}
|
|
|
|
}
|
|
}
|