新增倒车以及项目结构优化
This commit is contained in:
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user