拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1 @@
|
||||
// 兼容现有 MDCS 的 AbstractTrack
|
||||
@@ -0,0 +1,172 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Linq;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一条经过基本合法性检查的只读二维参考轨迹。
|
||||
/// </summary>
|
||||
public sealed class Trajectory2D
|
||||
{
|
||||
private const double StartArcLengthToleranceMeters = 1e-9;
|
||||
private const double MinimumSegmentLengthMeters = 1e-6;
|
||||
|
||||
private readonly TrajectoryPoint[] _points;
|
||||
private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
|
||||
|
||||
/// <summary>
|
||||
/// 复制并验证按累计弧长升序排列的参考轨迹点。
|
||||
/// </summary>
|
||||
public Trajectory2D(
|
||||
IEnumerable<TrajectoryPoint> points)
|
||||
{
|
||||
if (points == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
_points = points.ToArray();
|
||||
|
||||
if (_points.Length < 2)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"二维轨迹至少需要两个轨迹点。",
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
if (Math.Abs(_points[0].ArcLengthMeters) >
|
||||
StartArcLengthToleranceMeters)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"二维轨迹起点的累计弧长必须为0m。",
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
for (var index = 1;
|
||||
index < _points.Length;
|
||||
index++)
|
||||
{
|
||||
ValidateSegment(
|
||||
_points[index - 1],
|
||||
_points[index],
|
||||
index,
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
_readOnlyPoints =
|
||||
Array.AsReadOnly(_points);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹点数量。
|
||||
/// </summary>
|
||||
public int Count => _points.Length;
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定索引处的轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint this[int index] =>
|
||||
_points[index];
|
||||
|
||||
/// <summary>
|
||||
/// 获取不可修改的有序轨迹点集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<TrajectoryPoint> Points =>
|
||||
_readOnlyPoints;
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹起点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint StartPoint =>
|
||||
_points[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹终点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint EndPoint =>
|
||||
_points[_points.Length - 1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹总弧长，单位为m。
|
||||
/// </summary>
|
||||
public double TotalLengthMeters =>
|
||||
EndPoint.ArcLengthMeters;
|
||||
|
||||
/// <summary>
|
||||
/// 根据当前累计弧长计算到轨迹终点的剩余距离。
|
||||
/// </summary>
|
||||
public double GetRemainingDistanceMeters(
|
||||
double arcLengthMeters)
|
||||
{
|
||||
EnsureFinite(
|
||||
arcLengthMeters,
|
||||
nameof(arcLengthMeters));
|
||||
|
||||
if (arcLengthMeters <= 0.0)
|
||||
return TotalLengthMeters;
|
||||
|
||||
if (arcLengthMeters >= TotalLengthMeters)
|
||||
return 0.0;
|
||||
|
||||
return TotalLengthMeters - arcLengthMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查相邻轨迹点是否构成有效的非零长度有序线段。
|
||||
/// </summary>
|
||||
private static void ValidateSegment(
|
||||
TrajectoryPoint previous,
|
||||
TrajectoryPoint current,
|
||||
int currentIndex,
|
||||
string parameterName)
|
||||
{
|
||||
if (current.ArcLengthMeters <=
|
||||
previous.ArcLengthMeters)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
|
||||
parameterName);
|
||||
}
|
||||
|
||||
var deltaX =
|
||||
current.PoseInWorld.XMeters -
|
||||
previous.PoseInWorld.XMeters;
|
||||
var deltaY =
|
||||
current.PoseInWorld.YMeters -
|
||||
previous.PoseInWorld.YMeters;
|
||||
var segmentLengthSquared =
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY;
|
||||
var minimumLengthSquared =
|
||||
MinimumSegmentLengthMeters *
|
||||
MinimumSegmentLengthMeters;
|
||||
|
||||
if (segmentLengthSquared <
|
||||
minimumLengthSquared)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
|
||||
parameterName);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹弧长必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
|
||||
/// </summary>
|
||||
public readonly struct TrajectoryPoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint(
|
||||
double arcLengthMeters,
|
||||
Pose2D poseInWorld,
|
||||
double curvaturePerMeter,
|
||||
double referenceSpeedMetersPerSecond)
|
||||
{
|
||||
EnsureFiniteNonNegative(
|
||||
arcLengthMeters,
|
||||
nameof(arcLengthMeters));
|
||||
EnsureFinite(
|
||||
poseInWorld.XMeters,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
poseInWorld.YMeters,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
poseInWorld.YawRadians,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
curvaturePerMeter,
|
||||
nameof(curvaturePerMeter));
|
||||
EnsureFinite(
|
||||
referenceSpeedMetersPerSecond,
|
||||
nameof(referenceSpeedMetersPerSecond));
|
||||
ArcLengthMeters = arcLengthMeters;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
ReferenceSpeedMetersPerSecond =
|
||||
referenceSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取从轨迹起点累计到当前点的弧长，单位为m。
|
||||
/// </summary>
|
||||
public double ArcLengthMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心参考坐标系在世界坐标系中的位姿。
|
||||
/// </summary>
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
|
||||
/// </summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取沿轨迹切线方向的有符号参考速度，单位为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,
|
||||
"轨迹累计弧长不能为负数。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存车体中心投影到二维参考轨迹后得到的只读结果。
|
||||
/// </summary>
|
||||
public readonly struct TrajectoryProjection
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
|
||||
/// </summary>
|
||||
public TrajectoryProjection(
|
||||
int segmentStartIndex,
|
||||
TrajectoryPoint referencePoint,
|
||||
double lateralErrorMeters,
|
||||
double headingErrorRadians,
|
||||
double distanceToTrajectoryMeters,
|
||||
double remainingDistanceMeters)
|
||||
{
|
||||
if (segmentStartIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(segmentStartIndex),
|
||||
"投影线段起点索引不能为负数。");
|
||||
}
|
||||
|
||||
EnsureFinite(
|
||||
lateralErrorMeters,
|
||||
nameof(lateralErrorMeters));
|
||||
EnsureFinite(
|
||||
headingErrorRadians,
|
||||
nameof(headingErrorRadians));
|
||||
EnsureFiniteNonNegative(
|
||||
distanceToTrajectoryMeters,
|
||||
nameof(distanceToTrajectoryMeters));
|
||||
EnsureFiniteNonNegative(
|
||||
remainingDistanceMeters,
|
||||
nameof(remainingDistanceMeters));
|
||||
|
||||
SegmentStartIndex = segmentStartIndex;
|
||||
ReferencePoint = referencePoint;
|
||||
LateralErrorMeters = lateralErrorMeters;
|
||||
HeadingErrorRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
headingErrorRadians);
|
||||
DistanceToTrajectoryMeters =
|
||||
distanceToTrajectoryMeters;
|
||||
RemainingDistanceMeters =
|
||||
remainingDistanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
|
||||
/// </summary>
|
||||
public int SegmentStartIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置插值得到的车体中心参考轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint ReferencePoint { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
|
||||
/// </summary>
|
||||
public double LateralErrorMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
|
||||
/// </summary>
|
||||
public double HeadingErrorRadians { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心到投影点的欧氏距离，单位为m。
|
||||
/// </summary>
|
||||
public double DistanceToTrajectoryMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
|
||||
/// </summary>
|
||||
public double RemainingDistanceMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置从轨迹起点累计的弧长，单位为m。
|
||||
/// </summary>
|
||||
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,
|
||||
"轨迹投影距离不能为负数。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
|
||||
/// </summary>
|
||||
public static class TrajectoryProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
|
||||
/// </summary>
|
||||
public static TrajectoryProjection Project(
|
||||
Trajectory2D trajectory,
|
||||
Pose2D vehiclePoseInWorld)
|
||||
{
|
||||
if (trajectory == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
EnsureFinitePose(
|
||||
vehiclePoseInWorld,
|
||||
nameof(vehiclePoseInWorld));
|
||||
|
||||
var bestSegmentStartIndex = 0;
|
||||
var bestInterpolationRatio = 0.0;
|
||||
var bestProjectedX = 0.0;
|
||||
var bestProjectedY = 0.0;
|
||||
var bestDistanceSquared =
|
||||
double.PositiveInfinity;
|
||||
|
||||
for (var segmentStartIndex = 0;
|
||||
segmentStartIndex < trajectory.Count - 1;
|
||||
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;
|
||||
|
||||
if (distanceSquared >= bestDistanceSquared)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
bestSegmentStartIndex =
|
||||
segmentStartIndex;
|
||||
bestInterpolationRatio =
|
||||
interpolationRatio;
|
||||
bestProjectedX = projectedX;
|
||||
bestProjectedY = projectedY;
|
||||
bestDistanceSquared = distanceSquared;
|
||||
}
|
||||
|
||||
return BuildProjection(
|
||||
trajectory,
|
||||
vehiclePoseInWorld,
|
||||
bestSegmentStartIndex,
|
||||
bestInterpolationRatio,
|
||||
bestProjectedX,
|
||||
bestProjectedY,
|
||||
bestDistanceSquared);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据最近线段和插值比例生成控制器使用的完整投影结果。
|
||||
/// </summary>
|
||||
private static TrajectoryProjection BuildProjection(
|
||||
Trajectory2D 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);
|
||||
|
||||
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 =
|
||||
projectedX -
|
||||
vehiclePoseInWorld.XMeters;
|
||||
var vehicleToProjectionY =
|
||||
projectedY -
|
||||
vehiclePoseInWorld.YMeters;
|
||||
var lateralErrorMeters =
|
||||
(segmentX * vehicleToProjectionY -
|
||||
segmentY * vehicleToProjectionX) /
|
||||
segmentLength;
|
||||
|
||||
var headingErrorRadians =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
referenceYawRadians,
|
||||
vehiclePoseInWorld.YawRadians);
|
||||
|
||||
return new TrajectoryProjection(
|
||||
segmentStartIndex,
|
||||
referencePoint,
|
||||
lateralErrorMeters,
|
||||
headingErrorRadians,
|
||||
Math.Sqrt(distanceSquared),
|
||||
trajectory.GetRemainingDistanceMeters(
|
||||
referenceArcLengthMeters));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查用于投影的实际车体中心位姿是否包含有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
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))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"用于轨迹投影的车体位姿必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user