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

完善状态估计、车队协调与舵轮辨识日志

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-24 18:39:41 +08:00
parent 0ab409cd2a
commit 95c0b19a26
40 changed files with 5983 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
+380 -5
View File
@@ -6,6 +6,16 @@ using MyParking.Shared;
namespace MultiWheelC.StateEstimation
{
/// <summary>表示Detour位姿相对已标注地图关键帧的当前可信级别。</summary>
public enum DetourLocalizationQuality
{
Unknown = 0,
Normal = 1,
Degraded = 2,
Poor = 3,
Lost = 4
}
/// <summary>
/// 读取Detour位姿，并将可能发生重定位的原始世界坐标转换为当前任务使用的连续坐标。
/// </summary>
@@ -55,6 +65,10 @@ namespace MultiWheelC.StateEstimation
public const double
DefaultMaximumAutomaticHeadingShiftRadians =
5.0 * Math.PI / 180.0;
public const double DefaultMaximumCachedFrameAgeSeconds =
0.50;
public const int
DefaultLocalizationQualityConfirmationFrameCount = 3;
private const double MillimetersPerMeter = 1000.0;
private const double PositionEqualityToleranceMeters = 1e-9;
@@ -64,6 +78,9 @@ namespace MultiWheelC.StateEstimation
private const double
InPlaceRotationAngularSpeedThresholdRadiansPerSecond =
Math.PI / 180.0;
private const double NormalLocalizationStepUpperBound = 5.0;
private const double DegradedLocalizationStepUpperBound = 10.0;
private const double LostLocalizationStepLowerBound = 99.0;
private readonly object _syncRoot = new object();
private readonly Stopwatch _clock = Stopwatch.StartNew();
@@ -83,6 +100,9 @@ namespace MultiWheelC.StateEstimation
private readonly double _maximumAutomaticFrameShiftMeters;
private readonly double
_maximumAutomaticHeadingShiftRadians;
private readonly double _maximumCachedFrameAgeSeconds;
private readonly int
_localizationQualityConfirmationFrameCount;
private Pose2D _acceptedPoseInControl;
private double _acceptedTimestampSeconds;
@@ -94,8 +114,15 @@ namespace MultiWheelC.StateEstimation
private long _lastObservedTickRaw;
private bool _hasDetourSourceFrameInterval;
private double _lastDetourSourceFrameIntervalSeconds;
private double _lastNewDetourFrameTimestampSeconds;
private Pose2D _controlFromDetour = Pose2D.Identity;
private int _lostLocalizationStepConsecutiveFrameCount;
private int _normalLocalizationStepRecoveryFrameCount;
private bool _localizationStepUnavailable;
private bool _localizationStepPredictionActive;
private bool _cachedFramePredictionActive;
private bool _hasMotionPrediction;
private Pose2D _predictedPoseInControl;
private double _predictionTimestampSeconds;
@@ -144,7 +171,9 @@ namespace MultiWheelC.StateEstimation
DefaultJumpConfirmationFrameCount,
DefaultJumpConfirmationTimeoutSeconds,
DefaultMaximumAutomaticFrameShiftMeters,
DefaultMaximumAutomaticHeadingShiftRadians)
DefaultMaximumAutomaticHeadingShiftRadians,
DefaultMaximumCachedFrameAgeSeconds,
DefaultLocalizationQualityConfirmationFrameCount)
{
}
@@ -175,7 +204,9 @@ namespace MultiWheelC.StateEstimation
DefaultJumpConfirmationFrameCount,
DefaultJumpConfirmationTimeoutSeconds,
DefaultMaximumAutomaticFrameShiftMeters,
DefaultMaximumAutomaticHeadingShiftRadians)
DefaultMaximumAutomaticHeadingShiftRadians,
DefaultMaximumCachedFrameAgeSeconds,
DefaultLocalizationQualityConfirmationFrameCount)
{
}
@@ -197,6 +228,46 @@ namespace MultiWheelC.StateEstimation
double jumpConfirmationTimeoutSeconds,
double maximumAutomaticFrameShiftMeters,
double maximumAutomaticHeadingShiftRadians)
: this(
velocityEstimator,
maximumLinearSpeedMetersPerSecond,
maximumAngularSpeedRadiansPerSecond,
positionJumpMarginMeters,
headingJumpMarginRadians,
velocityPositionResidualMeters,
velocityHeadingResidualRadians,
stationaryConfirmationSeconds,
headingOutlierConfirmationFrameCount,
headingOutlierPredictionTimeoutSeconds,
jumpConfirmationFrameCount,
jumpConfirmationTimeoutSeconds,
maximumAutomaticFrameShiftMeters,
maximumAutomaticHeadingShiftRadians,
DefaultMaximumCachedFrameAgeSeconds,
DefaultLocalizationQualityConfirmationFrameCount)
{
}
/// <summary>
/// 创建同时配置缓存停更和定位质量确认参数的Detour状态源。
/// </summary>
public DetourVehicleStateProvider(
VelocityEstimator2D velocityEstimator,
double maximumLinearSpeedMetersPerSecond,
double maximumAngularSpeedRadiansPerSecond,
double positionJumpMarginMeters,
double headingJumpMarginRadians,
double velocityPositionResidualMeters,
double velocityHeadingResidualRadians,
double stationaryConfirmationSeconds,
int headingOutlierConfirmationFrameCount,
double headingOutlierPredictionTimeoutSeconds,
int jumpConfirmationFrameCount,
double jumpConfirmationTimeoutSeconds,
double maximumAutomaticFrameShiftMeters,
double maximumAutomaticHeadingShiftRadians,
double maximumCachedFrameAgeSeconds,
int localizationQualityConfirmationFrameCount)
{
_velocityEstimator = velocityEstimator ??
throw new ArgumentNullException(
@@ -235,6 +306,9 @@ namespace MultiWheelC.StateEstimation
NumericGuard.EnsureFinitePositive(
maximumAutomaticHeadingShiftRadians,
nameof(maximumAutomaticHeadingShiftRadians));
NumericGuard.EnsureFinitePositive(
maximumCachedFrameAgeSeconds,
nameof(maximumCachedFrameAgeSeconds));
if (jumpConfirmationFrameCount < 2)
{
@@ -250,6 +324,13 @@ namespace MultiWheelC.StateEstimation
"Detour航向异常至少需要两个新帧确认。");
}
if (localizationQualityConfirmationFrameCount < 2)
{
throw new ArgumentOutOfRangeException(
nameof(localizationQualityConfirmationFrameCount),
"Detour定位质量失效和恢复至少需要两个新帧确认。");
}
_maximumLinearSpeedMetersPerSecond =
maximumLinearSpeedMetersPerSecond;
_maximumAngularSpeedRadiansPerSecond =
@@ -274,6 +355,10 @@ namespace MultiWheelC.StateEstimation
maximumAutomaticFrameShiftMeters;
_maximumAutomaticHeadingShiftRadians =
maximumAutomaticHeadingShiftRadians;
_maximumCachedFrameAgeSeconds =
maximumCachedFrameAgeSeconds;
_localizationQualityConfirmationFrameCount =
localizationQualityConfirmationFrameCount;
}
/// <summary>
@@ -289,15 +374,22 @@ namespace MultiWheelC.StateEstimation
string.Empty;
/// <summary>
/// 获取最近一次读取到的Detour源时间戳原值。
/// 获取最近一次共享对象提交时的DateTime.Ticks原值。
/// </summary>
public long? LastDetourTickRaw { get; private set; }
/// <summary>
/// 获取最近一次Detour l_step原值；其精确定义仍由Detour接口文档确认。
/// 获取最近一次Detour l_step原值，即到已标注地图关键帧的图上步数。
/// </summary>
public double? LastDetourLocalizationStep { get; private set; }
/// <summary>获取最近一帧Detour定位质量的瞬时分级。</summary>
public DetourLocalizationQuality LocalizationQuality
{
get;
private set;
} = DetourLocalizationQuality.Unknown;
/// <summary>
/// 获取当前是否正在确认疑似Detour坐标跳变。
/// </summary>
@@ -435,10 +527,35 @@ namespace MultiWheelC.StateEstimation
LastDetourTickRaw = observation.TickRaw;
LastDetourLocalizationStep =
observation.LocalizationStep;
LocalizationQuality =
ClassifyLocalizationStep(
observation.LocalizationStep);
if (!_hasObservedDetourFrame)
{
SetLastObservedDetourFrame(observation);
_lastNewDetourFrameTimestampSeconds =
timestampSeconds;
if (LocalizationQuality ==
DetourLocalizationQuality.Lost)
{
_localizationStepUnavailable = true;
_lostLocalizationStepConsecutiveFrameCount =
_localizationQualityConfirmationFrameCount;
InvalidateHeadingObservation(
"Detour初始l_step达到丢失定位范围，航向暂不可用。");
state = default;
LastFailureReason =
"Detour初始l_step=" +
observation.LocalizationStep
.ToString(
"F0",
CultureInfo.InvariantCulture) +
"，定位状态不可用。";
return false;
}
AcceptReliableHeadingObservation(observation);
state = AcceptPoseAfterReset(
observation.PoseInDetour,
@@ -471,6 +588,9 @@ namespace MultiWheelC.StateEstimation
var previousPoseInDetour =
_lastObservedPoseInDetour;
SetLastObservedDetourFrame(observation);
_lastNewDetourFrameTimestampSeconds =
timestampSeconds;
_cachedFramePredictionActive = false;
if (!NumericGuard.IsFinite(
sourceDeltaTimeSeconds) ||
@@ -488,6 +608,14 @@ namespace MultiWheelC.StateEstimation
sourceDeltaTimeSeconds;
_hasDetourSourceFrameInterval = true;
if (HandleLocalizationStepQuality(
observation,
timestampSeconds,
out state))
{
return !_localizationStepUnavailable;
}
var poseInControl =
FrameTransform2D.TransformPose(
_controlFromDetour,
@@ -605,6 +733,18 @@ namespace MultiWheelC.StateEstimation
{
lock (_syncRoot)
{
if ((_cachedFramePredictionActive ||
_localizationStepPredictionActive) &&
_hasReliableHeadingObservation &&
_latestWheelVelocityValid &&
_hasMotionPrediction)
{
headingRadians =
GetPredictedPoseInControl().YawRadians;
LastHeadingFailureReason = string.Empty;
return true;
}
if (_headingOutlierCandidateActive)
{
var candidateAgeSeconds =
@@ -742,8 +882,15 @@ namespace MultiWheelC.StateEstimation
_lastObservedTickRaw = 0L;
_hasDetourSourceFrameInterval = false;
_lastDetourSourceFrameIntervalSeconds = 0.0;
_lastNewDetourFrameTimestampSeconds = 0.0;
_controlFromDetour = Pose2D.Identity;
_lostLocalizationStepConsecutiveFrameCount = 0;
_normalLocalizationStepRecoveryFrameCount = 0;
_localizationStepUnavailable = false;
_localizationStepPredictionActive = false;
_cachedFramePredictionActive = false;
_hasMotionPrediction = false;
_predictedPoseInControl = Pose2D.Identity;
_predictionTimestampSeconds = 0.0;
@@ -765,15 +912,158 @@ namespace MultiWheelC.StateEstimation
AutomaticFrameShiftCount = 0;
LastDetourTickRaw = null;
LastDetourLocalizationStep = null;
LocalizationQuality =
DetourLocalizationQuality.Unknown;
LastFailureReason = string.Empty;
LastHeadingFailureReason = string.Empty;
}
}
/// <summary>按l_step确认定位丢失，并要求连续正常新帧后才恢复。</summary>
private bool HandleLocalizationStepQuality(
DetourObservation observation,
double timestampSeconds,
out VehicleState state)
{
if (LocalizationQuality ==
DetourLocalizationQuality.Lost)
{
_normalLocalizationStepRecoveryFrameCount = 0;
_lostLocalizationStepConsecutiveFrameCount++;
if (_lostLocalizationStepConsecutiveFrameCount >=
_localizationQualityConfirmationFrameCount)
{
_localizationStepUnavailable = true;
_localizationStepPredictionActive = false;
InvalidateHeadingObservation(
"Detour l_step连续达到丢失定位范围，航向暂不可用。");
state = default;
LastFailureReason =
"Detour l_step=" +
observation.LocalizationStep.ToString(
"F0",
CultureInfo.InvariantCulture) +
"已连续" +
_lostLocalizationStepConsecutiveFrameCount +
"个新帧达到丢失定位范围，车辆状态暂不可用。";
return true;
}
if (!_hasMotionPrediction ||
!_latestWheelVelocityValid ||
!_hasReliableHeadingObservation)
{
_localizationStepUnavailable = true;
_localizationStepPredictionActive = false;
InvalidateHeadingObservation(
"Detour定位质量异常且缺少可靠轮组预测，航向暂不可用。");
state = default;
LastFailureReason =
"Detour定位质量异常且缺少可靠轮组预测，车辆状态暂不可用。";
return true;
}
_localizationStepPredictionActive = true;
state = CreatePredictedState(timestampSeconds);
LastFailureReason =
"Detour l_step达到丢失定位范围，正在等待连续新帧确认，" +
"当前使用轮组速度短时预测位姿。";
return true;
}
_lostLocalizationStepConsecutiveFrameCount = 0;
_localizationStepPredictionActive = false;
if (!_localizationStepUnavailable)
{
_normalLocalizationStepRecoveryFrameCount = 0;
state = default;
return false;
}
if (LocalizationQuality ==
DetourLocalizationQuality.Normal)
{
_normalLocalizationStepRecoveryFrameCount++;
}
else
{
_normalLocalizationStepRecoveryFrameCount = 0;
}
if (_normalLocalizationStepRecoveryFrameCount <
_localizationQualityConfirmationFrameCount)
{
state = default;
LastFailureReason =
"Detour定位质量正在恢复确认(" +
_normalLocalizationStepRecoveryFrameCount +
"/" +
_localizationQualityConfirmationFrameCount +
")，车辆状态暂不可用。";
return true;
}
_localizationStepUnavailable = false;
_normalLocalizationStepRecoveryFrameCount = 0;
AcceptReliableHeadingObservation(observation);
state = AcceptPoseAfterReset(
FrameTransform2D.TransformPose(
_controlFromDetour,
observation.PoseInDetour),
timestampSeconds);
LastFailureReason = string.Empty;
return true;
}
private bool HandleRepeatedDetourFrame(
double timestampSeconds,
out VehicleState state)
{
var cachedFrameAgeSeconds =
timestampSeconds -
_lastNewDetourFrameTimestampSeconds;
if (!NumericGuard.IsFinite(cachedFrameAgeSeconds) ||
cachedFrameAgeSeconds < 0.0)
{
InvalidateHeadingObservation(
"Detour缓存帧本机计时无效，航向暂不可用。");
state = default;
LastFailureReason =
"Detour缓存帧本机计时无效，车辆状态暂不可用。";
return false;
}
if (_localizationStepUnavailable)
{
state = default;
LastFailureReason =
"Detour定位质量尚未通过连续健康新帧恢复确认。";
return false;
}
if (cachedFrameAgeSeconds >
_maximumCachedFrameAgeSeconds)
{
_cachedFramePredictionActive = false;
InvalidateHeadingObservation(
"Detour源tick超过缓存停更上限，航向暂不可用。");
state = default;
LastFailureReason =
"Detour源tick已连续" +
cachedFrameAgeSeconds.ToString(
"F3",
CultureInfo.InvariantCulture) +
"s未更新，超过" +
_maximumCachedFrameAgeSeconds.ToString(
"F3",
CultureInfo.InvariantCulture) +
"s上限，车辆状态暂不可用。";
return false;
}
if (_jumpCandidateActive)
{
return ReturnCandidateState(
@@ -781,6 +1071,16 @@ namespace MultiWheelC.StateEstimation
out state);
}
if (IsVehicleMovingFromWheelFeedback())
{
_cachedFramePredictionActive = true;
state = CreatePredictedState(timestampSeconds);
LastFailureReason =
"Detour源tick暂未更新，当前使用轮组速度短时预测位姿。";
return true;
}
_cachedFramePredictionActive = false;
state = HandleRepeatedPose(timestampSeconds);
LastFailureReason = string.Empty;
return true;
@@ -974,6 +1274,54 @@ namespace MultiWheelC.StateEstimation
InPlaceRotationAngularSpeedThresholdRadiansPerSecond;
}
/// <summary>判断轮组反馈是否表明车辆仍在平移或转动。</summary>
private bool IsVehicleMovingFromWheelFeedback()
{
if (!_latestWheelVelocityValid)
{
return false;
}
var linearSpeedMetersPerSecond = Math.Sqrt(
_latestWheelTwistInBody.VxMetersPerSecond *
_latestWheelTwistInBody.VxMetersPerSecond +
_latestWheelTwistInBody.VyMetersPerSecond *
_latestWheelTwistInBody.VyMetersPerSecond);
return linearSpeedMetersPerSecond >
InPlaceRotationLinearSpeedThresholdMetersPerSecond ||
Math.Abs(
_latestWheelTwistInBody
.OmegaRadiansPerSecond) >
InPlaceRotationAngularSpeedThresholdRadiansPerSecond;
}
/// <summary>按照Detour源码中l_step到可信锚点的图距离进行质量分级。</summary>
public static DetourLocalizationQuality
ClassifyLocalizationStep(double localizationStep)
{
NumericGuard.EnsureFiniteNonNegative(
localizationStep,
nameof(localizationStep));
if (localizationStep >=
LostLocalizationStepLowerBound)
{
return DetourLocalizationQuality.Lost;
}
if (localizationStep >
DegradedLocalizationStepUpperBound)
{
return DetourLocalizationQuality.Poor;
}
return localizationStep >
NormalLocalizationStepUpperBound
? DetourLocalizationQuality.Degraded
: DetourLocalizationQuality.Normal;
}
private void ClearJumpCandidate()
{
_jumpCandidateActive = false;
@@ -1015,7 +1363,7 @@ namespace MultiWheelC.StateEstimation
NumericGuard.EnsureFinite(
yawDegrees,
"DetourTheta");
NumericGuard.EnsureFinite(
NumericGuard.EnsureFiniteNonNegative(
localizationStep,
"DetourLStep");
@@ -1459,6 +1807,21 @@ namespace MultiWheelC.StateEstimation
return "Uninitialized";
}
if (_localizationStepUnavailable)
{
return "LocalizationUnavailable";
}
if (_localizationStepPredictionActive)
{
return "LocalizationStepPrediction";
}
if (_cachedFramePredictionActive)
{
return "CachedFramePrediction";
}
if (_jumpCandidateActive)
{
var candidateAgeSeconds =
@@ -1482,6 +1845,18 @@ namespace MultiWheelC.StateEstimation
return "HeadingUnavailable";
}
if (LocalizationQuality ==
DetourLocalizationQuality.Poor)
{
return "LocalizationPoor";
}
if (LocalizationQuality ==
DetourLocalizationQuality.Degraded)
{
return "LocalizationDegraded";
}
return string.IsNullOrWhiteSpace(LastFailureReason)
? "Healthy"
: "Unavailable";