287 lines
11 KiB
C#
287 lines
11 KiB
C#
using System;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Trajectory
|
|
{
|
|
/// <summary>
|
|
/// 将实际车体中心位姿投影到二维离散参考轨迹，并支持按上次进度限制搜索范围。
|
|
/// </summary>
|
|
public static class TrajectoryProjector
|
|
{
|
|
private const double DistanceTieToleranceSquaredMeters =
|
|
1e-12;
|
|
|
|
/// <summary>
|
|
/// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
|
|
/// </summary>
|
|
public static TrajectoryProjection Project(
|
|
Trajectory2D trajectory,
|
|
Pose2D vehiclePoseInWorld)
|
|
{
|
|
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 bestDistanceSquared =
|
|
double.PositiveInfinity;
|
|
var bestProgressDifferenceMeters =
|
|
double.PositiveInfinity;
|
|
|
|
for (var segmentStartIndex =
|
|
firstSegmentStartIndex;
|
|
segmentStartIndex <=
|
|
lastSegmentStartIndex;
|
|
segmentStartIndex++)
|
|
{
|
|
var segmentStart =
|
|
trajectory[segmentStartIndex];
|
|
var segmentEnd =
|
|
trajectory[segmentStartIndex + 1];
|
|
|
|
var segmentX =
|
|
segmentEnd.PoseInWorld.XMeters -
|
|
segmentStart.PoseInWorld.XMeters;
|
|
var segmentY =
|
|
segmentEnd.PoseInWorld.YMeters -
|
|
segmentStart.PoseInWorld.YMeters;
|
|
var segmentLengthSquared =
|
|
segmentX * segmentX +
|
|
segmentY * segmentY;
|
|
|
|
var vehicleFromSegmentStartX =
|
|
vehiclePoseInWorld.XMeters -
|
|
segmentStart.PoseInWorld.XMeters;
|
|
var vehicleFromSegmentStartY =
|
|
vehiclePoseInWorld.YMeters -
|
|
segmentStart.PoseInWorld.YMeters;
|
|
|
|
var interpolationRatio =
|
|
InterpolationMath.Clamp01(
|
|
(vehicleFromSegmentStartX * segmentX +
|
|
vehicleFromSegmentStartY * segmentY) /
|
|
segmentLengthSquared);
|
|
|
|
var projectedX =
|
|
InterpolationMath.Lerp(
|
|
segmentStart.PoseInWorld.XMeters,
|
|
segmentEnd.PoseInWorld.XMeters,
|
|
interpolationRatio);
|
|
var projectedY =
|
|
InterpolationMath.Lerp(
|
|
segmentStart.PoseInWorld.YMeters,
|
|
segmentEnd.PoseInWorld.YMeters,
|
|
interpolationRatio);
|
|
|
|
var projectionErrorX =
|
|
projectedX -
|
|
vehiclePoseInWorld.XMeters;
|
|
var projectionErrorY =
|
|
projectedY -
|
|
vehiclePoseInWorld.YMeters;
|
|
var distanceSquared =
|
|
projectionErrorX * projectionErrorX +
|
|
projectionErrorY * projectionErrorY;
|
|
|
|
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;
|
|
}
|
|
|
|
bestSegmentStartIndex =
|
|
segmentStartIndex;
|
|
bestInterpolationRatio =
|
|
interpolationRatio;
|
|
bestDistanceSquared = distanceSquared;
|
|
bestProgressDifferenceMeters =
|
|
progressDifferenceMeters;
|
|
}
|
|
|
|
return BuildProjection(
|
|
trajectory,
|
|
vehiclePoseInWorld,
|
|
bestSegmentStartIndex,
|
|
bestInterpolationRatio,
|
|
bestDistanceSquared);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据最近线段和插值比例生成控制器使用的完整投影结果。
|
|
/// </summary>
|
|
private static TrajectoryProjection BuildProjection(
|
|
Trajectory2D trajectory,
|
|
Pose2D vehiclePoseInWorld,
|
|
int segmentStartIndex,
|
|
double interpolationRatio,
|
|
double distanceSquared)
|
|
{
|
|
var segmentStart =
|
|
trajectory[segmentStartIndex];
|
|
var segmentEnd =
|
|
trajectory[segmentStartIndex + 1];
|
|
var referencePoint =
|
|
trajectory.InterpolateSegment(
|
|
segmentStartIndex,
|
|
interpolationRatio);
|
|
|
|
var segmentX =
|
|
segmentEnd.PoseInWorld.XMeters -
|
|
segmentStart.PoseInWorld.XMeters;
|
|
var segmentY =
|
|
segmentEnd.PoseInWorld.YMeters -
|
|
segmentStart.PoseInWorld.YMeters;
|
|
var segmentLength =
|
|
Math.Sqrt(
|
|
segmentX * segmentX +
|
|
segmentY * segmentY);
|
|
|
|
// 以轨迹线段的前进方向判断左右：
|
|
// 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
|
|
var vehicleToProjectionX =
|
|
referencePoint.PoseInWorld.XMeters -
|
|
vehiclePoseInWorld.XMeters;
|
|
var vehicleToProjectionY =
|
|
referencePoint.PoseInWorld.YMeters -
|
|
vehiclePoseInWorld.YMeters;
|
|
var lateralErrorMeters =
|
|
(segmentX * vehicleToProjectionY -
|
|
segmentY * vehicleToProjectionX) /
|
|
segmentLength;
|
|
|
|
var headingErrorRadians =
|
|
AngleMath.ShortestDifferenceRadians(
|
|
referencePoint.PoseInWorld.YawRadians,
|
|
vehiclePoseInWorld.YawRadians);
|
|
|
|
return new TrajectoryProjection(
|
|
segmentStartIndex,
|
|
referencePoint,
|
|
lateralErrorMeters,
|
|
headingErrorRadians,
|
|
Math.Sqrt(distanceSquared),
|
|
trajectory.GetRemainingDistanceMeters(
|
|
referencePoint.ArcLengthMeters));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查轨迹对象和用于投影的实际车体中心位姿。
|
|
/// </summary>
|
|
private static void ValidateProjectionInput(
|
|
Trajectory2D trajectory,
|
|
Pose2D pose,
|
|
string parameterName)
|
|
{
|
|
if (trajectory == null)
|
|
{
|
|
throw new ArgumentNullException(
|
|
nameof(trajectory));
|
|
}
|
|
|
|
NumericGuard.EnsureFinite(
|
|
pose,
|
|
parameterName);
|
|
}
|
|
}
|
|
}
|