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/Trajectory/Trajectory2D.cs
+89 -18
View File
@@ -7,18 +7,22 @@ using MyParking.Shared;
namespace MultiWheelC.Trajectory
{
/// <summary>
/// 保存一条经过基本合法性检查的只读二维参考轨迹。
/// 保存一条按预定执行点序排列、以累计弧长参数化的只读二维参考轨迹。
/// </summary>
public sealed class Trajectory2D
{
private const double StartArcLengthToleranceMeters = 1e-9;
private const double MinimumSegmentLengthMeters = 1e-6;
private const double ArcLengthConsistencyAbsoluteToleranceMeters =
1e-6;
private const double ArcLengthConsistencyRelativeTolerance =
0.01;
private readonly TrajectoryPoint[] _points;
private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
/// <summary>
/// 复制并验证按累计弧长升序排列的参考轨迹点。
/// 复制并验证按实际执行顺序及累计弧长升序排列的参考轨迹点。
/// </summary>
public Trajectory2D(
IEnumerable<TrajectoryPoint> points)
@@ -102,7 +106,7 @@ namespace MultiWheelC.Trajectory
public double GetRemainingDistanceMeters(
double arcLengthMeters)
{
EnsureFinite(
NumericGuard.EnsureFinite(
arcLengthMeters,
nameof(arcLengthMeters));
@@ -121,7 +125,7 @@ namespace MultiWheelC.Trajectory
public TrajectoryPoint SampleAtArcLength(
double arcLengthMeters)
{
EnsureFinite(
NumericGuard.EnsureFinite(
arcLengthMeters,
nameof(arcLengthMeters));
@@ -148,6 +152,48 @@ namespace MultiWheelC.Trajectory
(segmentEnd.ArcLengthMeters -
segmentStart.ArcLengthMeters);
return InterpolateSegment(
segmentStartIndex,
interpolationRatio);
}
/// <summary>
/// 在指定线段上统一插值位置、车头航向、曲率和有符号参考速度。
/// </summary>
internal TrajectoryPoint InterpolateSegment(
int segmentStartIndex,
double interpolationRatio)
{
if (segmentStartIndex < 0 ||
segmentStartIndex >= _points.Length - 1)
{
throw new ArgumentOutOfRangeException(
nameof(segmentStartIndex),
"轨迹插值线段索引必须指向一条有效线段的起点。");
}
NumericGuard.EnsureFinite(
interpolationRatio,
nameof(interpolationRatio));
if (interpolationRatio < 0.0 ||
interpolationRatio > 1.0)
{
throw new ArgumentOutOfRangeException(
nameof(interpolationRatio),
"轨迹线段插值比例必须位于[0,1]范围内。");
}
var segmentStart =
_points[segmentStartIndex];
var segmentEnd =
_points[segmentStartIndex + 1];
var arcLengthMeters =
InterpolationMath.Lerp(
segmentStart.ArcLengthMeters,
segmentEnd.ArcLengthMeters,
interpolationRatio);
return new TrajectoryPoint(
arcLengthMeters,
new Pose2D(
@@ -176,9 +222,23 @@ namespace MultiWheelC.Trajectory
/// <summary>
/// 使用二分查找获取包含指定累计弧长的线段起点索引。
/// </summary>
private int FindSegmentStartIndex(
internal int FindSegmentStartIndex(
double arcLengthMeters)
{
NumericGuard.EnsureFinite(
arcLengthMeters,
nameof(arcLengthMeters));
if (arcLengthMeters <= 0.0)
{
return 0;
}
if (arcLengthMeters >= TotalLengthMeters)
{
return _points.Length - 2;
}
var lowerIndex = 0;
var upperIndex = _points.Length - 1;
@@ -239,21 +299,32 @@ namespace MultiWheelC.Trajectory
$"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
parameterName);
}
}
/// <summary>
/// 检查数值是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value))
var segmentLengthMeters =
Math.Sqrt(segmentLengthSquared);
var arcLengthIncrementMeters =
current.ArcLengthMeters -
previous.ArcLengthMeters;
var maximumAllowedDifferenceMeters =
Math.Max(
ArcLengthConsistencyAbsoluteToleranceMeters,
ArcLengthConsistencyRelativeTolerance *
Math.Max(
segmentLengthMeters,
arcLengthIncrementMeters));
// 当前轨迹在相邻采样点之间按直线段投影，因此累计弧长增量
// 必须与该离散线段长度近似一致，防止进度和实际几何脱节。
if (Math.Abs(
arcLengthIncrementMeters -
segmentLengthMeters) >
maximumAllowedDifferenceMeters)
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹弧长必须是有限值。");
throw new ArgumentException(
$"轨迹点{currentIndex}的累计弧长增量" +
$"{arcLengthIncrementMeters:F6}m与离散线段长度" +
$"{segmentLengthMeters:F6}m不一致。",
parameterName);
}
}
}
MultiWheelC/Trajectory/TrajectoryPoint.cs
+10 -48
View File
@@ -4,7 +4,7 @@ using MyParking.Shared;
namespace MultiWheelC.Trajectory
{
/// <summary>
/// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
/// 描述按执行点序和累计弧长参数化的车体中心参考轨迹点，统一使用SI单位。
/// </summary>
public readonly struct TrajectoryPoint
{
@@ -17,22 +17,16 @@ namespace MultiWheelC.Trajectory
double curvaturePerMeter,
double referenceSpeedMetersPerSecond)
{
EnsureFiniteNonNegative(
NumericGuard.EnsureFiniteNonNegative(
arcLengthMeters,
nameof(arcLengthMeters));
EnsureFinite(
poseInWorld.XMeters,
NumericGuard.EnsureFinite(
poseInWorld,
nameof(poseInWorld));
EnsureFinite(
poseInWorld.YMeters,
nameof(poseInWorld));
EnsureFinite(
poseInWorld.YawRadians,
nameof(poseInWorld));
EnsureFinite(
NumericGuard.EnsureFinite(
curvaturePerMeter,
nameof(curvaturePerMeter));
EnsureFinite(
NumericGuard.EnsureFinite(
referenceSpeedMetersPerSecond,
nameof(referenceSpeedMetersPerSecond));
ArcLengthMeters = arcLengthMeters;
@@ -47,56 +41,24 @@ namespace MultiWheelC.Trajectory
}
/// <summary>
/// 获取从轨迹起点累计到当前点的弧长，单位为m。
/// 获取沿预定执行点序从轨迹起点累计到当前点的弧长，单位为m。
/// </summary>
public double ArcLengthMeters { get; }
/// <summary>
/// 获取车体中心参考坐标系在世界坐标系中的位姿。
/// 获取车体中心参考坐标系在世界坐标系中的位姿；航向始终表示车头方向。
/// </summary>
public Pose2D PoseInWorld { get; }
/// <summary>
/// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
/// 获取沿累计弧长增加方向的车体中心参考轨迹曲率，单位为1/m，左弯为正。
/// </summary>
public double CurvaturePerMeter { get; }
/// <summary>
/// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
/// 获取车体纵向有符号参考速度，单位为m/s；正值前进、负值倒车、零值停车。
/// </summary>
public double ReferenceSpeedMetersPerSecond { get; }
/// <summary>
/// 检查数值是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹点参数必须是有限值。");
}
}
/// <summary>
/// 检查数值是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
EnsureFinite(value, parameterName);
if (value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹累计弧长不能为负数。");
}
}
}
}
MultiWheelC/Trajectory/TrajectoryProjection.cs
+5 -37
View File
@@ -26,16 +26,16 @@ namespace MultiWheelC.Trajectory
"投影线段起点索引不能为负数。");
}
EnsureFinite(
NumericGuard.EnsureFinite(
lateralErrorMeters,
nameof(lateralErrorMeters));
EnsureFinite(
NumericGuard.EnsureFinite(
headingErrorRadians,
nameof(headingErrorRadians));
EnsureFiniteNonNegative(
NumericGuard.EnsureFiniteNonNegative(
distanceToTrajectoryMeters,
nameof(distanceToTrajectoryMeters));
EnsureFiniteNonNegative(
NumericGuard.EnsureFiniteNonNegative(
remainingDistanceMeters,
nameof(remainingDistanceMeters));
@@ -62,7 +62,7 @@ namespace MultiWheelC.Trajectory
public TrajectoryPoint ReferencePoint { get; }
/// <summary>
/// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
/// 获取相对累计弧长增加方向的有符号横向误差，单位为m，参考轨迹位于该方向左侧时为正。
/// </summary>
public double LateralErrorMeters { get; }
@@ -87,37 +87,5 @@ namespace MultiWheelC.Trajectory
public double ArcLengthMeters =>
ReferencePoint.ArcLengthMeters;
/// <summary>
/// 检查数值是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹投影参数必须是有限值。");
}
}
/// <summary>
/// 检查数值是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
EnsureFinite(value, parameterName);
if (value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹投影距离不能为负数。");
}
}
}
}
MultiWheelC/Trajectory/TrajectoryProjector.cs
+131 -64
View File
@@ -4,10 +4,13 @@ using MyParking.Shared;
namespace MultiWheelC.Trajectory
{
/// <summary>
/// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
/// 将实际车体中心位姿投影到二维离散参考轨迹，并支持按上次进度限制搜索范围。
/// </summary>
public static class TrajectoryProjector
{
private const double DistanceTieToleranceSquaredMeters =
1e-12;
/// <summary>
/// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
/// </summary>
@@ -15,25 +18,98 @@ namespace MultiWheelC.Trajectory
Trajectory2D trajectory,
Pose2D vehiclePoseInWorld)
{
if (trajectory == null)
{
throw new ArgumentNullException(
nameof(trajectory));
}
EnsureFinitePose(
ValidateProjectionInput(
trajectory,
vehiclePoseInWorld,
nameof(vehiclePoseInWorld));
return ProjectRange(
trajectory,
vehiclePoseInWorld,
firstSegmentStartIndex: 0,
lastSegmentStartIndex:
trajectory.Count - 2,
preferredArcLengthMeters: null);
}
/// <summary>
/// 以上次投影弧长为中心，仅在指定前后物理距离窗口内查找最近线段。
/// </summary>
public static TrajectoryProjection Project(
Trajectory2D trajectory,
Pose2D vehiclePoseInWorld,
double previousArcLengthMeters,
double maximumBackwardSearchDistanceMeters,
double maximumForwardSearchDistanceMeters)
{
ValidateProjectionInput(
trajectory,
vehiclePoseInWorld,
nameof(vehiclePoseInWorld));
NumericGuard.EnsureFiniteNonNegative(
previousArcLengthMeters,
nameof(previousArcLengthMeters));
NumericGuard.EnsureFiniteNonNegative(
maximumBackwardSearchDistanceMeters,
nameof(maximumBackwardSearchDistanceMeters));
NumericGuard.EnsureFinitePositive(
maximumForwardSearchDistanceMeters,
nameof(maximumForwardSearchDistanceMeters));
if (previousArcLengthMeters >
trajectory.TotalLengthMeters)
{
throw new ArgumentOutOfRangeException(
nameof(previousArcLengthMeters),
"上次投影弧长不能超过轨迹总长度。");
}
var searchStartArcLengthMeters =
Math.Max(
0.0,
previousArcLengthMeters -
maximumBackwardSearchDistanceMeters);
var searchEndArcLengthMeters =
Math.Min(
trajectory.TotalLengthMeters,
previousArcLengthMeters +
maximumForwardSearchDistanceMeters);
var firstSegmentStartIndex =
trajectory.FindSegmentStartIndex(
searchStartArcLengthMeters);
var lastSegmentStartIndex =
trajectory.FindSegmentStartIndex(
searchEndArcLengthMeters);
return ProjectRange(
trajectory,
vehiclePoseInWorld,
firstSegmentStartIndex,
lastSegmentStartIndex,
previousArcLengthMeters);
}
/// <summary>
/// 在闭区间线段索引范围内查找最近投影，并在距离并列时优先保持原进度。
/// </summary>
private static TrajectoryProjection ProjectRange(
Trajectory2D trajectory,
Pose2D vehiclePoseInWorld,
int firstSegmentStartIndex,
int lastSegmentStartIndex,
double? preferredArcLengthMeters)
{
var bestSegmentStartIndex = 0;
var bestInterpolationRatio = 0.0;
var bestProjectedX = 0.0;
var bestProjectedY = 0.0;
var bestDistanceSquared =
double.PositiveInfinity;
var bestProgressDifferenceMeters =
double.PositiveInfinity;
for (var segmentStartIndex = 0;
segmentStartIndex < trajectory.Count - 1;
for (var segmentStartIndex =
firstSegmentStartIndex;
segmentStartIndex <=
lastSegmentStartIndex;
segmentStartIndex++)
{
var segmentStart =
@@ -85,7 +161,30 @@ namespace MultiWheelC.Trajectory
projectionErrorX * projectionErrorX +
projectionErrorY * projectionErrorY;
if (distanceSquared >= bestDistanceSquared)
var progressDifferenceMeters =
preferredArcLengthMeters.HasValue
? Math.Abs(
InterpolationMath.Lerp(
segmentStart.ArcLengthMeters,
segmentEnd.ArcLengthMeters,
interpolationRatio) -
preferredArcLengthMeters.Value)
: 0.0;
var hasMeaningfullyShorterDistance =
distanceSquared <
bestDistanceSquared -
DistanceTieToleranceSquaredMeters;
var hasEquivalentDistanceAndCloserProgress =
preferredArcLengthMeters.HasValue &&
Math.Abs(
distanceSquared -
bestDistanceSquared) <=
DistanceTieToleranceSquaredMeters &&
progressDifferenceMeters <
bestProgressDifferenceMeters;
if (!hasMeaningfullyShorterDistance &&
!hasEquivalentDistanceAndCloserProgress)
{
continue;
}
@@ -94,9 +193,9 @@ namespace MultiWheelC.Trajectory
segmentStartIndex;
bestInterpolationRatio =
interpolationRatio;
bestProjectedX = projectedX;
bestProjectedY = projectedY;
bestDistanceSquared = distanceSquared;
bestProgressDifferenceMeters =
progressDifferenceMeters;
}
return BuildProjection(
@@ -104,8 +203,6 @@ namespace MultiWheelC.Trajectory
vehiclePoseInWorld,
bestSegmentStartIndex,
bestInterpolationRatio,
bestProjectedX,
bestProjectedY,
bestDistanceSquared);
}
@@ -117,45 +214,16 @@ namespace MultiWheelC.Trajectory
Pose2D vehiclePoseInWorld,
int segmentStartIndex,
double interpolationRatio,
double projectedX,
double projectedY,
double distanceSquared)
{
var segmentStart =
trajectory[segmentStartIndex];
var segmentEnd =
trajectory[segmentStartIndex + 1];
var referenceYawRadians =
AngleMath.LerpRadians(
segmentStart.PoseInWorld.YawRadians,
segmentEnd.PoseInWorld.YawRadians,
interpolationRatio);
var referenceArcLengthMeters =
InterpolationMath.Lerp(
segmentStart.ArcLengthMeters,
segmentEnd.ArcLengthMeters,
interpolationRatio);
var referenceCurvaturePerMeter =
InterpolationMath.Lerp(
segmentStart.CurvaturePerMeter,
segmentEnd.CurvaturePerMeter,
interpolationRatio);
var referenceSpeedMetersPerSecond =
InterpolationMath.Lerp(
segmentStart.ReferenceSpeedMetersPerSecond,
segmentEnd.ReferenceSpeedMetersPerSecond,
interpolationRatio);
var referencePoint =
new TrajectoryPoint(
referenceArcLengthMeters,
new Pose2D(
projectedX,
projectedY,
referenceYawRadians),
referenceCurvaturePerMeter,
referenceSpeedMetersPerSecond);
trajectory.InterpolateSegment(
segmentStartIndex,
interpolationRatio);
var segmentX =
segmentEnd.PoseInWorld.XMeters -
@@ -171,10 +239,10 @@ namespace MultiWheelC.Trajectory
// 以轨迹线段的前进方向判断左右：
// 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
var vehicleToProjectionX =
projectedX -
referencePoint.PoseInWorld.XMeters -
vehiclePoseInWorld.XMeters;
var vehicleToProjectionY =
projectedY -
referencePoint.PoseInWorld.YMeters -
vehiclePoseInWorld.YMeters;
var lateralErrorMeters =
(segmentX * vehicleToProjectionY -
@@ -183,7 +251,7 @@ namespace MultiWheelC.Trajectory
var headingErrorRadians =
AngleMath.ShortestDifferenceRadians(
referenceYawRadians,
referencePoint.PoseInWorld.YawRadians,
vehiclePoseInWorld.YawRadians);
return new TrajectoryProjection(
@@ -193,27 +261,26 @@ namespace MultiWheelC.Trajectory
headingErrorRadians,
Math.Sqrt(distanceSquared),
trajectory.GetRemainingDistanceMeters(
referenceArcLengthMeters));
referencePoint.ArcLengthMeters));
}
/// <summary>
/// 检查用于投影的实际车体中心位姿是否包含有限数值。
/// 检查轨迹对象和用于投影的实际车体中心位姿。
/// </summary>
private static void EnsureFinitePose(
private static void ValidateProjectionInput(
Trajectory2D trajectory,
Pose2D pose,
string parameterName)
{
if (double.IsNaN(pose.XMeters) ||
double.IsInfinity(pose.XMeters) ||
double.IsNaN(pose.YMeters) ||
double.IsInfinity(pose.YMeters) ||
double.IsNaN(pose.YawRadians) ||
double.IsInfinity(pose.YawRadians))
if (trajectory == null)
{
throw new ArgumentOutOfRangeException(
parameterName,
"用于轨迹投影的车体位姿必须是有限值。");
throw new ArgumentNullException(
nameof(trajectory));
}
NumericGuard.EnsureFinite(
pose,
parameterName);
}
}
}