feat: apply ST stop constraints only at real boundaries
This commit is contained in:
梁薄云
@@ -19,6 +19,7 @@ internal static class LongitudinalModelChecks
|
||||
VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
|
||||
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
|
||||
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
|
||||
VerifiesModeSpecificSolutionValidation();
|
||||
}
|
||||
|
||||
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
|
||||
@@ -313,6 +314,17 @@ internal static class LongitudinalModelChecks
|
||||
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
|
||||
integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
|
||||
var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
||||
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
||||
EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput,
|
||||
out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus,
|
||||
"unit-scale rolling speed envelope: " + rollingSpeedFailure);
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(
|
||||
rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure),
|
||||
"rolling ST QP builds: " + rollingFailure);
|
||||
Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)),
|
||||
"normalized speed and previous-U P coefficient");
|
||||
Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)),
|
||||
@@ -344,10 +356,29 @@ internal static class LongitudinalModelChecks
|
||||
"exact initial U");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
|
||||
"exact initial A");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(4), 1d } }, 2d),
|
||||
"exact terminal S");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
||||
"exact terminal U");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
|
||||
"rolling has no exact terminal S");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
||||
"rolling has no exact terminal U");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
|
||||
"rolling has no exact terminal A");
|
||||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
|
||||
for (int index = stabilizationStart; index < layout.KnotCount; index++)
|
||||
{
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.S(index), 1d } }, input.StopBoundaryPathS),
|
||||
"stop tail exact S " + index);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
|
||||
"stop tail exact U " + index);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
|
||||
"stop tail exact A " + index);
|
||||
}
|
||||
Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.S(1), 1d }, { layout.S(0), -1d },
|
||||
@@ -367,6 +398,62 @@ internal static class LongitudinalModelChecks
|
||||
}, 0d), "exact ST progress equation");
|
||||
}
|
||||
|
||||
private static void VerifiesModeSpecificSolutionValidation()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateTaskFourConfiguration();
|
||||
IReadOnlyList<double> exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d);
|
||||
LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate(
|
||||
exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d });
|
||||
LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]);
|
||||
var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact,
|
||||
out PathSpeedLimit rollingEnvelope, out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure);
|
||||
|
||||
var validator = new LongitudinalSolutionValidator();
|
||||
Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact,
|
||||
out _, out string exactFailure), "exact stops reject a nonstationary internal tail");
|
||||
Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0,
|
||||
"exact-stop failure identifies the stabilized tail: " + exactFailure);
|
||||
|
||||
LateralPath rollingPath = CreateStraightPath(0.10d);
|
||||
var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure);
|
||||
LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate(
|
||||
exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d });
|
||||
Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate,
|
||||
out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure);
|
||||
|
||||
EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration();
|
||||
approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d;
|
||||
approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
IReadOnlyList<double> approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d);
|
||||
LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate(
|
||||
approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d });
|
||||
LateralPath approachPath = CreateStraightPath(0.035d);
|
||||
var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach,
|
||||
out PathSpeedLimit approachEnvelope, out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure);
|
||||
Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach,
|
||||
out _, out string approachFailure), "approach candidates outside the stoppable set are rejected");
|
||||
Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0,
|
||||
"approach failure identifies the jerk-limited stoppable set");
|
||||
}
|
||||
|
||||
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(fixtures.Count);
|
||||
@@ -412,6 +499,30 @@ internal static class LongitudinalModelChecks
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateTaskFourConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.30d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static LateralPath CreateStraightPath(double pathUpperBoundS)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
|
||||
{
|
||||
LongitudinalConfiguration limits = input.Configuration.Longitudinal;
|
||||
|
||||
Reference in New Issue
Block a user