泛化轨迹工厂参数并新增组合运动计划执行器与复合测试
This commit is contained in:
@@ -21,10 +21,33 @@ namespace MultiWheelC
|
||||
double accelerationMetersPerSecondSquared = 0.20,
|
||||
double decelerationMetersPerSecondSquared = 0.20,
|
||||
double pointSpacingMeters = 0.02)
|
||||
{
|
||||
return CreateStraight(
|
||||
startPoseInWorld,
|
||||
StraightLengthMeters,
|
||||
cruiseSpeedMetersPerSecond,
|
||||
accelerationMetersPerSecondSquared,
|
||||
decelerationMetersPerSecondSquared,
|
||||
pointSpacingMeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
|
||||
/// </summary>
|
||||
public static Trajectory2D CreateStraight(
|
||||
Pose2D startPoseInWorld,
|
||||
double lengthMeters,
|
||||
double cruiseSpeedMetersPerSecond = 0.30,
|
||||
double accelerationMetersPerSecondSquared = 0.20,
|
||||
double decelerationMetersPerSecondSquared = 0.20,
|
||||
double pointSpacingMeters = 0.02)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
startPoseInWorld,
|
||||
nameof(startPoseInWorld));
|
||||
EnsureFinitePositive(
|
||||
lengthMeters,
|
||||
nameof(lengthMeters));
|
||||
EnsureFinitePositive(
|
||||
cruiseSpeedMetersPerSecond,
|
||||
nameof(cruiseSpeedMetersPerSecond));
|
||||
@@ -38,7 +61,7 @@ namespace MultiWheelC
|
||||
pointSpacingMeters,
|
||||
nameof(pointSpacingMeters));
|
||||
|
||||
if (pointSpacingMeters > StraightLengthMeters)
|
||||
if (pointSpacingMeters > lengthMeters)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(pointSpacingMeters),
|
||||
@@ -46,7 +69,7 @@ namespace MultiWheelC
|
||||
}
|
||||
|
||||
var segmentCount = (int)Math.Ceiling(
|
||||
StraightLengthMeters /
|
||||
lengthMeters /
|
||||
pointSpacingMeters);
|
||||
var points = new List<TrajectoryPoint>(
|
||||
segmentCount + 1);
|
||||
@@ -59,13 +82,13 @@ namespace MultiWheelC
|
||||
index <= segmentCount;
|
||||
index++)
|
||||
{
|
||||
// 均分后最后一个点严格落在4m终点，避免浮点累加越界。
|
||||
// 均分后最后一个点严格落在指定终点，避免浮点累加越界。
|
||||
var arcLengthMeters =
|
||||
StraightLengthMeters *
|
||||
lengthMeters *
|
||||
index /
|
||||
segmentCount;
|
||||
var remainingDistanceMeters =
|
||||
StraightLengthMeters -
|
||||
lengthMeters -
|
||||
arcLengthMeters;
|
||||
var referenceSpeedMetersPerSecond =
|
||||
CalculateReferenceSpeed(
|
||||
@@ -105,6 +128,34 @@ namespace MultiWheelC
|
||||
double accelerationMetersPerSecondSquared = 0.20,
|
||||
double decelerationMetersPerSecondSquared = 0.12,
|
||||
double pointSpacingMeters = 0.02)
|
||||
{
|
||||
return CreateStraightSmoothLeftTurnStraight(
|
||||
startPoseInWorld,
|
||||
straightLengthMeters,
|
||||
turnRadiusMeters,
|
||||
Math.PI,
|
||||
curvatureTransitionLengthMeters,
|
||||
straightMaximumSpeedMetersPerSecond,
|
||||
semicircleMaximumSpeedMetersPerSecond,
|
||||
accelerationMetersPerSecondSquared,
|
||||
decelerationMetersPerSecondSquared,
|
||||
pointSpacingMeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
|
||||
/// </summary>
|
||||
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
|
||||
Pose2D startPoseInWorld,
|
||||
double straightLengthMeters,
|
||||
double turnRadiusMeters,
|
||||
double turnAngleRadians,
|
||||
double curvatureTransitionLengthMeters,
|
||||
double straightMaximumSpeedMetersPerSecond,
|
||||
double turnMaximumSpeedMetersPerSecond,
|
||||
double accelerationMetersPerSecondSquared,
|
||||
double decelerationMetersPerSecondSquared,
|
||||
double pointSpacingMeters)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
startPoseInWorld,
|
||||
@@ -115,6 +166,9 @@ namespace MultiWheelC
|
||||
EnsureFinitePositive(
|
||||
turnRadiusMeters,
|
||||
nameof(turnRadiusMeters));
|
||||
EnsureFinitePositive(
|
||||
turnAngleRadians,
|
||||
nameof(turnAngleRadians));
|
||||
EnsureFinitePositive(
|
||||
curvatureTransitionLengthMeters,
|
||||
nameof(curvatureTransitionLengthMeters));
|
||||
@@ -122,8 +176,8 @@ namespace MultiWheelC
|
||||
straightMaximumSpeedMetersPerSecond,
|
||||
nameof(straightMaximumSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
semicircleMaximumSpeedMetersPerSecond,
|
||||
nameof(semicircleMaximumSpeedMetersPerSecond));
|
||||
turnMaximumSpeedMetersPerSecond,
|
||||
nameof(turnMaximumSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
accelerationMetersPerSecondSquared,
|
||||
nameof(accelerationMetersPerSecondSquared));
|
||||
@@ -134,21 +188,28 @@ namespace MultiWheelC
|
||||
pointSpacingMeters,
|
||||
nameof(pointSpacingMeters));
|
||||
|
||||
var originalSemicircleLengthMeters =
|
||||
Math.PI * turnRadiusMeters;
|
||||
if (turnAngleRadians > 2.0 * Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(turnAngleRadians),
|
||||
"单段平滑左转角度不能大于2π。");
|
||||
}
|
||||
|
||||
var nominalTurnArcLengthMeters =
|
||||
turnAngleRadians * turnRadiusMeters;
|
||||
var constantCurvatureLengthMeters =
|
||||
originalSemicircleLengthMeters -
|
||||
nominalTurnArcLengthMeters -
|
||||
curvatureTransitionLengthMeters;
|
||||
|
||||
if (constantCurvatureLengthMeters <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(curvatureTransitionLengthMeters),
|
||||
"曲率过渡段长度必须小于半径对应的原始半圆弧长。");
|
||||
"曲率过渡段长度必须小于指定转角对应的圆弧长度。");
|
||||
}
|
||||
|
||||
// 两段平滑过渡的平均曲率均为最大曲率的一半；
|
||||
// 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于π。
|
||||
// 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
|
||||
var turnLengthMeters =
|
||||
2.0 * curvatureTransitionLengthMeters +
|
||||
constantCurvatureLengthMeters;
|
||||
@@ -184,7 +245,7 @@ namespace MultiWheelC
|
||||
turnStartArcLengthMeters &&
|
||||
arcLengthMeters <=
|
||||
turnEndArcLengthMeters
|
||||
? semicircleMaximumSpeedMetersPerSecond
|
||||
? turnMaximumSpeedMetersPerSecond
|
||||
: straightMaximumSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
@@ -341,7 +402,7 @@ namespace MultiWheelC
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算180°左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
|
||||
/// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
|
||||
/// </summary>
|
||||
private static double CalculateSmoothTurnCurvature(
|
||||
double distanceInTurnMeters,
|
||||
|
||||
Reference in New Issue
Block a user