完善Detour状态估计与轨迹跟踪验证
This commit is contained in:
@@ -0,0 +1,202 @@
|
||||
using System;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Lateral;
|
||||
using MultiWheelC.StateEstimation;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证Stanley控制器在前进和倒车时的横向误差符号及差动转角方向。
|
||||
/// </summary>
|
||||
internal static class Program
|
||||
{
|
||||
private const double SpeedMagnitudeMetersPerSecond = 0.4;
|
||||
private const double TestLateralErrorMeters = 0.1;
|
||||
private const double TestHeadingErrorRadians = 0.1;
|
||||
private const double TestCurvaturePerMeter = 0.2;
|
||||
|
||||
/// <summary>
|
||||
/// 运行不依赖宿主、Detour或实车底盘的控制器数学测试。
|
||||
/// </summary>
|
||||
private static void Main()
|
||||
{
|
||||
foreach (var travelDirection in new[] { 1.0, -1.0 })
|
||||
{
|
||||
VerifyCrossTrackConvergence(
|
||||
travelDirection,
|
||||
TestLateralErrorMeters);
|
||||
VerifyCrossTrackConvergence(
|
||||
travelDirection,
|
||||
-TestLateralErrorMeters);
|
||||
VerifyHeadingDirection(travelDirection);
|
||||
VerifyCurvatureDirection(travelDirection);
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
"Stanley前进/倒车横向符号测试通过。共8个场景。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证共同转角产生的横向速度始终使轨迹点序横向误差绝对值减小。
|
||||
/// </summary>
|
||||
private static void VerifyCrossTrackConvergence(
|
||||
double travelDirection,
|
||||
double lateralErrorMeters)
|
||||
{
|
||||
var signedSpeedMetersPerSecond =
|
||||
travelDirection *
|
||||
SpeedMagnitudeMetersPerSecond;
|
||||
var controller = CreateController();
|
||||
var command = controller.Compute(
|
||||
CreateContext(
|
||||
signedSpeedMetersPerSecond,
|
||||
lateralErrorMeters,
|
||||
headingErrorRadians: 0.0,
|
||||
feedforwardCurvaturePerMeter: 0.0));
|
||||
|
||||
var bodyLateralSpeedMetersPerSecond =
|
||||
signedSpeedMetersPerSecond *
|
||||
Math.Sin(command.CommonAngleRadians);
|
||||
|
||||
// 轨迹执行方向在倒车时与车体X轴相反，因此需要先把车体
|
||||
// 横向速度换算到轨迹点序坐标系，再计算参考轨迹相对车辆的误差变化率。
|
||||
var lateralErrorDerivativeMetersPerSecond =
|
||||
-travelDirection *
|
||||
bodyLateralSpeedMetersPerSecond;
|
||||
|
||||
AssertTrue(
|
||||
lateralErrorMeters *
|
||||
lateralErrorDerivativeMetersPerSecond < 0.0,
|
||||
$"横向误差没有收敛：direction={travelDirection}，" +
|
||||
$"error={lateralErrorMeters:F3}m，" +
|
||||
$"common={command.CommonAngleRadians:F6}rad，" +
|
||||
$"errorDerivative={lateralErrorDerivativeMetersPerSecond:F6}m/s。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证航向误差差动转角在倒车时仍按行驶方向反号。
|
||||
/// </summary>
|
||||
private static void VerifyHeadingDirection(
|
||||
double travelDirection)
|
||||
{
|
||||
var command = CreateController().Compute(
|
||||
CreateContext(
|
||||
travelDirection *
|
||||
SpeedMagnitudeMetersPerSecond,
|
||||
lateralErrorMeters: 0.0,
|
||||
headingErrorRadians:
|
||||
TestHeadingErrorRadians,
|
||||
feedforwardCurvaturePerMeter: 0.0));
|
||||
|
||||
AssertSameSign(
|
||||
command.DifferentialAngleRadians,
|
||||
travelDirection,
|
||||
"航向误差差动转角");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证正曲率前馈差动转角在倒车时仍按行驶方向反号。
|
||||
/// </summary>
|
||||
private static void VerifyCurvatureDirection(
|
||||
double travelDirection)
|
||||
{
|
||||
var command = CreateController().Compute(
|
||||
CreateContext(
|
||||
travelDirection *
|
||||
SpeedMagnitudeMetersPerSecond,
|
||||
lateralErrorMeters: 0.0,
|
||||
headingErrorRadians: 0.0,
|
||||
feedforwardCurvaturePerMeter:
|
||||
TestCurvaturePerMeter));
|
||||
|
||||
AssertSameSign(
|
||||
command.DifferentialAngleRadians,
|
||||
travelDirection,
|
||||
"曲率前馈差动转角");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用固定参数的无状态Stanley控制器。
|
||||
/// </summary>
|
||||
private static StanleyLateralController CreateController()
|
||||
{
|
||||
return new StanleyLateralController(
|
||||
controlPointRadiusMeters: 0.5,
|
||||
crossTrackGainPerSecond: 1.0,
|
||||
headingErrorGain: 1.0,
|
||||
minimumSpeedMetersPerSecond: 0.05,
|
||||
useActualSpeedForGain: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建只包含本次符号测试所需字段的轨迹跟踪上下文。
|
||||
/// </summary>
|
||||
private static PathTrackingContext CreateContext(
|
||||
double signedSpeedMetersPerSecond,
|
||||
double lateralErrorMeters,
|
||||
double headingErrorRadians,
|
||||
double feedforwardCurvaturePerMeter)
|
||||
{
|
||||
var vehicleState = new VehicleState(
|
||||
sampleTimestampSeconds: 0.0,
|
||||
poseInWorld: Pose2D.Identity,
|
||||
twistInWorld: new Twist2D(
|
||||
signedSpeedMetersPerSecond,
|
||||
0.0,
|
||||
0.0),
|
||||
hasValidVelocityEstimate: true);
|
||||
var referencePoint = new TrajectoryPoint(
|
||||
arcLengthMeters: 0.0,
|
||||
poseInWorld: Pose2D.Identity,
|
||||
curvaturePerMeter: 0.0,
|
||||
referenceSpeedMetersPerSecond:
|
||||
signedSpeedMetersPerSecond);
|
||||
var projection = new TrajectoryProjection(
|
||||
segmentStartIndex: 0,
|
||||
referencePoint: referencePoint,
|
||||
lateralErrorMeters: lateralErrorMeters,
|
||||
headingErrorRadians: headingErrorRadians,
|
||||
distanceToTrajectoryMeters:
|
||||
Math.Abs(lateralErrorMeters),
|
||||
remainingDistanceMeters: 1.0);
|
||||
|
||||
return new PathTrackingContext(
|
||||
vehicleState,
|
||||
projection,
|
||||
signedSpeedMetersPerSecond,
|
||||
feedforwardCurvaturePerMeter,
|
||||
deltaTimeSeconds: 0.02);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证实际值与预期符号一致。
|
||||
/// </summary>
|
||||
private static void AssertSameSign(
|
||||
double actualValue,
|
||||
double expectedSign,
|
||||
string valueName)
|
||||
{
|
||||
AssertTrue(
|
||||
Math.Sign(actualValue) ==
|
||||
Math.Sign(expectedSign),
|
||||
$"{valueName}方向错误：actual={actualValue:F6}，" +
|
||||
$"expectedSign={expectedSign:F0}。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 条件不成立时抛出异常，使测试进程以非零状态结束。
|
||||
/// </summary>
|
||||
private static void AssertTrue(
|
||||
bool condition,
|
||||
string failureMessage)
|
||||
{
|
||||
if (!condition)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
failureMessage);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user