完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -0,0 +1,654 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Fleet;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetCoordinatorTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyCommandCycle();
|
||||
VerifySmallLayoutErrorIsCorrected();
|
||||
VerifyWarningRangeScalesAllCommands();
|
||||
VerifyUnavailableStateStopsAndRecovers();
|
||||
VerifyUnsafeLayoutFaultsAndLatches();
|
||||
VerifyCompletionStops();
|
||||
VerifyTrackingFaultStops();
|
||||
VerifyCancelReturnsInactive();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetCoordinator车队协调测试通过。共8个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyCommandCycle()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
|
||||
var result = coordinator.ExecuteCycle(
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(0.5, 0.0, 0.0),
|
||||
new Twist2D(0.4, 0.0, 0.0),
|
||||
1.0),
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.CommandGenerated,
|
||||
"正常协调周期");
|
||||
if (!output.State.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"正常协调周期没有返回车队状态。");
|
||||
}
|
||||
|
||||
AssertNear(
|
||||
output.State.Value.FleetPoseInWorld.XMeters,
|
||||
0.5,
|
||||
"正常协调周期中心X");
|
||||
AssertNear(
|
||||
output.SpeedScale,
|
||||
1.0,
|
||||
"正常协调周期速度比例");
|
||||
AssertTwist(
|
||||
output.FleetCommand.TwistAtReferencePoint,
|
||||
0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"正常协调周期车队命令");
|
||||
AssertTwist(
|
||||
FindCommand(output.MemberCommands, 1)
|
||||
.TwistInVehicleBody,
|
||||
0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"正常协调周期车辆1");
|
||||
AssertTwist(
|
||||
FindCommand(output.MemberCommands, 2)
|
||||
.TwistInVehicleBody,
|
||||
-0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"正常协调周期车辆2");
|
||||
}
|
||||
|
||||
private static void VerifySmallLayoutErrorIsCorrected()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
var result = coordinator.ExecuteCycle(
|
||||
new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
new Pose2D(-0.99, 0.0, 0.0),
|
||||
1.0),
|
||||
CreateMemberState(
|
||||
2,
|
||||
new Pose2D(0.99, 0.0, Math.PI),
|
||||
1.0)
|
||||
},
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.CommandGenerated,
|
||||
"小范围布局误差纠偏");
|
||||
AssertNear(
|
||||
output.SpeedScale,
|
||||
1.0,
|
||||
"小范围布局误差速度比例");
|
||||
AssertTwist(
|
||||
FindCommand(output.BaseMemberCommands, 1)
|
||||
.TwistInVehicleBody,
|
||||
0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆1基础命令");
|
||||
AssertTwist(
|
||||
FindCommand(output.BaseMemberCommands, 2)
|
||||
.TwistInVehicleBody,
|
||||
-0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆2基础命令");
|
||||
AssertTwist(
|
||||
FindCommand(output.MemberCommands, 1)
|
||||
.TwistInVehicleBody,
|
||||
0.395,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆1纠偏后命令");
|
||||
AssertTwist(
|
||||
FindCommand(output.MemberCommands, 2)
|
||||
.TwistInVehicleBody,
|
||||
-0.405,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆2纠偏后命令");
|
||||
}
|
||||
|
||||
private static void VerifyWarningRangeScalesAllCommands()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
var result = coordinator.ExecuteCycle(
|
||||
new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
new Pose2D(-0.965, 0.0, 0.0),
|
||||
1.0),
|
||||
CreateMemberState(
|
||||
2,
|
||||
new Pose2D(0.965, 0.0, Math.PI),
|
||||
1.0)
|
||||
},
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.CommandGenerated,
|
||||
"警告区间统一缩放");
|
||||
AssertNear(
|
||||
output.SpeedScale,
|
||||
0.5,
|
||||
"警告区间速度比例");
|
||||
AssertTwist(
|
||||
output.FleetCommand.TwistAtReferencePoint,
|
||||
0.2,
|
||||
0.0,
|
||||
0.0,
|
||||
"警告区间车队命令");
|
||||
AssertTwist(
|
||||
FindCommand(output.MemberCommands, 1)
|
||||
.TwistInVehicleBody,
|
||||
0.2,
|
||||
0.0,
|
||||
0.0,
|
||||
"警告区间车辆1");
|
||||
AssertTwist(
|
||||
FindCommand(output.MemberCommands, 2)
|
||||
.TwistInVehicleBody,
|
||||
-0.2,
|
||||
0.0,
|
||||
0.0,
|
||||
"警告区间车辆2");
|
||||
|
||||
if (string.IsNullOrWhiteSpace(output.Reason))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"警告区间缩放没有返回限制原因。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyUnavailableStateStopsAndRecovers()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
var unavailableStates = CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(0.5, 0.0, 0.0),
|
||||
new Twist2D(0.4, 0.0, 0.0),
|
||||
1.0);
|
||||
unavailableStates[1] = new FleetMemberStateSample(
|
||||
unavailableStates[1].VehicleId,
|
||||
unavailableStates[1].SampleTimestampSeconds,
|
||||
unavailableStates[1].PoseInWorld,
|
||||
unavailableStates[1].TwistAtVehicleOriginInWorld,
|
||||
isStateAvailable: false,
|
||||
hasValidVelocityEstimate: true);
|
||||
|
||||
var waitingResult = coordinator.ExecuteCycle(
|
||||
unavailableStates,
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var waitingOutput);
|
||||
|
||||
AssertResult(
|
||||
waitingResult,
|
||||
FleetCoordinationCycleResult.WaitingForState,
|
||||
"状态暂时不可用");
|
||||
AssertStop(waitingOutput, layout.VehicleCount);
|
||||
|
||||
if (!coordinator.IsActive)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"状态暂时不可用不应取消车队控制器。");
|
||||
}
|
||||
|
||||
var recoveredResult = coordinator.ExecuteCycle(
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(0.5, 0.0, 0.0),
|
||||
new Twist2D(0.4, 0.0, 0.0),
|
||||
1.02),
|
||||
targetTimestampSeconds: 1.02,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out _);
|
||||
|
||||
AssertResult(
|
||||
recoveredResult,
|
||||
FleetCoordinationCycleResult.CommandGenerated,
|
||||
"状态恢复");
|
||||
}
|
||||
|
||||
private static void VerifyUnsafeLayoutFaultsAndLatches()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
var deformedStates = new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
new Pose2D(-0.9, 0.0, 0.0),
|
||||
1.0),
|
||||
CreateMemberState(
|
||||
2,
|
||||
new Pose2D(0.9, 0.0, Math.PI),
|
||||
1.0)
|
||||
};
|
||||
|
||||
var result = coordinator.ExecuteCycle(
|
||||
deformedStates,
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.Faulted,
|
||||
"布局误差超限");
|
||||
AssertStop(output, layout.VehicleCount);
|
||||
|
||||
if (!coordinator.IsFaulted ||
|
||||
string.IsNullOrWhiteSpace(
|
||||
coordinator.LastFailureReason))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"布局误差故障没有被锁存。");
|
||||
}
|
||||
|
||||
var latchedResult = coordinator.ExecuteCycle(
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(0.5, 0.0, 0.0),
|
||||
new Twist2D(0.4, 0.0, 0.0),
|
||||
1.02),
|
||||
targetTimestampSeconds: 1.02,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var latchedOutput);
|
||||
|
||||
AssertResult(
|
||||
latchedResult,
|
||||
FleetCoordinationCycleResult.Faulted,
|
||||
"布局误差故障锁存");
|
||||
AssertStop(latchedOutput, layout.VehicleCount);
|
||||
}
|
||||
|
||||
private static void VerifyCompletionStops()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
|
||||
var result = coordinator.ExecuteCycle(
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(1.0, 0.0, 0.0),
|
||||
Twist2D.Zero,
|
||||
1.0),
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.Completed,
|
||||
"车队轨迹完成");
|
||||
AssertStop(output, layout.VehicleCount);
|
||||
|
||||
if (!coordinator.IsCompleted)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"车队轨迹完成状态没有被保存。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyTrackingFaultStops()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
|
||||
var result = coordinator.ExecuteCycle(
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(0.5, 0.5, 0.0),
|
||||
Twist2D.Zero,
|
||||
1.0),
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.Faulted,
|
||||
"车队中心跟踪故障");
|
||||
AssertStop(output, layout.VehicleCount);
|
||||
|
||||
if (!coordinator.IsFaulted)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"车队中心跟踪故障没有传递到协调器。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyCancelReturnsInactive()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var coordinator = CreateCoordinator();
|
||||
coordinator.Start(layout, CreateTrajectory());
|
||||
coordinator.Cancel();
|
||||
|
||||
var result = coordinator.ExecuteCycle(
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
new Pose2D(0.5, 0.0, 0.0),
|
||||
Twist2D.Zero,
|
||||
1.0),
|
||||
targetTimestampSeconds: 1.0,
|
||||
deltaTimeSeconds: 0.02,
|
||||
out var output);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetCoordinationCycleResult.Inactive,
|
||||
"取消车队协调");
|
||||
AssertStop(output, layout.VehicleCount);
|
||||
}
|
||||
|
||||
private static FleetCoordinator CreateCoordinator()
|
||||
{
|
||||
var estimator = new FleetStateEstimator(
|
||||
maximumMemberStateAgeSeconds: 0.25,
|
||||
maximumPositionDisagreementMeters: 0.5,
|
||||
maximumYawDisagreementRadians:
|
||||
AngleMath.DegreesToRadians(10.0));
|
||||
var controller = new FleetController(
|
||||
new StraightLateralController(),
|
||||
new ReferenceLongitudinalController(),
|
||||
new GcpCommandAllocator(Math.PI / 4.0),
|
||||
virtualControlPointRadiusMeters: 0.5);
|
||||
var commandCorrector =
|
||||
new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.005,
|
||||
yawErrorDeadbandRadians:
|
||||
AngleMath.DegreesToRadians(0.5),
|
||||
maximumLinearCorrectionMetersPerSecond:
|
||||
0.03,
|
||||
maximumAngularCorrectionRadiansPerSecond:
|
||||
AngleMath.DegreesToRadians(2.0));
|
||||
|
||||
return new FleetCoordinator(
|
||||
estimator,
|
||||
controller,
|
||||
commandCorrector,
|
||||
memberPositionErrorWarningMeters: 0.02,
|
||||
maximumMemberPositionErrorMeters: 0.05,
|
||||
memberYawErrorWarningRadians:
|
||||
AngleMath.DegreesToRadians(1.0),
|
||||
maximumMemberYawErrorRadians:
|
||||
AngleMath.DegreesToRadians(3.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 Trajectory2D CreateTrajectory()
|
||||
{
|
||||
return new Trajectory2D(
|
||||
new[]
|
||||
{
|
||||
new TrajectoryPoint(
|
||||
0.0,
|
||||
Pose2D.Identity,
|
||||
0.0,
|
||||
0.4),
|
||||
new TrajectoryPoint(
|
||||
1.0,
|
||||
new Pose2D(1.0, 0.0, 0.0),
|
||||
0.0,
|
||||
0.4)
|
||||
});
|
||||
}
|
||||
|
||||
private static FleetMemberStateSample[]
|
||||
CreateRigidMemberStates(
|
||||
FleetLayout layout,
|
||||
Pose2D fleetPoseInWorld,
|
||||
Twist2D twistAtFleetOriginInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
var states =
|
||||
new FleetMemberStateSample[layout.VehicleCount];
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicle = layout.Vehicles[index];
|
||||
var poseInWorld = FrameTransform2D.Compose(
|
||||
fleetPoseInWorld,
|
||||
vehicle.PoseInFleet);
|
||||
var offsetX =
|
||||
poseInWorld.XMeters -
|
||||
fleetPoseInWorld.XMeters;
|
||||
var offsetY =
|
||||
poseInWorld.YMeters -
|
||||
fleetPoseInWorld.YMeters;
|
||||
var twistInWorld = new Twist2D(
|
||||
twistAtFleetOriginInWorld.VxMetersPerSecond -
|
||||
twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
|
||||
offsetY,
|
||||
twistAtFleetOriginInWorld.VyMetersPerSecond +
|
||||
twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
|
||||
offsetX,
|
||||
twistAtFleetOriginInWorld.OmegaRadiansPerSecond);
|
||||
|
||||
states[index] = new FleetMemberStateSample(
|
||||
vehicle.VehicleId,
|
||||
timestampSeconds,
|
||||
poseInWorld,
|
||||
twistInWorld,
|
||||
isStateAvailable: true,
|
||||
hasValidVelocityEstimate: true);
|
||||
}
|
||||
|
||||
return states;
|
||||
}
|
||||
|
||||
private static FleetMemberStateSample CreateMemberState(
|
||||
int vehicleId,
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
return new FleetMemberStateSample(
|
||||
vehicleId,
|
||||
timestampSeconds,
|
||||
poseInWorld,
|
||||
Twist2D.Zero,
|
||||
isStateAvailable: true,
|
||||
hasValidVelocityEstimate: true);
|
||||
}
|
||||
|
||||
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 AssertStop(
|
||||
FleetCoordinationCycleOutput output,
|
||||
int expectedMemberCount)
|
||||
{
|
||||
AssertTwist(
|
||||
output.FleetCommand.TwistAtReferencePoint,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
"车队停止命令");
|
||||
|
||||
if (output.MemberCommands.Count != expectedMemberCount)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"停止输出的成员命令数量错误。");
|
||||
}
|
||||
|
||||
if (output.BaseMemberCommands.Count != expectedMemberCount)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"停止输出的成员基础命令数量错误。");
|
||||
}
|
||||
|
||||
for (var index = 0;
|
||||
index < output.MemberCommands.Count;
|
||||
index++)
|
||||
{
|
||||
AssertTwist(
|
||||
output.BaseMemberCommands[index]
|
||||
.TwistInVehicleBody,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
"成员基础停止命令");
|
||||
AssertTwist(
|
||||
output.MemberCommands[index].TwistInVehicleBody,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
"成员停止命令");
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertResult(
|
||||
FleetCoordinationCycleResult actual,
|
||||
FleetCoordinationCycleResult expected,
|
||||
string scenario)
|
||||
{
|
||||
if (actual != expected)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}结果错误：" +
|
||||
$"actual={actual}, expected={expected}。");
|
||||
}
|
||||
}
|
||||
|
||||
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}。");
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class StraightLateralController :
|
||||
ILateralController
|
||||
{
|
||||
public LateralControlCommand Compute(
|
||||
PathTrackingContext context)
|
||||
{
|
||||
return LateralControlCommand.Straight;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class ReferenceLongitudinalController :
|
||||
ILongitudinalController
|
||||
{
|
||||
public double ComputeSpeedMetersPerSecond(
|
||||
PathTrackingContext context)
|
||||
{
|
||||
return context.ControlReferenceSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user