完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -1 +1,579 @@
|
||||
// 主车汇总各车状态、时间对齐、计算车队中心和相对布局误差；输出FleetState
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 一辆成员车在主车统一时间轴上的状态样本。
|
||||
public readonly struct FleetMemberStateSample
|
||||
{
|
||||
public FleetMemberStateSample(
|
||||
int vehicleId,
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D poseInWorld,
|
||||
Twist2D twistAtVehicleOriginInWorld,
|
||||
bool isStateAvailable,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"编队成员车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
NumericGuard.EnsureFinite(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
NumericGuard.EnsureFinite(
|
||||
twistAtVehicleOriginInWorld,
|
||||
nameof(twistAtVehicleOriginInWorld));
|
||||
|
||||
VehicleId = vehicleId;
|
||||
SampleTimestampSeconds = sampleTimestampSeconds;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
TwistAtVehicleOriginInWorld =
|
||||
twistAtVehicleOriginInWorld;
|
||||
IsStateAvailable = isStateAvailable;
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
// 该时间戳必须已经换算到主车/协调器的单调时间轴。
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
// 成员车体中心处的实际速度，在世界坐标系中表达。
|
||||
public Twist2D TwistAtVehicleOriginInWorld { get; }
|
||||
|
||||
public bool IsStateAvailable { get; }
|
||||
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
}
|
||||
|
||||
// 成员实际位姿相对固定布局目标位姿的误差。
|
||||
public readonly struct FleetMemberLayoutError
|
||||
{
|
||||
public FleetMemberLayoutError(
|
||||
int vehicleId,
|
||||
Pose2D actualPoseInExpectedVehicleFrame)
|
||||
{
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"编队成员车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
actualPoseInExpectedVehicleFrame,
|
||||
nameof(actualPoseInExpectedVehicleFrame));
|
||||
|
||||
VehicleId = vehicleId;
|
||||
ActualPoseInExpectedVehicleFrame =
|
||||
new Pose2D(
|
||||
actualPoseInExpectedVehicleFrame.XMeters,
|
||||
actualPoseInExpectedVehicleFrame.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
actualPoseInExpectedVehicleFrame
|
||||
.YawRadians));
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
// 期望成员车体系中表达的实际成员位姿；理想刚体布局时为Identity。
|
||||
public Pose2D ActualPoseInExpectedVehicleFrame { get; }
|
||||
}
|
||||
|
||||
// 一次车队状态估计的结果；不可用时不提供FleetState。
|
||||
public sealed class FleetStateEstimateResult
|
||||
{
|
||||
private static readonly IReadOnlyList<FleetMemberLayoutError>
|
||||
EmptyMemberErrors = Array.AsReadOnly(
|
||||
Array.Empty<FleetMemberLayoutError>());
|
||||
|
||||
private FleetStateEstimateResult(
|
||||
bool isAvailable,
|
||||
FleetState? state,
|
||||
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
||||
string unavailableReason)
|
||||
{
|
||||
IsAvailable = isAvailable;
|
||||
State = state;
|
||||
MemberErrors = memberErrors;
|
||||
UnavailableReason = unavailableReason;
|
||||
}
|
||||
|
||||
public bool IsAvailable { get; }
|
||||
|
||||
public FleetState? State { get; }
|
||||
|
||||
public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }
|
||||
|
||||
public string UnavailableReason { get; }
|
||||
|
||||
internal static FleetStateEstimateResult Available(
|
||||
FleetState state,
|
||||
FleetMemberLayoutError[] memberErrors)
|
||||
{
|
||||
return new FleetStateEstimateResult(
|
||||
true,
|
||||
state,
|
||||
Array.AsReadOnly(memberErrors),
|
||||
string.Empty);
|
||||
}
|
||||
|
||||
internal static FleetStateEstimateResult Unavailable(
|
||||
string reason)
|
||||
{
|
||||
return new FleetStateEstimateResult(
|
||||
false,
|
||||
null,
|
||||
EmptyMemberErrors,
|
||||
reason ?? string.Empty);
|
||||
}
|
||||
}
|
||||
|
||||
// 从各成员状态反算并融合车队虚拟中心状态。
|
||||
public sealed class FleetStateEstimator
|
||||
{
|
||||
private const double TimestampToleranceSeconds = 1e-9;
|
||||
private const double MinimumCircularMeanMagnitude = 1e-12;
|
||||
|
||||
private readonly double _maximumMemberStateAgeSeconds;
|
||||
private readonly double _maximumPositionDisagreementMeters;
|
||||
private readonly double _maximumYawDisagreementRadians;
|
||||
|
||||
public FleetStateEstimator(
|
||||
double maximumMemberStateAgeSeconds,
|
||||
double maximumPositionDisagreementMeters,
|
||||
double maximumYawDisagreementRadians)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumMemberStateAgeSeconds,
|
||||
nameof(maximumMemberStateAgeSeconds));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumPositionDisagreementMeters,
|
||||
nameof(maximumPositionDisagreementMeters));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumYawDisagreementRadians,
|
||||
nameof(maximumYawDisagreementRadians));
|
||||
|
||||
if (maximumYawDisagreementRadians > Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(maximumYawDisagreementRadians),
|
||||
"车队候选航向差阈值不能大于π。");
|
||||
}
|
||||
|
||||
_maximumMemberStateAgeSeconds =
|
||||
maximumMemberStateAgeSeconds;
|
||||
_maximumPositionDisagreementMeters =
|
||||
maximumPositionDisagreementMeters;
|
||||
_maximumYawDisagreementRadians =
|
||||
maximumYawDisagreementRadians;
|
||||
}
|
||||
|
||||
public FleetStateEstimateResult Estimate(
|
||||
FleetLayout layout,
|
||||
IReadOnlyList<FleetMemberStateSample> memberStates,
|
||||
double targetTimestampSeconds)
|
||||
{
|
||||
if (layout == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(layout));
|
||||
}
|
||||
|
||||
if (memberStates == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(memberStates));
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
targetTimestampSeconds,
|
||||
nameof(targetTimestampSeconds));
|
||||
|
||||
if (memberStates.Count != layout.VehicleCount)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"成员状态数量{memberStates.Count}与布局数量" +
|
||||
$"{layout.VehicleCount}不一致。");
|
||||
}
|
||||
|
||||
var statesByVehicleId =
|
||||
new Dictionary<int, FleetMemberStateSample>(
|
||||
memberStates.Count);
|
||||
|
||||
for (var index = 0;
|
||||
index < memberStates.Count;
|
||||
index++)
|
||||
{
|
||||
var memberState = memberStates[index];
|
||||
if (memberState.VehicleId <= 0)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"第{index}个成员状态的车号无效。");
|
||||
}
|
||||
|
||||
if (!layout.TryGetVehicle(
|
||||
memberState.VehicleId,
|
||||
out _))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"成员状态包含布局外车辆" +
|
||||
$"{memberState.VehicleId}。");
|
||||
}
|
||||
|
||||
if (statesByVehicleId.ContainsKey(
|
||||
memberState.VehicleId))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"成员状态包含重复车号" +
|
||||
$"{memberState.VehicleId}。");
|
||||
}
|
||||
|
||||
statesByVehicleId.Add(
|
||||
memberState.VehicleId,
|
||||
memberState);
|
||||
}
|
||||
|
||||
var alignedMembers =
|
||||
new AlignedMemberState[layout.VehicleCount];
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicleLayout = layout.Vehicles[index];
|
||||
if (!statesByVehicleId.TryGetValue(
|
||||
vehicleLayout.VehicleId,
|
||||
out var memberState))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"缺少车辆{vehicleLayout.VehicleId}的状态。");
|
||||
}
|
||||
|
||||
var alignmentResult = AlignMemberState(
|
||||
memberState,
|
||||
targetTimestampSeconds,
|
||||
out var alignedPoseInWorld);
|
||||
if (alignmentResult != null)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
alignmentResult);
|
||||
}
|
||||
|
||||
var candidateFleetPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
alignedPoseInWorld,
|
||||
FrameTransform2D.Inverse(
|
||||
vehicleLayout.PoseInFleet));
|
||||
|
||||
alignedMembers[index] =
|
||||
new AlignedMemberState(
|
||||
vehicleLayout,
|
||||
memberState,
|
||||
alignedPoseInWorld,
|
||||
candidateFleetPoseInWorld);
|
||||
}
|
||||
|
||||
var disagreementReason =
|
||||
FindCandidateDisagreement(alignedMembers);
|
||||
if (disagreementReason != null)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
disagreementReason);
|
||||
}
|
||||
|
||||
if (!TryAverageCandidateFleetPose(
|
||||
alignedMembers,
|
||||
out var fleetPoseInWorld))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
"成员候选航向无法形成唯一的车队平均航向。");
|
||||
}
|
||||
|
||||
var hasValidVelocityEstimate =
|
||||
TryAverageFleetOriginTwist(
|
||||
alignedMembers,
|
||||
fleetPoseInWorld,
|
||||
out var twistAtFleetOriginInWorld);
|
||||
var fleetState = new FleetState(
|
||||
targetTimestampSeconds,
|
||||
fleetPoseInWorld,
|
||||
twistAtFleetOriginInWorld,
|
||||
hasValidVelocityEstimate);
|
||||
var memberErrors = CalculateMemberErrors(
|
||||
alignedMembers,
|
||||
fleetPoseInWorld);
|
||||
|
||||
return FleetStateEstimateResult.Available(
|
||||
fleetState,
|
||||
memberErrors);
|
||||
}
|
||||
|
||||
private string AlignMemberState(
|
||||
FleetMemberStateSample memberState,
|
||||
double targetTimestampSeconds,
|
||||
out Pose2D alignedPoseInWorld)
|
||||
{
|
||||
alignedPoseInWorld = memberState.PoseInWorld;
|
||||
|
||||
if (!memberState.IsStateAvailable)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}状态不可用。";
|
||||
}
|
||||
|
||||
var ageSeconds =
|
||||
targetTimestampSeconds -
|
||||
memberState.SampleTimestampSeconds;
|
||||
|
||||
if (ageSeconds < -TimestampToleranceSeconds)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}的状态时间晚于" +
|
||||
"本次估计目标时间。";
|
||||
}
|
||||
|
||||
if (ageSeconds > _maximumMemberStateAgeSeconds)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}的状态已过期：" +
|
||||
$"{ageSeconds:F3}s。";
|
||||
}
|
||||
|
||||
if (ageSeconds <= TimestampToleranceSeconds)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
if (!memberState.HasValidVelocityEstimate)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}缺少时间对齐所需的" +
|
||||
"有效速度。";
|
||||
}
|
||||
|
||||
var twist = memberState.TwistAtVehicleOriginInWorld;
|
||||
alignedPoseInWorld = new Pose2D(
|
||||
memberState.PoseInWorld.XMeters +
|
||||
twist.VxMetersPerSecond * ageSeconds,
|
||||
memberState.PoseInWorld.YMeters +
|
||||
twist.VyMetersPerSecond * ageSeconds,
|
||||
AngleMath.NormalizeRadians(
|
||||
memberState.PoseInWorld.YawRadians +
|
||||
twist.OmegaRadiansPerSecond * ageSeconds));
|
||||
return null;
|
||||
}
|
||||
|
||||
private string FindCandidateDisagreement(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers)
|
||||
{
|
||||
for (var firstIndex = 0;
|
||||
firstIndex < alignedMembers.Count;
|
||||
firstIndex++)
|
||||
{
|
||||
var first = alignedMembers[firstIndex];
|
||||
for (var secondIndex = firstIndex + 1;
|
||||
secondIndex < alignedMembers.Count;
|
||||
secondIndex++)
|
||||
{
|
||||
var second = alignedMembers[secondIndex];
|
||||
var dx =
|
||||
first.CandidateFleetPoseInWorld.XMeters -
|
||||
second.CandidateFleetPoseInWorld.XMeters;
|
||||
var dy =
|
||||
first.CandidateFleetPoseInWorld.YMeters -
|
||||
second.CandidateFleetPoseInWorld.YMeters;
|
||||
var positionDifferenceMeters =
|
||||
Math.Sqrt(dx * dx + dy * dy);
|
||||
var yawDifferenceRadians = Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
first.CandidateFleetPoseInWorld
|
||||
.YawRadians,
|
||||
second.CandidateFleetPoseInWorld
|
||||
.YawRadians));
|
||||
|
||||
if (positionDifferenceMeters >
|
||||
_maximumPositionDisagreementMeters)
|
||||
{
|
||||
return $"车辆{first.VehicleLayout.VehicleId}与" +
|
||||
$"车辆{second.VehicleLayout.VehicleId}反算的" +
|
||||
$"车队中心相差{positionDifferenceMeters:F3}m，" +
|
||||
"超过允许值。";
|
||||
}
|
||||
|
||||
if (yawDifferenceRadians >
|
||||
_maximumYawDisagreementRadians)
|
||||
{
|
||||
return $"车辆{first.VehicleLayout.VehicleId}与" +
|
||||
$"车辆{second.VehicleLayout.VehicleId}反算的" +
|
||||
$"车队航向相差" +
|
||||
$"{AngleMath.RadiansToDegrees(yawDifferenceRadians):F2}°，" +
|
||||
"超过允许值。";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
private static bool TryAverageCandidateFleetPose(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers,
|
||||
out Pose2D fleetPoseInWorld)
|
||||
{
|
||||
var xMeters = 0.0;
|
||||
var yMeters = 0.0;
|
||||
var yawCosineSum = 0.0;
|
||||
var yawSineSum = 0.0;
|
||||
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
var candidate =
|
||||
alignedMembers[index]
|
||||
.CandidateFleetPoseInWorld;
|
||||
xMeters += candidate.XMeters;
|
||||
yMeters += candidate.YMeters;
|
||||
yawCosineSum += Math.Cos(candidate.YawRadians);
|
||||
yawSineSum += Math.Sin(candidate.YawRadians);
|
||||
}
|
||||
|
||||
var count = alignedMembers.Count;
|
||||
var circularMeanMagnitude = Math.Sqrt(
|
||||
yawCosineSum * yawCosineSum +
|
||||
yawSineSum * yawSineSum);
|
||||
if (circularMeanMagnitude <
|
||||
MinimumCircularMeanMagnitude)
|
||||
{
|
||||
fleetPoseInWorld = Pose2D.Identity;
|
||||
return false;
|
||||
}
|
||||
|
||||
fleetPoseInWorld = new Pose2D(
|
||||
xMeters / count,
|
||||
yMeters / count,
|
||||
Math.Atan2(yawSineSum, yawCosineSum));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAverageFleetOriginTwist(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers,
|
||||
Pose2D fleetPoseInWorld,
|
||||
out Twist2D twistAtFleetOriginInWorld)
|
||||
{
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
if (!alignedMembers[index]
|
||||
.MemberState
|
||||
.HasValidVelocityEstimate)
|
||||
{
|
||||
twistAtFleetOriginInWorld = Twist2D.Zero;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
var vxMetersPerSecond = 0.0;
|
||||
var vyMetersPerSecond = 0.0;
|
||||
var omegaRadiansPerSecond = 0.0;
|
||||
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
var member = alignedMembers[index];
|
||||
var twist = member.MemberState
|
||||
.TwistAtVehicleOriginInWorld;
|
||||
var memberXFromFleetOrigin =
|
||||
member.AlignedPoseInWorld.XMeters -
|
||||
fleetPoseInWorld.XMeters;
|
||||
var memberYFromFleetOrigin =
|
||||
member.AlignedPoseInWorld.YMeters -
|
||||
fleetPoseInWorld.YMeters;
|
||||
|
||||
vxMetersPerSecond +=
|
||||
twist.VxMetersPerSecond +
|
||||
twist.OmegaRadiansPerSecond *
|
||||
memberYFromFleetOrigin;
|
||||
vyMetersPerSecond +=
|
||||
twist.VyMetersPerSecond -
|
||||
twist.OmegaRadiansPerSecond *
|
||||
memberXFromFleetOrigin;
|
||||
omegaRadiansPerSecond +=
|
||||
twist.OmegaRadiansPerSecond;
|
||||
}
|
||||
|
||||
var count = alignedMembers.Count;
|
||||
twistAtFleetOriginInWorld = new Twist2D(
|
||||
vxMetersPerSecond / count,
|
||||
vyMetersPerSecond / count,
|
||||
omegaRadiansPerSecond / count);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static FleetMemberLayoutError[] CalculateMemberErrors(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers,
|
||||
Pose2D fleetPoseInWorld)
|
||||
{
|
||||
var errors =
|
||||
new FleetMemberLayoutError[alignedMembers.Count];
|
||||
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
var member = alignedMembers[index];
|
||||
var expectedPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
fleetPoseInWorld,
|
||||
member.VehicleLayout.PoseInFleet);
|
||||
var actualPoseInExpectedVehicleFrame =
|
||||
FrameTransform2D.Compose(
|
||||
FrameTransform2D.Inverse(
|
||||
expectedPoseInWorld),
|
||||
member.AlignedPoseInWorld);
|
||||
|
||||
errors[index] = new FleetMemberLayoutError(
|
||||
member.VehicleLayout.VehicleId,
|
||||
actualPoseInExpectedVehicleFrame);
|
||||
}
|
||||
|
||||
return errors;
|
||||
}
|
||||
|
||||
private readonly struct AlignedMemberState
|
||||
{
|
||||
public AlignedMemberState(
|
||||
VehicleLayout vehicleLayout,
|
||||
FleetMemberStateSample memberState,
|
||||
Pose2D alignedPoseInWorld,
|
||||
Pose2D candidateFleetPoseInWorld)
|
||||
{
|
||||
VehicleLayout = vehicleLayout;
|
||||
MemberState = memberState;
|
||||
AlignedPoseInWorld = alignedPoseInWorld;
|
||||
CandidateFleetPoseInWorld =
|
||||
candidateFleetPoseInWorld;
|
||||
}
|
||||
|
||||
public VehicleLayout VehicleLayout { get; }
|
||||
|
||||
public FleetMemberStateSample MemberState { get; }
|
||||
|
||||
public Pose2D AlignedPoseInWorld { get; }
|
||||
|
||||
public Pose2D CandidateFleetPoseInWorld { get; }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user