docs: document smoothing processing core

This commit is contained in:
梁薄云
2026-08-04 14:29:51 +08:00
parent 4a7dd875a4
commit 8f6e97e88f
17 changed files with 153 additions and 11 deletions
@@ -6,6 +6,16 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
internal sealed class CurvatureTransition
{
/// <summary>记录同一方向段内相邻粗路径采样点之间的车辆曲率跳变。</summary>
/// <param name="segmentIndex">所属方向段在预处理路径中的从零开始索引。</param>
/// <param name="leftCoarsePathIndex">跳变左侧粗路径采样点索引。</param>
/// <param name="rightCoarsePathIndex">跳变右侧粗路径采样点索引,必须等于左索引加一。</param>
/// <param name="localArcLengthMeters">跳变位置在方向段内的局部弧长,单位 m。</param>
/// <param name="x">跳变位置世界 X 坐标,单位 m。</param>
/// <param name="y">跳变位置世界 Y 坐标,单位 m。</param>
/// <param name="vehicleHeadingRadians">跳变位置的车辆航向,单位 rad。</param>
/// <param name="leftVehicleCurvaturePerMeter">左侧车辆曲率,单位 1/m。</param>
/// <param name="rightVehicleCurvaturePerMeter">右侧车辆曲率,单位 1/m。</param>
internal CurvatureTransition(
int segmentIndex,
int leftCoarsePathIndex,
@@ -9,6 +9,13 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>从原始粗路径的同向相邻点中识别恒曲率原语边界。</summary>
internal sealed class CurvatureTransitionDetector
{
/// <summary>检测同一行驶方向内超过阈值的相邻车辆曲率跳变,不跨越换向边界。</summary>
/// <param name="request">含粗路径、方向段和配置的平滑请求。</param>
/// <param name="maximumVehicleCurvaturePerMeter">车辆允许的最大曲率,单位 1/m。</param>
/// <param name="options">已校验的跳变绝对阈值与相对阈值选项。</param>
/// <param name="transitions">成功时为只读曲率跳变集合;失败时为空集合。</param>
/// <param name="reason">失败原因;成功时为空字符串。</param>
/// <returns>输入与检测过程有效时为 <see langword="true"/>;否则为 <see langword="false"/>。</returns>
internal bool TryDetect(
PathSmoothingRequest request,
double maximumVehicleCurvaturePerMeter,
@@ -18,6 +18,13 @@ internal sealed class LocalG2CandidateBuilder
private const int MaximumSubdivisionDepth = 32;
private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };
/// <summary>按确定的窗口与导数尺度顺序生成有限个五次 Hermite 局部 G2 候选。</summary>
/// <param name="originalSegment">候选所属、未经替换的单方向预处理段。</param>
/// <param name="region">包含曲率跳变和可用窗口变体的局部平滑区域。</param>
/// <param name="outputSpacingMeters">候选采样间距,单位 m,必须为正且有限。</param>
/// <param name="options">已校验的局部 G2 窗口、偏差和候选数限制。</param>
/// <param name="cancellationToken">取消令牌;取消时抛出 <see cref="OperationCanceledException"/>。</param>
/// <returns>只读的候选几何集合;没有可构造候选时返回空集合而非 <see langword="null"/>。</returns>
internal IReadOnlyList<LocalG2CandidateGeometry> Build(
PreparedDirectionSegment originalSegment,
LocalG2SmoothingRegion region,
@@ -9,6 +9,19 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>一个尚未经过安全和质量评价的局部 G2 替换几何。</summary>
internal sealed class LocalG2CandidateGeometry
{
/// <summary>创建尚未通过安全与质量门的局部 G2 候选几何快照。</summary>
/// <param name="candidateIndex">区域内候选的稳定从零开始索引。</param>
/// <param name="segmentIndex">候选所属方向段索引。</param>
/// <param name="startArcLengthMeters">替换窗口起点弧长,单位 m。</param>
/// <param name="endArcLengthMeters">替换窗口终点弧长,单位 m。</param>
/// <param name="leftWindowLengthMeters">过渡点左侧窗口长度,单位 m。</param>
/// <param name="rightWindowLengthMeters">过渡点右侧窗口长度,单位 m。</param>
/// <param name="regionPoints">含精确边界端点的候选采样点只读集合。</param>
/// <param name="startVehicleCurvaturePerMeter">窗口起点车辆曲率,单位 1/m。</param>
/// <param name="endVehicleCurvaturePerMeter">窗口终点车辆曲率,单位 1/m。</param>
/// <param name="startGeometricCurvaturePerMeter">窗口起点几何曲率,单位 1/m。</param>
/// <param name="endGeometricCurvaturePerMeter">窗口终点几何曲率,单位 1/m。</param>
/// <param name="internalConnectionsAreG2">候选内部连接是否满足 G2 连续性。</param>
internal LocalG2CandidateGeometry(
int candidateIndex,
int segmentIndex,
@@ -6,6 +6,8 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>局部 G2 预平滑一次运行使用的已校验不可变选项。</summary>
internal sealed class LocalG2OptionsSnapshot
{
/// <summary>从已配置的请求选项提取并校验一次局部 G2 平滑运行所需的不可变阈值。</summary>
/// <param name="configuration">包含 <see cref="LocalG2QuinticOptions"/> 的路径平滑配置。</param>
internal LocalG2OptionsSnapshot(PathSmoothingConfiguration configuration)
{
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
@@ -5,9 +5,15 @@ using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>只替换一个方向段内局部窗口,并保持其余方向拓扑不变。</summary>
/// <summary>将已验证的局部候选替换进同方向预处理路径;保持窗口外点、端点和换向拓扑不变。</summary>
internal sealed class LocalG2PathSplicer
{
/// <summary>用候选窗口点替换目标方向段的局部弧长区间。</summary>
/// <param name="currentPath">当前已接受替换的预处理路径。</param>
/// <param name="candidate">候选替换几何;位置和局部弧长单位为 m。</param>
/// <param name="replacedPath">成功时为保持其他段与边界不变的新预处理路径;失败时为 <see langword="null"/>。</param>
/// <param name="reason">失败原因;成功时为空字符串。</param>
/// <returns>替换范围、端点和方向拓扑均合法时为 <see langword="true"/>;否则为 <see langword="false"/>。</returns>
internal bool TryReplace(
PreparedPath currentPath,
LocalG2CandidateGeometry candidate,
@@ -8,6 +8,12 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
internal sealed class LocalG2WindowVariant
{
/// <summary>创建同一平滑区域内一个可尝试替换的弧长窗口变体。</summary>
/// <param name="candidateIndex">区域内候选的稳定从零开始索引。</param>
/// <param name="startArcLengthMeters">窗口起点弧长,单位 m。</param>
/// <param name="endArcLengthMeters">窗口终点弧长,单位 m。</param>
/// <param name="leftWindowLengthMeters">过渡点左侧窗口长度,单位 m。</param>
/// <param name="rightWindowLengthMeters">过渡点右侧窗口长度,单位 m。</param>
internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters,
double leftWindowLengthMeters, double rightWindowLengthMeters)
{
@@ -33,6 +39,12 @@ internal sealed class LocalG2WindowVariant
/// <summary>至少存在一个联合合法生成窗口变体的一组曲率过渡。</summary>
internal sealed class LocalG2SmoothingRegion
{
/// <summary>创建一个包含共同可用窗口变体的局部曲率过渡区域。</summary>
/// <param name="segmentIndex">所属方向段在预处理路径中的从零开始索引。</param>
/// <param name="transitions">归并到该区域的曲率跳变集合。</param>
/// <param name="maximumStartArcLengthMeters">所有候选允许的最晚窗口起点,单位 m。</param>
/// <param name="maximumEndArcLengthMeters">所有候选允许的最早窗口终点,单位 m。</param>
/// <param name="windowVariants">至少一个合法的窗口变体集合。</param>
internal LocalG2SmoothingRegion(
int segmentIndex,
IReadOnlyList<CurvatureTransition> transitions,
@@ -7,11 +7,18 @@ using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
/// <summary>硬方向边界内为局部 G2 曲率事件生成、合并候选窗口;绝不让窗口跨越换向段。</summary>
internal sealed class LocalG2WindowPlanner
{
private const double MergeToleranceMeters = 1e-9d;
/// <summary>按段索引排序曲率事件并生成每个区域可用的窗口变体。</summary>
/// <param name="originalPath">已准备的方向段路径,提供每段局部弧长范围。</param>
/// <param name="transitions">检测到的曲率过渡事件;局部弧长单位为 m。</param>
/// <param name="options">窗口长度、候选上限和偏移相关的 Local G2 只读选项。</param>
/// <param name="regions">成功时为按路径顺序排列的只读平滑区域;失败时为空集合。</param>
/// <param name="reason">失败时描述事件、段或窗口长度问题;成功时为空字符串。</param>
/// <returns>所有事件可在所属方向段内构造合法窗口时为 <see langword="true"/>;否则为 <see langword="false"/>。</returns>
internal bool TryPlan(
PreparedPath originalPath,
IReadOnlyList<CurvatureTransition> transitions,
@@ -36,6 +36,22 @@ internal sealed class QuinticHermiteCurve2D
_y5 = y5;
}
/// <summary>从两个端点的位置、一阶导数和二阶导数创建参数区间 [0, 1] 上的五次 Hermite 曲线。</summary>
/// <param name="x0">起点 X 坐标,单位 m。</param>
/// <param name="y0">起点 Y 坐标,单位 m。</param>
/// <param name="dx0">起点对参数的 X 一阶导数。</param>
/// <param name="dy0">起点对参数的 Y 一阶导数。</param>
/// <param name="ddx0">起点对参数的 X 二阶导数。</param>
/// <param name="ddy0">起点对参数的 Y 二阶导数。</param>
/// <param name="x1">终点 X 坐标,单位 m。</param>
/// <param name="y1">终点 Y 坐标,单位 m。</param>
/// <param name="dx1">终点对参数的 X 一阶导数。</param>
/// <param name="dy1">终点对参数的 Y 一阶导数。</param>
/// <param name="ddx1">终点对参数的 X 二阶导数。</param>
/// <param name="ddy1">终点对参数的 Y 二阶导数。</param>
/// <param name="curve">成功时为可求值曲线;失败时为 <see langword="null"/>。</param>
/// <param name="reason">失败原因;成功时为空字符串。</param>
/// <returns>全部边界条件有限且一阶导数非零时为 <see langword="true"/>;否则为 <see langword="false"/>。</returns>
internal static bool TryCreate(
double x0, double y0, double dx0, double dy0, double ddx0, double ddy0,
double x1, double y1, double dx1, double dy1, double ddx1, double ddy1,
@@ -69,6 +85,14 @@ internal sealed class QuinticHermiteCurve2D
return true;
}
/// <summary>在归一化参数处求曲线位置及其一阶、二阶导数。</summary>
/// <param name="u">闭区间 [0, 1] 内的无量纲曲线参数。</param>
/// <param name="x">返回位置 X,单位 m。</param>
/// <param name="y">返回位置 Y,单位 m。</param>
/// <param name="dx">返回 X 对参数的一阶导数。</param>
/// <param name="dy">返回 Y 对参数的一阶导数。</param>
/// <param name="ddx">返回 X 对参数的二阶导数。</param>
/// <param name="ddy">返回 Y 对参数的二阶导数。</param>
internal void Evaluate(
double u,
out double x, out double y,