First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

新增倒车以及项目结构优化

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-11 17:06:26 +08:00
parent a59499e638
commit 33a33af710
41 changed files with 817 additions and 2654 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/TestTrajectoryFactory.cs
+78 -57
View File
@@ -13,7 +13,7 @@ namespace MultiWheelC
private const double StraightLengthMeters = 4.0;
/// <summary>
/// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。
/// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的4m直线轨迹。
/// </summary>
public static Trajectory2D CreateStraight4Meters(
Pose2D startPoseInWorld,
@@ -32,7 +32,7 @@ namespace MultiWheelC
}
/// <summary>
/// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
/// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
/// </summary>
public static Trajectory2D CreateStraight(
Pose2D startPoseInWorld,
@@ -42,22 +42,22 @@ namespace MultiWheelC
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
{
EnsureFinitePose(
NumericGuard.EnsureFinite(
startPoseInWorld,
nameof(startPoseInWorld));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
lengthMeters,
nameof(lengthMeters));
EnsureFinitePositive(
var travelDirection = GetTravelDirection(
cruiseSpeedMetersPerSecond,
nameof(cruiseSpeedMetersPerSecond));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
decelerationMetersPerSecondSquared,
nameof(decelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
@@ -73,10 +73,10 @@ namespace MultiWheelC
pointSpacingMeters);
var points = new List<TrajectoryPoint>(
segmentCount + 1);
var directionX = Math.Cos(
startPoseInWorld.YawRadians);
var directionY = Math.Sin(
startPoseInWorld.YawRadians);
var directionX = travelDirection *
Math.Cos(startPoseInWorld.YawRadians);
var directionY = travelDirection *
Math.Sin(startPoseInWorld.YawRadians);
for (var index = 0;
index <= segmentCount;
@@ -116,7 +116,7 @@ namespace MultiWheelC
}
/// <summary>
/// 从当前位姿生成“3m直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
/// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
/// </summary>
public static Trajectory2D CreateStraightLeftSemicircleStraight(
Pose2D startPoseInWorld,
@@ -143,7 +143,7 @@ namespace MultiWheelC
}
/// <summary>
/// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
/// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
/// </summary>
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
Pose2D startPoseInWorld,
@@ -157,34 +157,33 @@ namespace MultiWheelC
double decelerationMetersPerSecondSquared,
double pointSpacingMeters)
{
EnsureFinitePose(
NumericGuard.EnsureFinite(
startPoseInWorld,
nameof(startPoseInWorld));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
straightLengthMeters,
nameof(straightLengthMeters));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
turnRadiusMeters,
nameof(turnRadiusMeters));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
turnAngleRadians,
nameof(turnAngleRadians));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
curvatureTransitionLengthMeters,
nameof(curvatureTransitionLengthMeters));
EnsureFinitePositive(
var travelDirection = GetCommonTravelDirection(
straightMaximumSpeedMetersPerSecond,
nameof(straightMaximumSpeedMetersPerSecond));
EnsureFinitePositive(
nameof(straightMaximumSpeedMetersPerSecond),
turnMaximumSpeedMetersPerSecond,
nameof(turnMaximumSpeedMetersPerSecond));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
decelerationMetersPerSecondSquared,
nameof(decelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
@@ -245,8 +244,10 @@ namespace MultiWheelC
turnStartArcLengthMeters &&
arcLengthMeters <=
turnEndArcLengthMeters
? turnMaximumSpeedMetersPerSecond
: straightMaximumSpeedMetersPerSecond;
? Math.Abs(
turnMaximumSpeedMetersPerSecond)
: Math.Abs(
straightMaximumSpeedMetersPerSecond);
}
ApplyAccelerationAndBrakingLimits(
@@ -256,6 +257,14 @@ namespace MultiWheelC
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared);
for (var index = 0;
index < referenceSpeeds.Length;
index++)
{
referenceSpeeds[index] *=
travelDirection;
}
var points = new List<TrajectoryPoint>(
sampleArcLengths.Count);
var startCos = Math.Cos(
@@ -296,10 +305,10 @@ namespace MultiWheelC
if (Math.Abs(segmentCurvaturePerMeter) <=
1e-12)
{
localX +=
localX += travelDirection *
Math.Cos(localYawRadians) *
segmentLengthMeters;
localY +=
localY += travelDirection *
Math.Sin(localYawRadians) *
segmentLengthMeters;
}
@@ -308,11 +317,11 @@ namespace MultiWheelC
var nextYawRadians =
localYawRadians +
segmentYawChangeRadians;
localX +=
localX += travelDirection *
(Math.Sin(nextYawRadians) -
Math.Sin(localYawRadians)) /
segmentCurvaturePerMeter;
localY +=
localY += travelDirection *
(Math.Cos(localYawRadians) -
Math.Cos(nextYawRadians)) /
segmentCurvaturePerMeter;
@@ -490,7 +499,7 @@ namespace MultiWheelC
}
/// <summary>
/// 对逐点速度上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
/// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
/// </summary>
private static void ApplyAccelerationAndBrakingLimits(
IReadOnlyList<double> arcLengthsMeters,
@@ -547,7 +556,7 @@ namespace MultiWheelC
}
/// <summary>
/// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
/// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
/// </summary>
private static double CalculateReferenceSpeed(
double arcLengthMeters,
@@ -566,52 +575,64 @@ namespace MultiWheelC
decelerationMetersPerSecondSquared *
Math.Max(0.0, remainingDistanceMeters));
return Math.Min(
var travelDirection = GetTravelDirection(
cruiseSpeedMetersPerSecond,
nameof(cruiseSpeedMetersPerSecond));
var speedMagnitude = Math.Min(
Math.Abs(cruiseSpeedMetersPerSecond),
Math.Min(
accelerationLimitedSpeed,
brakingLimitedSpeed));
return travelDirection * speedMagnitude;
}
/// <summary>
/// 检查世界坐标系起点位姿是否全部为有限值。
/// 获取非零有符号速度表示的前进或倒车方向。
/// </summary>
private static void EnsureFinitePose(
Pose2D pose,
private static double GetTravelDirection(
double signedSpeedMetersPerSecond,
string parameterName)
{
if (!IsFinite(pose.XMeters) ||
!IsFinite(pose.YMeters) ||
!IsFinite(pose.YawRadians))
NumericGuard.EnsureFinite(
signedSpeedMetersPerSecond,
parameterName);
if (signedSpeedMetersPerSecond == 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"直线测试轨迹的起点位姿必须由有限值组成。");
"测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
}
return Math.Sign(
signedSpeedMetersPerSecond);
}
/// <summary>
/// 检查测试轨迹参数是否为正有限值。
/// 确保直线段和转弯段速度使用相同的前进或倒车方向。
/// </summary>
private static void EnsureFinitePositive(
double value,
string parameterName)
private static double GetCommonTravelDirection(
double firstSpeedMetersPerSecond,
string firstParameterName,
double secondSpeedMetersPerSecond,
string secondParameterName)
{
if (!IsFinite(value) || value <= 0.0)
var firstDirection = GetTravelDirection(
firstSpeedMetersPerSecond,
firstParameterName);
var secondDirection = GetTravelDirection(
secondSpeedMetersPerSecond,
secondParameterName);
if (firstDirection != secondDirection)
{
throw new ArgumentOutOfRangeException(
parameterName,
"直线测试轨迹的速度、加速度和点间距必须是正有限值。");
throw new ArgumentException(
"同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
firstParameterName);
}
}
/// <summary>
/// 判断数值是否可用于轨迹计算。
/// </summary>
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
return firstDirection;
}
}
}