First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善Detour状态估计与轨迹跟踪验证

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
+332 -16
View File
@@ -6,7 +6,7 @@ using System.Diagnostics;
namespace MultiWheelC.StateEstimation
{
/// <summary>
/// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
/// 保留外部状态源的Detour位姿，以轮组反馈替换平面线速度，并向短时位姿预测提供角速度。
/// </summary>
public sealed class WheelFeedbackVehicleStateProvider
: IVehicleStateProvider
@@ -20,6 +20,7 @@ namespace MultiWheelC.StateEstimation
private readonly MultiWheelChassis _chassis;
private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
private readonly FirstOrderLowPassFilter _angularSpeedFilter;
private bool _hasPreviousTimestamp;
private double _previousTimestampSeconds;
@@ -30,7 +31,11 @@ namespace MultiWheelC.StateEstimation
private double _latestFilteredWheelBodyVxMetersPerSecond;
private double _latestRawWheelBodyVyMetersPerSecond;
private double _latestFilteredWheelBodyVyMetersPerSecond;
private double _latestRawWheelBodyOmegaRadiansPerSecond;
private double _latestFilteredWheelBodyOmegaRadiansPerSecond;
private double _latestWheelSampleTimestampSeconds;
private bool _latestWheelVelocityValid;
private bool _latestWheelFeedbackReadSucceeded;
/// <summary>
/// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
@@ -65,6 +70,9 @@ namespace MultiWheelC.StateEstimation
_lateralSpeedFilter =
new FirstOrderLowPassFilter(
velocityFilterTimeConstantSeconds);
_angularSpeedFilter =
new FirstOrderLowPassFilter(
velocityFilterTimeConstantSeconds);
}
/// <summary>
@@ -73,6 +81,12 @@ namespace MultiWheelC.StateEstimation
public string LastFailureReason { get; private set; } =
string.Empty;
/// <summary>
/// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
/// </summary>
public string LastHeadingFailureReason { get; private set; } =
string.Empty;
/// <summary>
/// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
/// </summary>
@@ -80,15 +94,6 @@ namespace MultiWheelC.StateEstimation
{
lock (_syncRoot)
{
if (!_poseProvider.TryGetState(
out var poseState))
{
state = default;
LastFailureReason =
"基础位姿状态源暂时不可用。";
return false;
}
try
{
var actualCarSpeed =
@@ -97,6 +102,11 @@ namespace MultiWheelC.StateEstimation
(double)actualCarSpeed.Vx;
var rawBodyVyMetersPerSecond =
(double)actualCarSpeed.Vy;
// CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
// 状态估计内部统一转换为rad/s。
var rawBodyOmegaRadiansPerSecond =
AngleMath.DegreesToRadians(
actualCarSpeed.Vw);
NumericGuard.EnsureFinite(
rawBodyVxMetersPerSecond,
@@ -104,6 +114,9 @@ namespace MultiWheelC.StateEstimation
NumericGuard.EnsureFinite(
rawBodyVyMetersPerSecond,
"电机反馈车体横向速度");
NumericGuard.EnsureFinite(
rawBodyOmegaRadiansPerSecond,
"电机反馈车体角速度");
var wheelSpeedTimestampSeconds =
_wheelSpeedClock.Elapsed.TotalSeconds;
@@ -111,15 +124,13 @@ namespace MultiWheelC.StateEstimation
UpdateBodyVelocityFilters(
rawBodyVxMetersPerSecond,
rawBodyVyMetersPerSecond,
rawBodyOmegaRadiansPerSecond,
wheelSpeedTimestampSeconds,
out var filteredBodyVxMetersPerSecond,
out var filteredBodyVyMetersPerSecond,
out var filteredBodyOmegaRadiansPerSecond,
out var hasValidWheelSpeedEstimate);
_latestDetourBodyVxMetersPerSecond =
poseState.TwistInBody.VxMetersPerSecond;
_latestDetourVelocityValid =
poseState.HasValidVelocityEstimate;
_latestRawWheelBodyVxMetersPerSecond =
rawBodyVxMetersPerSecond;
_latestFilteredWheelBodyVxMetersPerSecond =
@@ -128,8 +139,40 @@ namespace MultiWheelC.StateEstimation
rawBodyVyMetersPerSecond;
_latestFilteredWheelBodyVyMetersPerSecond =
filteredBodyVyMetersPerSecond;
_latestRawWheelBodyOmegaRadiansPerSecond =
rawBodyOmegaRadiansPerSecond;
_latestFilteredWheelBodyOmegaRadiansPerSecond =
filteredBodyOmegaRadiansPerSecond;
_latestWheelSampleTimestampSeconds =
wheelSpeedTimestampSeconds;
_latestWheelVelocityValid =
hasValidWheelSpeedEstimate;
_latestWheelFeedbackReadSucceeded = true;
// Detour位姿跳变确认期间需要用轮速维持短时运动预测。
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider)
{
detourStateProvider.UpdateWheelVelocityEstimate(
filteredBodyVxMetersPerSecond,
filteredBodyVyMetersPerSecond,
filteredBodyOmegaRadiansPerSecond,
hasValidWheelSpeedEstimate);
}
if (!_poseProvider.TryGetState(
out var poseState))
{
state = default;
LastFailureReason =
GetPoseProviderFailureReason();
return false;
}
_latestDetourBodyVxMetersPerSecond =
poseState.TwistInBody.VxMetersPerSecond;
_latestDetourVelocityValid =
poseState.HasValidVelocityEstimate;
_hasVelocityDiagnostics = true;
// 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
@@ -157,6 +200,7 @@ namespace MultiWheelC.StateEstimation
}
catch (Exception exception)
{
_latestWheelFeedbackReadSucceeded = false;
state = default;
LastFailureReason =
"舵轮电机反馈车体速度解算失败：" +
@@ -166,6 +210,85 @@ namespace MultiWheelC.StateEstimation
}
}
/// <summary>
/// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
/// </summary>
public bool TryGetHeadingRadians(
out double headingRadians)
{
lock (_syncRoot)
{
TryGetState(out _);
if (!_latestWheelFeedbackReadSucceeded)
{
headingRadians = 0.0;
LastHeadingFailureReason =
string.IsNullOrWhiteSpace(
LastFailureReason)
? "舵轮反馈当前不可用，无法校验航向。"
: LastFailureReason;
return false;
}
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider)
{
var success = detourStateProvider
.TryGetLatestReliableHeadingRadians(
out headingRadians);
LastHeadingFailureReason = success
? string.Empty
: detourStateProvider
.LastHeadingFailureReason;
return success;
}
if (_poseProvider.TryGetState(
out var poseState))
{
headingRadians =
poseState.PoseInWorld.YawRadians;
LastHeadingFailureReason = string.Empty;
return true;
}
headingRadians = 0.0;
LastHeadingFailureReason =
GetPoseProviderFailureReason();
return false;
}
}
/// <summary>
/// 原地自转停车后，允许基础Detour状态源重新确认有限位置偏移。
/// </summary>
public void BeginPostRotationPositionRecovery()
{
lock (_syncRoot)
{
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider)
{
detourStateProvider
.BeginPostRotationPositionRecovery();
}
}
}
private string GetPoseProviderFailureReason()
{
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider &&
!string.IsNullOrWhiteSpace(
detourStateProvider.LastFailureReason))
{
return detourStateProvider.LastFailureReason;
}
return "基础位姿状态源暂时不可用。";
}
/// <summary>
/// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
/// </summary>
@@ -199,14 +322,188 @@ namespace MultiWheelC.StateEstimation
}
/// <summary>
/// 清除电机反馈速度的时间基准和低通滤波历史。
/// 读取最近一帧Detour纵向速度及轮组原始/滤波Vx、Vy、Vw和采样时间。
/// </summary>
public bool TryGetLatestVelocityDiagnostics(
out double detourBodyVxMetersPerSecond,
out bool detourVelocityValid,
out double rawWheelBodyVxMetersPerSecond,
out double filteredWheelBodyVxMetersPerSecond,
out double rawWheelBodyVyMetersPerSecond,
out double filteredWheelBodyVyMetersPerSecond,
out double rawWheelBodyOmegaRadiansPerSecond,
out double filteredWheelBodyOmegaRadiansPerSecond,
out double wheelSampleTimestampSeconds,
out bool wheelVelocityValid)
{
lock (_syncRoot)
{
detourBodyVxMetersPerSecond =
_latestDetourBodyVxMetersPerSecond;
detourVelocityValid =
_latestDetourVelocityValid;
rawWheelBodyVxMetersPerSecond =
_latestRawWheelBodyVxMetersPerSecond;
filteredWheelBodyVxMetersPerSecond =
_latestFilteredWheelBodyVxMetersPerSecond;
rawWheelBodyVyMetersPerSecond =
_latestRawWheelBodyVyMetersPerSecond;
filteredWheelBodyVyMetersPerSecond =
_latestFilteredWheelBodyVyMetersPerSecond;
rawWheelBodyOmegaRadiansPerSecond =
_latestRawWheelBodyOmegaRadiansPerSecond;
filteredWheelBodyOmegaRadiansPerSecond =
_latestFilteredWheelBodyOmegaRadiansPerSecond;
wheelSampleTimestampSeconds =
_latestWheelSampleTimestampSeconds;
wheelVelocityValid =
_latestWheelVelocityValid;
return _hasVelocityDiagnostics;
}
}
/// <summary>
/// 读取Detour跳变候选、自动坐标连续化和数据新鲜度诊断。
/// </summary>
public bool TryGetLatestDetourDiagnostics(
out bool jumpCandidateActive,
out int jumpCandidateConsistentFrameCount,
out double estimatedShiftDistanceMeters,
out double estimatedShiftHeadingRadians,
out int automaticFrameShiftCount,
out string stateStatusReason,
out double detourDataAgeMilliseconds)
{
lock (_syncRoot)
{
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider)
{
var hasDiagnostics = detourStateProvider
.TryGetLatestDiagnostics(
out jumpCandidateActive,
out jumpCandidateConsistentFrameCount,
out estimatedShiftDistanceMeters,
out estimatedShiftHeadingRadians,
out automaticFrameShiftCount,
out stateStatusReason,
out detourDataAgeMilliseconds);
if (string.IsNullOrWhiteSpace(
stateStatusReason) &&
!string.IsNullOrWhiteSpace(
LastFailureReason))
{
stateStatusReason = LastFailureReason;
}
return hasDiagnostics;
}
jumpCandidateActive = false;
jumpCandidateConsistentFrameCount = 0;
estimatedShiftDistanceMeters = 0.0;
estimatedShiftHeadingRadians = 0.0;
automaticFrameShiftCount = 0;
stateStatusReason = LastFailureReason;
detourDataAgeMilliseconds = 0.0;
return false;
}
}
/// <summary>
/// 读取Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
/// </summary>
public bool TryGetLatestDetourDiagnostics(
out bool jumpCandidateActive,
out int jumpCandidateConsistentFrameCount,
out double estimatedShiftDistanceMeters,
out double estimatedShiftHeadingRadians,
out int automaticFrameShiftCount,
out double sourceFrameIntervalSeconds,
out double motionPredictionTimestampSeconds,
out bool hasInnovationDiagnostics,
out double positionInnovationMeters,
out double allowedPositionInnovationMeters,
out double headingInnovationRadians,
out double allowedHeadingInnovationRadians,
out string jumpCandidateTriggerReason,
out string stateStatus,
out string stateStatusReason,
out double detourDataAgeMilliseconds)
{
lock (_syncRoot)
{
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider)
{
var hasDiagnostics = detourStateProvider
.TryGetLatestDiagnostics(
out jumpCandidateActive,
out jumpCandidateConsistentFrameCount,
out estimatedShiftDistanceMeters,
out estimatedShiftHeadingRadians,
out automaticFrameShiftCount,
out sourceFrameIntervalSeconds,
out motionPredictionTimestampSeconds,
out hasInnovationDiagnostics,
out positionInnovationMeters,
out allowedPositionInnovationMeters,
out headingInnovationRadians,
out allowedHeadingInnovationRadians,
out jumpCandidateTriggerReason,
out stateStatus,
out stateStatusReason,
out detourDataAgeMilliseconds);
if (string.IsNullOrWhiteSpace(
stateStatusReason) &&
!string.IsNullOrWhiteSpace(
LastFailureReason))
{
stateStatus = "Unavailable";
stateStatusReason = LastFailureReason;
}
return hasDiagnostics;
}
jumpCandidateActive = false;
jumpCandidateConsistentFrameCount = 0;
estimatedShiftDistanceMeters = 0.0;
estimatedShiftHeadingRadians = 0.0;
automaticFrameShiftCount = 0;
sourceFrameIntervalSeconds = 0.0;
motionPredictionTimestampSeconds = 0.0;
hasInnovationDiagnostics = false;
positionInnovationMeters = 0.0;
allowedPositionInnovationMeters = 0.0;
headingInnovationRadians = 0.0;
allowedHeadingInnovationRadians = 0.0;
jumpCandidateTriggerReason = string.Empty;
stateStatus = "Unavailable";
stateStatusReason = LastFailureReason;
detourDataAgeMilliseconds = 0.0;
return false;
}
}
/// <summary>
/// 清除基础位姿状态、坐标连续化状态以及电机反馈速度滤波历史。
/// </summary>
public void Reset()
{
lock (_syncRoot)
{
if (_poseProvider is DetourVehicleStateProvider
detourStateProvider)
{
detourStateProvider.Reset();
}
_longitudinalSpeedFilter.Reset();
_lateralSpeedFilter.Reset();
_angularSpeedFilter.Reset();
_wheelSpeedClock.Restart();
_hasPreviousTimestamp = false;
_previousTimestampSeconds = 0.0;
@@ -217,20 +514,27 @@ namespace MultiWheelC.StateEstimation
_latestFilteredWheelBodyVxMetersPerSecond = 0.0;
_latestRawWheelBodyVyMetersPerSecond = 0.0;
_latestFilteredWheelBodyVyMetersPerSecond = 0.0;
_latestRawWheelBodyOmegaRadiansPerSecond = 0.0;
_latestFilteredWheelBodyOmegaRadiansPerSecond = 0.0;
_latestWheelSampleTimestampSeconds = 0.0;
_latestWheelVelocityValid = false;
_latestWheelFeedbackReadSucceeded = false;
LastFailureReason = string.Empty;
LastHeadingFailureReason = string.Empty;
}
}
/// <summary>
/// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
/// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
/// </summary>
private void UpdateBodyVelocityFilters(
double rawBodyVxMetersPerSecond,
double rawBodyVyMetersPerSecond,
double rawBodyOmegaRadiansPerSecond,
double timestampSeconds,
out double filteredBodyVxMetersPerSecond,
out double filteredBodyVyMetersPerSecond,
out double filteredBodyOmegaRadiansPerSecond,
out bool hasValidWheelSpeedEstimate)
{
NumericGuard.EnsureFiniteNonNegative(
@@ -243,6 +547,8 @@ namespace MultiWheelC.StateEstimation
rawBodyVxMetersPerSecond);
_lateralSpeedFilter.Reset(
rawBodyVyMetersPerSecond);
_angularSpeedFilter.Reset(
rawBodyOmegaRadiansPerSecond);
_previousTimestampSeconds = timestampSeconds;
_hasPreviousTimestamp = true;
hasValidWheelSpeedEstimate = false;
@@ -250,6 +556,8 @@ namespace MultiWheelC.StateEstimation
rawBodyVxMetersPerSecond;
filteredBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
filteredBodyOmegaRadiansPerSecond =
rawBodyOmegaRadiansPerSecond;
return;
}
@@ -264,11 +572,15 @@ namespace MultiWheelC.StateEstimation
rawBodyVxMetersPerSecond);
_lateralSpeedFilter.Reset(
rawBodyVyMetersPerSecond);
_angularSpeedFilter.Reset(
rawBodyOmegaRadiansPerSecond);
hasValidWheelSpeedEstimate = false;
filteredBodyVxMetersPerSecond =
rawBodyVxMetersPerSecond;
filteredBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
filteredBodyOmegaRadiansPerSecond =
rawBodyOmegaRadiansPerSecond;
return;
}
@@ -281,6 +593,10 @@ namespace MultiWheelC.StateEstimation
_lateralSpeedFilter.Update(
rawBodyVyMetersPerSecond,
deltaTimeSeconds);
filteredBodyOmegaRadiansPerSecond =
_angularSpeedFilter.Update(
rawBodyOmegaRadiansPerSecond,
deltaTimeSeconds);
}
}