添加直线-半圆组合轨迹测试并调优Stanley参数与夹臂限位类型
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -92,6 +92,399 @@ namespace MultiWheelC
|
||||
return new Trajectory2D(points);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前位姿生成“3m直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
|
||||
/// </summary>
|
||||
public static Trajectory2D CreateStraightLeftSemicircleStraight(
|
||||
Pose2D startPoseInWorld,
|
||||
double straightLengthMeters = 3.0,
|
||||
double turnRadiusMeters = 2.0,
|
||||
double curvatureTransitionLengthMeters = 0.60,
|
||||
double straightMaximumSpeedMetersPerSecond = 0.30,
|
||||
double semicircleMaximumSpeedMetersPerSecond = 0.25,
|
||||
double accelerationMetersPerSecondSquared = 0.20,
|
||||
double decelerationMetersPerSecondSquared = 0.12,
|
||||
double pointSpacingMeters = 0.02)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
startPoseInWorld,
|
||||
nameof(startPoseInWorld));
|
||||
EnsureFinitePositive(
|
||||
straightLengthMeters,
|
||||
nameof(straightLengthMeters));
|
||||
EnsureFinitePositive(
|
||||
turnRadiusMeters,
|
||||
nameof(turnRadiusMeters));
|
||||
EnsureFinitePositive(
|
||||
curvatureTransitionLengthMeters,
|
||||
nameof(curvatureTransitionLengthMeters));
|
||||
EnsureFinitePositive(
|
||||
straightMaximumSpeedMetersPerSecond,
|
||||
nameof(straightMaximumSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
semicircleMaximumSpeedMetersPerSecond,
|
||||
nameof(semicircleMaximumSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
accelerationMetersPerSecondSquared,
|
||||
nameof(accelerationMetersPerSecondSquared));
|
||||
EnsureFinitePositive(
|
||||
decelerationMetersPerSecondSquared,
|
||||
nameof(decelerationMetersPerSecondSquared));
|
||||
EnsureFinitePositive(
|
||||
pointSpacingMeters,
|
||||
nameof(pointSpacingMeters));
|
||||
|
||||
var originalSemicircleLengthMeters =
|
||||
Math.PI * turnRadiusMeters;
|
||||
var constantCurvatureLengthMeters =
|
||||
originalSemicircleLengthMeters -
|
||||
curvatureTransitionLengthMeters;
|
||||
|
||||
if (constantCurvatureLengthMeters <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(curvatureTransitionLengthMeters),
|
||||
"曲率过渡段长度必须小于半径对应的原始半圆弧长。");
|
||||
}
|
||||
|
||||
// 两段平滑过渡的平均曲率均为最大曲率的一半；
|
||||
// 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于π。
|
||||
var turnLengthMeters =
|
||||
2.0 * curvatureTransitionLengthMeters +
|
||||
constantCurvatureLengthMeters;
|
||||
var turnStartArcLengthMeters =
|
||||
straightLengthMeters;
|
||||
var turnEndArcLengthMeters =
|
||||
straightLengthMeters +
|
||||
turnLengthMeters;
|
||||
var maximumCurvaturePerMeter =
|
||||
1.0 / turnRadiusMeters;
|
||||
|
||||
var sampleArcLengths =
|
||||
BuildCompositeArcLengthSamples(
|
||||
straightLengthMeters,
|
||||
curvatureTransitionLengthMeters,
|
||||
constantCurvatureLengthMeters,
|
||||
pointSpacingMeters);
|
||||
var speedLimits = new double[
|
||||
sampleArcLengths.Count];
|
||||
var referenceSpeeds = new double[
|
||||
sampleArcLengths.Count];
|
||||
|
||||
for (var index = 0;
|
||||
index < sampleArcLengths.Count;
|
||||
index++)
|
||||
{
|
||||
var arcLengthMeters =
|
||||
sampleArcLengths[index];
|
||||
|
||||
// 整个转弯及两侧曲率过渡段采用转弯限速。
|
||||
speedLimits[index] =
|
||||
arcLengthMeters >=
|
||||
turnStartArcLengthMeters &&
|
||||
arcLengthMeters <=
|
||||
turnEndArcLengthMeters
|
||||
? semicircleMaximumSpeedMetersPerSecond
|
||||
: straightMaximumSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
ApplyAccelerationAndBrakingLimits(
|
||||
sampleArcLengths,
|
||||
speedLimits,
|
||||
referenceSpeeds,
|
||||
accelerationMetersPerSecondSquared,
|
||||
decelerationMetersPerSecondSquared);
|
||||
|
||||
var points = new List<TrajectoryPoint>(
|
||||
sampleArcLengths.Count);
|
||||
var startCos = Math.Cos(
|
||||
startPoseInWorld.YawRadians);
|
||||
var startSin = Math.Sin(
|
||||
startPoseInWorld.YawRadians);
|
||||
var localX = 0.0;
|
||||
var localY = 0.0;
|
||||
var localYawRadians = 0.0;
|
||||
var previousArcLengthMeters = 0.0;
|
||||
|
||||
for (var index = 0;
|
||||
index < sampleArcLengths.Count;
|
||||
index++)
|
||||
{
|
||||
var arcLengthMeters =
|
||||
sampleArcLengths[index];
|
||||
if (index > 0)
|
||||
{
|
||||
var segmentLengthMeters =
|
||||
arcLengthMeters -
|
||||
previousArcLengthMeters;
|
||||
var segmentMiddleArcLengthMeters =
|
||||
(arcLengthMeters +
|
||||
previousArcLengthMeters) /
|
||||
2.0;
|
||||
var segmentCurvaturePerMeter =
|
||||
CalculateSmoothTurnCurvature(
|
||||
segmentMiddleArcLengthMeters -
|
||||
turnStartArcLengthMeters,
|
||||
curvatureTransitionLengthMeters,
|
||||
constantCurvatureLengthMeters,
|
||||
maximumCurvaturePerMeter);
|
||||
var segmentYawChangeRadians =
|
||||
segmentCurvaturePerMeter *
|
||||
segmentLengthMeters;
|
||||
|
||||
if (Math.Abs(segmentCurvaturePerMeter) <=
|
||||
1e-12)
|
||||
{
|
||||
localX +=
|
||||
Math.Cos(localYawRadians) *
|
||||
segmentLengthMeters;
|
||||
localY +=
|
||||
Math.Sin(localYawRadians) *
|
||||
segmentLengthMeters;
|
||||
}
|
||||
else
|
||||
{
|
||||
var nextYawRadians =
|
||||
localYawRadians +
|
||||
segmentYawChangeRadians;
|
||||
localX +=
|
||||
(Math.Sin(nextYawRadians) -
|
||||
Math.Sin(localYawRadians)) /
|
||||
segmentCurvaturePerMeter;
|
||||
localY +=
|
||||
(Math.Cos(localYawRadians) -
|
||||
Math.Cos(nextYawRadians)) /
|
||||
segmentCurvaturePerMeter;
|
||||
}
|
||||
|
||||
localYawRadians +=
|
||||
segmentYawChangeRadians;
|
||||
}
|
||||
|
||||
var curvaturePerMeter =
|
||||
CalculateSmoothTurnCurvature(
|
||||
arcLengthMeters -
|
||||
turnStartArcLengthMeters,
|
||||
curvatureTransitionLengthMeters,
|
||||
constantCurvatureLengthMeters,
|
||||
maximumCurvaturePerMeter);
|
||||
|
||||
var worldX =
|
||||
startPoseInWorld.XMeters +
|
||||
startCos * localX -
|
||||
startSin * localY;
|
||||
var worldY =
|
||||
startPoseInWorld.YMeters +
|
||||
startSin * localX +
|
||||
startCos * localY;
|
||||
var worldYawRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
startPoseInWorld.YawRadians +
|
||||
localYawRadians);
|
||||
|
||||
points.Add(
|
||||
new TrajectoryPoint(
|
||||
arcLengthMeters,
|
||||
new Pose2D(
|
||||
worldX,
|
||||
worldY,
|
||||
worldYawRadians),
|
||||
curvaturePerMeter,
|
||||
referenceSpeeds[index]));
|
||||
|
||||
previousArcLengthMeters =
|
||||
arcLengthMeters;
|
||||
}
|
||||
|
||||
return new Trajectory2D(points);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 分别采样直线、入弯过渡、等曲率段和出弯过渡，保证所有边界均为精确轨迹点。
|
||||
/// </summary>
|
||||
private static List<double> BuildCompositeArcLengthSamples(
|
||||
double straightLengthMeters,
|
||||
double curvatureTransitionLengthMeters,
|
||||
double constantCurvatureLengthMeters,
|
||||
double pointSpacingMeters)
|
||||
{
|
||||
var samples = new List<double> { 0.0 };
|
||||
var accumulatedArcLengthMeters = 0.0;
|
||||
|
||||
AppendSectionArcLengthSamples(
|
||||
samples,
|
||||
ref accumulatedArcLengthMeters,
|
||||
straightLengthMeters,
|
||||
pointSpacingMeters);
|
||||
AppendSectionArcLengthSamples(
|
||||
samples,
|
||||
ref accumulatedArcLengthMeters,
|
||||
curvatureTransitionLengthMeters,
|
||||
pointSpacingMeters);
|
||||
AppendSectionArcLengthSamples(
|
||||
samples,
|
||||
ref accumulatedArcLengthMeters,
|
||||
constantCurvatureLengthMeters,
|
||||
pointSpacingMeters);
|
||||
AppendSectionArcLengthSamples(
|
||||
samples,
|
||||
ref accumulatedArcLengthMeters,
|
||||
curvatureTransitionLengthMeters,
|
||||
pointSpacingMeters);
|
||||
AppendSectionArcLengthSamples(
|
||||
samples,
|
||||
ref accumulatedArcLengthMeters,
|
||||
straightLengthMeters,
|
||||
pointSpacingMeters);
|
||||
|
||||
return samples;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算180°左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
|
||||
/// </summary>
|
||||
private static double CalculateSmoothTurnCurvature(
|
||||
double distanceInTurnMeters,
|
||||
double transitionLengthMeters,
|
||||
double constantCurvatureLengthMeters,
|
||||
double maximumCurvaturePerMeter)
|
||||
{
|
||||
var totalTurnLengthMeters =
|
||||
2.0 * transitionLengthMeters +
|
||||
constantCurvatureLengthMeters;
|
||||
|
||||
if (distanceInTurnMeters <= 0.0 ||
|
||||
distanceInTurnMeters >= totalTurnLengthMeters)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (distanceInTurnMeters < transitionLengthMeters)
|
||||
{
|
||||
return maximumCurvaturePerMeter *
|
||||
SmoothStep01(
|
||||
distanceInTurnMeters /
|
||||
transitionLengthMeters);
|
||||
}
|
||||
|
||||
var exitTransitionStartMeters =
|
||||
transitionLengthMeters +
|
||||
constantCurvatureLengthMeters;
|
||||
if (distanceInTurnMeters <=
|
||||
exitTransitionStartMeters)
|
||||
{
|
||||
return maximumCurvaturePerMeter;
|
||||
}
|
||||
|
||||
var exitRatio =
|
||||
(distanceInTurnMeters -
|
||||
exitTransitionStartMeters) /
|
||||
transitionLengthMeters;
|
||||
return maximumCurvaturePerMeter *
|
||||
(1.0 - SmoothStep01(exitRatio));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将零到一的比例转换为两端一阶导数均为零的三次平滑比例。
|
||||
/// </summary>
|
||||
private static double SmoothStep01(double ratio)
|
||||
{
|
||||
var limitedRatio = Math.Max(
|
||||
0.0,
|
||||
Math.Min(1.0, ratio));
|
||||
return limitedRatio *
|
||||
limitedRatio *
|
||||
(3.0 - 2.0 * limitedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将一段指定长度的轨迹追加为均匀弧长采样，并使最后一个点严格落在该段终点。
|
||||
/// </summary>
|
||||
private static void AppendSectionArcLengthSamples(
|
||||
ICollection<double> samples,
|
||||
ref double accumulatedArcLengthMeters,
|
||||
double sectionLengthMeters,
|
||||
double pointSpacingMeters)
|
||||
{
|
||||
var sectionStartArcLengthMeters =
|
||||
accumulatedArcLengthMeters;
|
||||
var segmentCount = (int)Math.Ceiling(
|
||||
sectionLengthMeters /
|
||||
pointSpacingMeters);
|
||||
|
||||
for (var index = 1;
|
||||
index <= segmentCount;
|
||||
index++)
|
||||
{
|
||||
samples.Add(
|
||||
sectionStartArcLengthMeters +
|
||||
sectionLengthMeters *
|
||||
index /
|
||||
segmentCount);
|
||||
}
|
||||
|
||||
accumulatedArcLengthMeters =
|
||||
sectionStartArcLengthMeters +
|
||||
sectionLengthMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对逐点速度上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
|
||||
/// </summary>
|
||||
private static void ApplyAccelerationAndBrakingLimits(
|
||||
IReadOnlyList<double> arcLengthsMeters,
|
||||
IReadOnlyList<double> speedLimitsMetersPerSecond,
|
||||
double[] referenceSpeedsMetersPerSecond,
|
||||
double accelerationMetersPerSecondSquared,
|
||||
double decelerationMetersPerSecondSquared)
|
||||
{
|
||||
referenceSpeedsMetersPerSecond[0] = 0.0;
|
||||
|
||||
for (var index = 1;
|
||||
index < arcLengthsMeters.Count;
|
||||
index++)
|
||||
{
|
||||
var segmentLengthMeters =
|
||||
arcLengthsMeters[index] -
|
||||
arcLengthsMeters[index - 1];
|
||||
var accelerationLimitedSpeed = Math.Sqrt(
|
||||
referenceSpeedsMetersPerSecond[index - 1] *
|
||||
referenceSpeedsMetersPerSecond[index - 1] +
|
||||
2.0 *
|
||||
accelerationMetersPerSecondSquared *
|
||||
segmentLengthMeters);
|
||||
|
||||
referenceSpeedsMetersPerSecond[index] =
|
||||
Math.Min(
|
||||
speedLimitsMetersPerSecond[index],
|
||||
accelerationLimitedSpeed);
|
||||
}
|
||||
|
||||
var finalIndex =
|
||||
referenceSpeedsMetersPerSecond.Length - 1;
|
||||
referenceSpeedsMetersPerSecond[finalIndex] = 0.0;
|
||||
|
||||
for (var index = finalIndex - 1;
|
||||
index >= 0;
|
||||
index--)
|
||||
{
|
||||
var segmentLengthMeters =
|
||||
arcLengthsMeters[index + 1] -
|
||||
arcLengthsMeters[index];
|
||||
var brakingLimitedSpeed = Math.Sqrt(
|
||||
referenceSpeedsMetersPerSecond[index + 1] *
|
||||
referenceSpeedsMetersPerSecond[index + 1] +
|
||||
2.0 *
|
||||
decelerationMetersPerSecondSquared *
|
||||
segmentLengthMeters);
|
||||
|
||||
referenceSpeedsMetersPerSecond[index] =
|
||||
Math.Min(
|
||||
referenceSpeedsMetersPerSecond[index],
|
||||
brakingLimitedSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user