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.Tests/FleetStateEstimatorTests.cs
+430
View File
@@ -0,0 +1,430 @@
using System;
using System.Collections.Generic;
using MultiWheelC.Fleet;
using MyParking.Shared;
namespace MultiWheelC.Tests
{
internal static class FleetStateEstimatorTests
{
private const double Tolerance = 1e-9;
public static void Run()
{
VerifyRigidStateIsRecovered();
VerifyOlderSamplesAreAligned();
VerifyYawWrapAroundIsAveraged();
VerifySmallLayoutErrorIsReported();
VerifyInconsistentCentersAreRejected();
VerifyMissingMemberIsRejected();
VerifyInvalidVelocityRemainsExplicit();
Console.WriteLine(
"FleetStateEstimator车队状态估计测试通过。共7个场景。");
}
private static void VerifyRigidStateIsRecovered()
{
var layout = CreateLayout();
var fleetPose = new Pose2D(4.0, -2.0, 0.4);
var fleetTwist = new Twist2D(0.3, -0.1, 0.2);
var result = CreateEstimator().Estimate(
layout,
CreateRigidMemberStates(
layout,
fleetPose,
fleetTwist,
sampleTimestampSeconds: 5.0,
hasValidVelocityEstimate: true),
targetTimestampSeconds: 5.0);
var state = RequireState(result, "刚体状态还原");
AssertPose(
state.FleetPoseInWorld,
fleetPose,
"刚体状态还原");
AssertTwist(
state.TwistAtFleetOriginInWorld,
fleetTwist,
"刚体速度还原");
for (var index = 0;
index < result.MemberErrors.Count;
index++)
{
AssertPose(
result.MemberErrors[index]
.ActualPoseInExpectedVehicleFrame,
Pose2D.Identity,
"刚体布局误差");
}
}
private static void VerifyOlderSamplesAreAligned()
{
var layout = CreateLayout();
var sampleFleetPose =
new Pose2D(1.0, 2.0, 0.3);
var fleetTwist =
new Twist2D(0.4, -0.2, 0.0);
var result = CreateEstimator().Estimate(
layout,
CreateRigidMemberStates(
layout,
sampleFleetPose,
fleetTwist,
sampleTimestampSeconds: 0.9,
hasValidVelocityEstimate: true),
targetTimestampSeconds: 1.0);
var state = RequireState(result, "成员时间对齐");
AssertPose(
state.FleetPoseInWorld,
new Pose2D(1.04, 1.98, 0.3),
"成员时间对齐");
AssertTwist(
state.TwistAtFleetOriginInWorld,
fleetTwist,
"时间对齐后速度");
}
private static void VerifySmallLayoutErrorIsReported()
{
var layout = CreateLayout();
var result = CreateEstimator().Estimate(
layout,
new[]
{
CreateMemberState(
1,
new Pose2D(-0.98, 0.0, 0.0),
Twist2D.Zero,
1.0,
true),
CreateMemberState(
2,
new Pose2D(0.99, 0.0, Math.PI),
Twist2D.Zero,
1.0,
true)
},
targetTimestampSeconds: 1.0);
var state = RequireState(result, "小范围布局误差");
AssertNear(
state.FleetPoseInWorld.XMeters,
0.005,
"小范围布局误差中心X");
AssertNear(
FindError(result.MemberErrors, 1)
.ActualPoseInExpectedVehicleFrame.XMeters,
0.015,
"车辆1布局误差X");
AssertNear(
FindError(result.MemberErrors, 2)
.ActualPoseInExpectedVehicleFrame.XMeters,
0.015,
"车辆2布局误差X");
}
private static void VerifyYawWrapAroundIsAveraged()
{
var layout = CreateLayout();
var firstCandidate = new Pose2D(
0.0,
0.0,
AngleMath.DegreesToRadians(179.0));
var secondCandidate = new Pose2D(
0.0,
0.0,
AngleMath.DegreesToRadians(-179.0));
var result = CreateEstimator().Estimate(
layout,
new[]
{
CreateMemberState(
1,
FrameTransform2D.Compose(
firstCandidate,
layout.Vehicles[0].PoseInFleet),
Twist2D.Zero,
1.0,
true),
CreateMemberState(
2,
FrameTransform2D.Compose(
secondCandidate,
layout.Vehicles[1].PoseInFleet),
Twist2D.Zero,
1.0,
true)
},
targetTimestampSeconds: 1.0);
var state = RequireState(result, "跨正负π航向平均");
AssertNear(
Math.Abs(state.FleetPoseInWorld.YawRadians),
Math.PI,
"跨正负π航向平均");
}
private static void VerifyInconsistentCentersAreRejected()
{
var result = CreateEstimator().Estimate(
CreateLayout(),
new[]
{
CreateMemberState(
1,
new Pose2D(-1.0, 0.0, 0.0),
Twist2D.Zero,
1.0,
true),
CreateMemberState(
2,
new Pose2D(1.3, 0.0, Math.PI),
Twist2D.Zero,
1.0,
true)
},
targetTimestampSeconds: 1.0);
AssertUnavailable(result, "候选中心冲突");
}
private static void VerifyMissingMemberIsRejected()
{
var result = CreateEstimator().Estimate(
CreateLayout(),
new[]
{
CreateMemberState(
1,
new Pose2D(-1.0, 0.0, 0.0),
Twist2D.Zero,
1.0,
true)
},
targetTimestampSeconds: 1.0);
AssertUnavailable(result, "成员缺失");
}
private static void VerifyInvalidVelocityRemainsExplicit()
{
var layout = CreateLayout();
var result = CreateEstimator().Estimate(
layout,
CreateRigidMemberStates(
layout,
Pose2D.Identity,
new Twist2D(0.4, 0.0, 0.0),
sampleTimestampSeconds: 1.0,
hasValidVelocityEstimate: false),
targetTimestampSeconds: 1.0);
var state = RequireState(result, "速度未初始化");
if (state.HasValidVelocityEstimate)
{
throw new InvalidOperationException(
"成员速度无效时车队速度不应标记为有效。");
}
AssertTwist(
state.TwistAtFleetOriginInWorld,
Twist2D.Zero,
"速度未初始化");
}
private static FleetStateEstimator CreateEstimator()
{
return new FleetStateEstimator(
maximumMemberStateAgeSeconds: 0.25,
maximumPositionDisagreementMeters: 0.1,
maximumYawDisagreementRadians:
AngleMath.DegreesToRadians(5.0));
}
private static FleetLayout CreateLayout()
{
return new FleetLayout(
new[]
{
new VehicleLayout(
1,
new Pose2D(-1.0, 0.0, 0.0)),
new VehicleLayout(
2,
new Pose2D(1.0, 0.0, Math.PI))
});
}
private static FleetMemberStateSample[]
CreateRigidMemberStates(
FleetLayout layout,
Pose2D fleetPoseInWorld,
Twist2D twistAtFleetOriginInWorld,
double sampleTimestampSeconds,
bool hasValidVelocityEstimate)
{
var states =
new FleetMemberStateSample[layout.VehicleCount];
for (var index = 0;
index < layout.Vehicles.Count;
index++)
{
var vehicle = layout.Vehicles[index];
var memberPoseInWorld =
FrameTransform2D.Compose(
fleetPoseInWorld,
vehicle.PoseInFleet);
var xFromFleetOrigin =
memberPoseInWorld.XMeters -
fleetPoseInWorld.XMeters;
var yFromFleetOrigin =
memberPoseInWorld.YMeters -
fleetPoseInWorld.YMeters;
var memberTwistInWorld = new Twist2D(
twistAtFleetOriginInWorld
.VxMetersPerSecond -
twistAtFleetOriginInWorld
.OmegaRadiansPerSecond *
yFromFleetOrigin,
twistAtFleetOriginInWorld
.VyMetersPerSecond +
twistAtFleetOriginInWorld
.OmegaRadiansPerSecond *
xFromFleetOrigin,
twistAtFleetOriginInWorld
.OmegaRadiansPerSecond);
states[index] = new FleetMemberStateSample(
vehicle.VehicleId,
sampleTimestampSeconds,
memberPoseInWorld,
memberTwistInWorld,
isStateAvailable: true,
hasValidVelocityEstimate:
hasValidVelocityEstimate);
}
return states;
}
private static FleetMemberStateSample CreateMemberState(
int vehicleId,
Pose2D poseInWorld,
Twist2D twistInWorld,
double timestampSeconds,
bool hasValidVelocityEstimate)
{
return new FleetMemberStateSample(
vehicleId,
timestampSeconds,
poseInWorld,
twistInWorld,
isStateAvailable: true,
hasValidVelocityEstimate:
hasValidVelocityEstimate);
}
private static FleetState RequireState(
FleetStateEstimateResult result,
string scenario)
{
if (!result.IsAvailable || !result.State.HasValue)
{
throw new InvalidOperationException(
$"{scenario}应产生可用状态：" +
result.UnavailableReason);
}
return result.State.Value;
}
private static void AssertUnavailable(
FleetStateEstimateResult result,
string scenario)
{
if (result.IsAvailable ||
string.IsNullOrWhiteSpace(
result.UnavailableReason))
{
throw new InvalidOperationException(
$"{scenario}应返回带原因的不可用结果。");
}
}
private static FleetMemberLayoutError FindError(
IReadOnlyList<FleetMemberLayoutError> errors,
int vehicleId)
{
for (var index = 0;
index < errors.Count;
index++)
{
if (errors[index].VehicleId == vehicleId)
{
return errors[index];
}
}
throw new InvalidOperationException(
$"没有找到车辆{vehicleId}的布局误差。");
}
private static void AssertPose(
Pose2D actual,
Pose2D expected,
string scenario)
{
AssertNear(
actual.XMeters,
expected.XMeters,
scenario + " X");
AssertNear(
actual.YMeters,
expected.YMeters,
scenario + " Y");
AssertNear(
AngleMath.ShortestDifferenceRadians(
actual.YawRadians,
expected.YawRadians),
0.0,
scenario + " Yaw");
}
private static void AssertTwist(
Twist2D actual,
Twist2D expected,
string scenario)
{
AssertNear(
actual.VxMetersPerSecond,
expected.VxMetersPerSecond,
scenario + " Vx");
AssertNear(
actual.VyMetersPerSecond,
expected.VyMetersPerSecond,
scenario + " Vy");
AssertNear(
actual.OmegaRadiansPerSecond,
expected.OmegaRadiansPerSecond,
scenario + " Omega");
}
private static void AssertNear(
double actual,
double expected,
string valueName)
{
if (Math.Abs(actual - expected) > Tolerance)
{
throw new InvalidOperationException(
$"{valueName}错误：" +
$"actual={actual:F9}, expected={expected:F9}。");
}
}
}
}