完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -0,0 +1,302 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetMemberCommandCorrectorTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyZeroErrorsPreserveBaseCommands();
|
||||
VerifyRelativePositionErrorProducesOpposingCorrection();
|
||||
VerifyCommonTranslationIsRemoved();
|
||||
VerifyCommonRotationIsRemoved();
|
||||
VerifyDeadbandSuppressesSmallErrors();
|
||||
VerifyCorrectionLimits();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetMemberCommandCorrector测试通过，共6个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyZeroErrorsPreserveBaseCommands()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var baseCommands = FleetKinematics.Decompose(
|
||||
layout,
|
||||
new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
new Twist2D(0.4, 0.1, 0.05)));
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
baseCommands,
|
||||
CreateErrors(Pose2D.Identity, Pose2D.Identity));
|
||||
|
||||
AssertCommandsEqual(
|
||||
corrected,
|
||||
baseCommands,
|
||||
"零布局误差");
|
||||
}
|
||||
|
||||
private static void
|
||||
VerifyRelativePositionErrorProducesOpposingCorrection()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.02, 0.0, 0.0),
|
||||
new Pose2D(0.02, 0.0, 0.0)));
|
||||
|
||||
AssertTwist(
|
||||
FindCommand(corrected, 1).TwistInVehicleBody,
|
||||
-0.02,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆1相对位置纠偏");
|
||||
AssertTwist(
|
||||
FindCommand(corrected, 2).TwistInVehicleBody,
|
||||
-0.02,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆2相对位置纠偏");
|
||||
}
|
||||
|
||||
private static void VerifyCommonTranslationIsRemoved()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.02, 0.0, 0.0),
|
||||
new Pose2D(-0.02, 0.0, 0.0)));
|
||||
|
||||
AssertAllStopped(
|
||||
corrected,
|
||||
"共同平移不应成为成员相对纠偏");
|
||||
}
|
||||
|
||||
private static void VerifyCommonRotationIsRemoved()
|
||||
{
|
||||
const double fleetYawErrorRadians = 0.02;
|
||||
var layout = CreateLayout();
|
||||
var commonRotationError = new Pose2D(
|
||||
0.0,
|
||||
-fleetYawErrorRadians,
|
||||
fleetYawErrorRadians);
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
commonRotationError,
|
||||
commonRotationError));
|
||||
|
||||
AssertAllStopped(
|
||||
corrected,
|
||||
"共同旋转不应成为成员相对纠偏");
|
||||
}
|
||||
|
||||
private static void VerifyDeadbandSuppressesSmallErrors()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrector = new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.005,
|
||||
yawErrorDeadbandRadians: 0.02,
|
||||
maximumLinearCorrectionMetersPerSecond: 1.0,
|
||||
maximumAngularCorrectionRadiansPerSecond: 1.0);
|
||||
var corrected = corrector.Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.004, 0.003, 0.01),
|
||||
new Pose2D(0.004, -0.003, -0.01)));
|
||||
|
||||
AssertAllStopped(corrected, "布局误差死区");
|
||||
}
|
||||
|
||||
private static void VerifyCorrectionLimits()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrector = new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.0,
|
||||
yawErrorDeadbandRadians: 0.0,
|
||||
maximumLinearCorrectionMetersPerSecond: 0.03,
|
||||
maximumAngularCorrectionRadiansPerSecond: 0.05);
|
||||
var corrected = corrector.Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.2, 0.0, 0.2),
|
||||
new Pose2D(0.2, 0.0, -0.2)));
|
||||
|
||||
for (var index = 0; index < corrected.Count; index++)
|
||||
{
|
||||
var twist = corrected[index].TwistInVehicleBody;
|
||||
var linearMagnitude = Math.Sqrt(
|
||||
twist.VxMetersPerSecond *
|
||||
twist.VxMetersPerSecond +
|
||||
twist.VyMetersPerSecond *
|
||||
twist.VyMetersPerSecond);
|
||||
|
||||
AssertNear(
|
||||
linearMagnitude,
|
||||
0.03,
|
||||
"线速度纠偏限幅");
|
||||
AssertNear(
|
||||
Math.Abs(twist.OmegaRadiansPerSecond),
|
||||
0.05,
|
||||
"角速度纠偏限幅");
|
||||
}
|
||||
}
|
||||
|
||||
private static FleetMemberCommandCorrector CreateCorrector()
|
||||
{
|
||||
return new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.0,
|
||||
yawErrorDeadbandRadians: 0.0,
|
||||
maximumLinearCorrectionMetersPerSecond: 1.0,
|
||||
maximumAngularCorrectionRadiansPerSecond: 1.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 IReadOnlyList<FleetMemberCommand>
|
||||
CreateStopCommands(FleetLayout layout)
|
||||
{
|
||||
return FleetKinematics.Decompose(
|
||||
layout,
|
||||
FleetMotionCommand.Stop());
|
||||
}
|
||||
|
||||
private static FleetMemberLayoutError[] CreateErrors(
|
||||
Pose2D vehicle1Error,
|
||||
Pose2D vehicle2Error)
|
||||
{
|
||||
return new[]
|
||||
{
|
||||
new FleetMemberLayoutError(1, vehicle1Error),
|
||||
new FleetMemberLayoutError(2, vehicle2Error)
|
||||
};
|
||||
}
|
||||
|
||||
private static FleetMemberCommand FindCommand(
|
||||
IReadOnlyList<FleetMemberCommand> commands,
|
||||
int vehicleId)
|
||||
{
|
||||
for (var index = 0; index < commands.Count; index++)
|
||||
{
|
||||
if (commands[index].VehicleId == vehicleId)
|
||||
{
|
||||
return commands[index];
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"没有找到车辆{vehicleId}的成员命令。");
|
||||
}
|
||||
|
||||
private static void AssertCommandsEqual(
|
||||
IReadOnlyList<FleetMemberCommand> actual,
|
||||
IReadOnlyList<FleetMemberCommand> expected,
|
||||
string scenario)
|
||||
{
|
||||
if (actual.Count != expected.Count)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}的命令数量不一致。");
|
||||
}
|
||||
|
||||
for (var index = 0; index < expected.Count; index++)
|
||||
{
|
||||
var expectedCommand = expected[index];
|
||||
var actualCommand = FindCommand(
|
||||
actual,
|
||||
expectedCommand.VehicleId);
|
||||
AssertTwist(
|
||||
actualCommand.TwistInVehicleBody,
|
||||
expectedCommand.TwistInVehicleBody
|
||||
.VxMetersPerSecond,
|
||||
expectedCommand.TwistInVehicleBody
|
||||
.VyMetersPerSecond,
|
||||
expectedCommand.TwistInVehicleBody
|
||||
.OmegaRadiansPerSecond,
|
||||
scenario);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertAllStopped(
|
||||
IReadOnlyList<FleetMemberCommand> commands,
|
||||
string scenario)
|
||||
{
|
||||
for (var index = 0; index < commands.Count; index++)
|
||||
{
|
||||
AssertTwist(
|
||||
commands[index].TwistInVehicleBody,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
scenario);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertTwist(
|
||||
Twist2D actual,
|
||||
double expectedVx,
|
||||
double expectedVy,
|
||||
double expectedOmega,
|
||||
string scenario)
|
||||
{
|
||||
AssertNear(
|
||||
actual.VxMetersPerSecond,
|
||||
expectedVx,
|
||||
scenario + " Vx");
|
||||
AssertNear(
|
||||
actual.VyMetersPerSecond,
|
||||
expectedVy,
|
||||
scenario + " Vy");
|
||||
AssertNear(
|
||||
actual.OmegaRadiansPerSecond,
|
||||
expectedOmega,
|
||||
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}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user