First_Dev_Dept/ParkingRobot

Fork 0

Files

T

梁薄云 476e87c75f docs: design path smoothing comparison

2026-07-28 16:07:49 +08:00

25 KiB

Raw Blame History

粗路径平滑算法对比实验台设计

1. 背景

ParkrobTrajplanner/CoarsePath 已经通过 CoarsePathPlanningService 输出经过连续车体碰撞、曲率、终点和方向分段复核的 Hybrid A* 粗路径。成功的 PlanningResult 包含：

稠密的 CoarsePathPoint；
覆盖整条路径的前进/倒车 PathSegment；
累计弧长、车辆曲率、保守车体净空和换向标记；
地图、搜索和最终复核诊断。

粗路径由离散恒曲率运动原语拼接而成。它解决绕障拓扑、行驶方向和换向结构，但相邻原语之间的曲率可能跳变，不适合作为后续 SQP 的最终空间参考线。

本设计在 CoarsePath 之后增加独立的 PathSmoothing 模块。第一阶段先建设离线算法对比实验台，使用相同粗路径并行比较三次 B 样条、局部三次 Bézier 和分段五次多项式。实验台统一执行几何分析、安全复核、指标排序和 IEEE 风格绘图。算法成熟后，正式规划流程只调用选定的默认平滑方法。

2. 目标

本阶段必须完成：

定义稳定的粗路径平滑输入、输出、状态、配置和诊断契约。
保持粗路径已经确定的绕障侧、前进/倒车顺序、换向次数和目标进入方向。
使用三种互相独立的平滑方法处理同一条粗路径。
用统一的几何分析器计算原粗路径和三种平滑路径的弧长、航向与曲率。
对所有平滑候选重新执行完整车体碰撞、地图边界、最大车辆曲率、端点和方向分段复核。
支持现有粗路径端到端案例和不重复运行 Hybrid A* 的快速粗路径夹具。
为每个场景输出 IEEE 风格的四路径对比图、四曲率对比图和指标表。
输出可供后续 SQP 使用的不可变平滑空间路径，不包含速度和时间参数。

3. 非目标

本阶段不包含：

SQP、QP、Frenet 横向优化或速度规划；
时间戳、速度、加速度、角速度或 jerk；
改变 Hybrid A* 选定的绕障拓扑；
新增、删除或移动换向点；
现场实时重规划；
四舵轮角度、轮速和底盘跟踪控制；
把不可行平滑候选作为可执行路径发布；
用单一五次多项式覆盖任意长度的完整路径。

4. 总体架构

新增与 CoarsePath 平级的目录：

ClumsyPilot/ParkrobTrajplanner/
├── CoarsePath/
└── PathSmoothing/
    ├── Contracts/
    ├── Processing/
    ├── Algorithms/
    ├── Validation/
    ├── Comparison/
    ├── Visualization/
    ├── Facade/
    └── Test/

各目录职责如下：

目录	职责
`Contracts`	平滑请求、结果、点、方向段、配置、状态、指标和诊断
`Processing`	输入校验、按方向拆段、等弧长重采样、关键点保护和统一几何分析
`Algorithms`	三次 B 样条、局部三次 Bézier、分段五次多项式及内部算法契约
`Validation`	完整车体碰撞、地图边界、曲率、端点、换向和输出不变量复核
`Comparison`	同输入多算法运行、结果隔离、指标汇总、排序和推荐
`Visualization`	图形场景模型、IEEE 样式、SVG/PNG/CSV 导出和字体检查
`Facade`	正式单算法平滑入口和离线多算法比较入口
`Test`	固定夹具、端到端案例和报告生成入口

PathSmoothing 只消费 CoarsePath 和 Map 的公开不可变数据，不读取传感器、定位、UI 或底盘。它不得修改已有 PlanningResult。

5. 数据流

PlanningResult.Path + PlanningResult.Segments
PlanningGridMap + VehicleParameters + PathSmoothingConfiguration
                            │
                            ▼
                 输入契约和原粗路径安全复核
                            │
                            ▼
                 按 Forward/Reverse 独立拆段
                            │
                            ▼
              等弧长重采样、固定端点和换向点
                            │
                ┌───────────┼───────────┐
                ▼           ▼           ▼
          三次 B 样条   局部 Bézier   分段五次多项式
                │           │           │
                └───────────┼───────────┘
                            ▼
                 统一航向、弧长和曲率分析
                            │
                            ▼
          车体碰撞、边界、曲率、端点和分段复核
                            │
                            ▼
              指标汇总、方法排序和 IEEE 风格报告

三种方法不能串联。每种方法从同一份预处理输入开始，失败状态和重试次数分别记录。

6. 公共接口

6.1 正式单算法入口

public sealed class PathSmoothingService
{
    public PathSmoothingResult Smooth(
        PathSmoothingRequest request,
        CancellationToken cancellationToken = default);
}

PathSmoothingRequest 包含：

CoarsePath       IReadOnlyList<CoarsePathPoint>
Segments         IReadOnlyList<PathSegment>
Map              PlanningGridMap
Vehicle          VehicleParameters
Configuration    PathSmoothingConfiguration

调用方可以从成功的 PlanningResult.Path 和 PlanningResult.Segments 直接构造请求。请求仍需携带规划时使用的地图和车辆参数，因为平滑路径必须独立进行安全复核。

6.2 离线比较入口

public sealed class PathSmoothingComparisonService
{
    public PathSmoothingComparisonResult Compare(
        PathSmoothingComparisonRequest request,
        CancellationToken cancellationToken = default);
}

比较请求复用同一份粗路径、地图、车辆和公共配置，并明确列出需要运行的方法。第一版默认方法集合固定为：

CubicBSpline
LocalCubicBezier
PiecewiseQuintic

比较服务不绘图。报告导出器只消费不可变比较结果，因此绘图失败不会改变算法状态或平滑路径。

7. 核心契约

7.1 平滑方法

SmoothingMethod:
    CubicBSpline
    LocalCubicBezier
    PiecewiseQuintic

7.2 平滑状态

PathSmoothingStatus:
    Success
    FallbackToCoarsePath
    InvalidInput
    Infeasible
    Failed
    Cancelled

Success：平滑路径通过所有复核。
FallbackToCoarsePath：所有允许的平滑尝试失败，且原粗路径重新通过安全复核后，按显式配置返回原路径。
InvalidInput：输入路径、方向段、地图、车辆或配置不符合契约。
Infeasible：算法产生了有限数值候选，但候选未通过安全或运动学复核。
Failed：算法发生数值退化或无法产生完整候选。
Cancelled：调用被取消，不能发布部分路径。

除 Success 和 FallbackToCoarsePath 外，正式 PathSmoothingResult.Path 和 Segments 必须为空。

7.3 平滑路径点

SmoothedPathPoint 是不可变值，包含：

X、Y                    世界坐标，m
Heading                 归一化车头航向，rad
UnwrappedHeading        连续展开车头航向，rad
ArcLength               从完整路径起点累计的非负弧长，m
Direction               Forward 或 Reverse
GeometricCurvature      按行进弧长计算的有符号几何曲率，1/m
VehicleCurvature        车辆模型使用的有符号曲率，1/m
BodyClearance           扩大车体的保守净空下界，m
IsGearSwitchPoint       是否为新方向段开始的换向点
Source                  Anchor、Interpolated、GearSwitch 或 CoarsePathFallback

回退原粗路径时 Source 使用单独的 CoarsePathFallback，避免把未经平滑的普通粗路径点错误标记为算法插值点。

设 directionSign 在前进时为 +1，倒车时为 -1。每个方向段内部采用：

GeometricCurvature = d(Heading) / ds
VehicleCurvature   = directionSign × GeometricCurvature

曲率不得跨合法换向重复点求导。

7.4 平滑方向段

平滑后点数与粗路径不同，不能直接复用原 PathSegment 索引。SmoothedPathSegment 包含：

SegmentIndex
Direction
StartIndex
EndIndex
StartsAtGearSwitch
EndsAtGearSwitch

其索引必须连续、无重叠并完整覆盖平滑路径。

7.5 平滑配置

公共配置至少包含：

Method
OutputSpacingMeters                 默认 0.05
MaximumCollisionCheckStepMeters    默认 0.025
MinimumClearanceReserveMeters
AllowFallbackToCoarsePath
SmoothingStrength
RetryStrengthScales

RetryStrengthScales 是有限、严格递减的只读集合。第一版建议为：

1.00, 0.75, 0.50, 0.25

每种算法拥有独立的强类型子配置，不能用无语义的字符串字典传参。

MaximumCollisionCheckStepMeters 默认值为当前粗路径默认安全精度 0.025 m。若上游粗规划采用了更小的检查步长，调用方必须传入相同或更小的值；平滑模块不能从未携带的粗规划配置中猜测该参数。

8. 公共预处理

8.1 输入校验

处理前必须验证：

粗路径和方向段均非空；
所有数值有限；
弧长非负且不递减；
唯一允许的相邻重复位姿是合法换向对；
方向段完整覆盖粗路径；
换向点的位置、航向和弧长保持成对一致；
地图可用于规划；
车辆尺寸、安全余量和最大曲率有效；
原粗路径在当前地图和车辆参数下仍然安全。

原粗路径复核失败时不得执行平滑，也不得回退。

8.2 方向拆分

每个 PathSegment 独立处理。方向段首尾点全部是强制锚点。换向两侧的重复位姿分别属于相邻方向段，两个点都必须保留。

8.3 重采样

所有方向段先转换为统一的等弧长采样。默认间距为 0.05 m，最后一个不足整步的区间保留精确段尾。

重采样不能跨换向点，不能改变段首、段尾的位置和车头航向。

8.4 安全移动范围

预处理器根据原路径的保守车体净空和 MinimumClearanceReserveMeters 计算每个样点允许的最大移动范围。该范围只用于减少平滑曲线切弯进入障碍物的概率，不构成安全证明。最终安全性只能由完整车体碰撞和扫掠复核确认。

9. 三种平滑方法

9.1 三次 B 样条

每个方向段使用夹持三次 B 样条近似：

固定段首和段尾位置；
使用行进方向切向约束段首和段尾；
不把换向两侧合并成一条曲线；
通过平滑强度控制控制点对原粗路径的贴合程度；
输出后统一按弧长重采样。

该方法是普通长路径段的主要候选，优点是整体连续、控制点数量适中；风险是绕障拐角处可能切弯。

9.2 局部三次 Bézier

先从统一重采样路径中识别超过配置航向变化阈值的局部转折区域，再使用三次 Bézier 替换该区域：

区域外路径保持不变；
每个局部连接固定入口和出口位置；
控制柄沿入口和出口切向；
相邻局部区域重叠时先合并，避免重复处理；
直线和微小转折不创建无意义 Bézier 段。

该方法计算最快且局部可控，但只能保证所设计连接阶次内的连续性，整体曲率质量需要通过统一指标判断。

9.3 分段五次多项式

五次方法采用分段二维 Hermite 形式，而不是用单条五次多项式拟合完整方向段：

每个子段约束首尾位置；
首尾一阶导数由行进切向确定；
首尾二阶导数由相邻曲率估计并进行连续化；
共享内部结点的一阶和二阶导数保持一致；
固定完整方向段的首尾位置、航向和曲率约束。

该方法追求二阶连续，适合终端连接和曲率连续性要求较高的参考线，但参数敏感且更容易在短段上数值退化。

10. 几何分析

原粗路径、三种候选和平滑回退路径全部通过同一个 PathGeometryAnalyzer。分析器逐方向段计算，禁止跨换向点差分。

分析器负责：

按位置重新计算累计弧长；
根据行进切向和方向恢复车头航向；
连续展开航向；
计算几何曲率和车辆曲率；
计算曲率总变化和曲率变化能量；
生成完整路径和方向段索引。

原 CoarsePathPoint.VehicleCurvature 仍保留为粗规划诊断，但算法比较不直接拿它与重新计算的平滑曲率混用。

11. 平滑安全复核

SmoothedPathValidator 必须独立复核：

所有点和派生数值有限；
起点、终点位置和车头航向与粗路径对应端点一致；
每个换向点的位置和车头航向不变；
前进/倒车段数量、顺序和边界拓扑不变；
累计弧长严格增加，合法换向重复点除外；
abs(VehicleCurvature) 不超过车辆最大曲率；
每个点的扩大车体无碰撞且位于地图内；
相邻点之间的完整车体扫掠无碰撞；
发布的 BodyClearance 不高于复核得到的保守净空；
平滑方向段完整覆盖输出。

验证器复用现有车辆足迹和碰撞语义，不能引入另一套障碍膨胀规则。

12. 有限安全回退

每种方法先按 SmoothingStrength 运行。候选不通过复核时，依次使用 RetryStrengthScales 降低平滑强度。

默认强度
    ↓ 不可行
0.75 倍
    ↓ 不可行
0.50 倍
    ↓ 不可行
0.25 倍
    ↓ 不可行
该方法标记 Infeasible

重试次数、采用强度和每次失败原因必须进入诊断。达到有限次数后必须停止，不能无限调参。

正式单算法服务只有在 AllowFallbackToCoarsePath = true 且原粗路径重新通过完整安全复核时，才能返回 FallbackToCoarsePath。比较实验中的方法失败不能被回退结果伪装成该方法成功。

13. 质量指标

每条原始或平滑路径都计算：

Feasible
PathLengthMeters
MaximumAbsoluteVehicleCurvaturePerMeter
RootMeanSquareVehicleCurvaturePerMeter
TotalAbsoluteCurvatureVariationPerMeter
CurvatureVariationEnergy
MinimumBodyClearanceMeters
ComputationElapsed
RetryCount
AcceptedStrength

曲率变化指标逐方向段计算后累加，不跨换向点产生虚假的曲率跳变。

每个平滑结果还报告相对于原粗路径的：

LengthChangePercent
PeakCurvatureChangePercent
CurvatureVariationChangePercent
MinimumClearanceChangeMeters

任何分母接近零的相对指标必须使用明确定义的绝对值替代，不能产生 NaN 或无穷值。

14. 方法排序

方法选择采用可解释的字典序，不使用隐藏的加权总分。对全部必选场景按以下聚合值依次比较：

安全可行场景数量更多；
曲率变化能量的场景中位数更低；
各场景最大曲率利用率中的最坏值更低；
各场景最小净空损失中的最坏值更小；
路径长度增量的场景中位数更小；
计算耗时的场景中位数更短。

完全相同时使用稳定的方法枚举顺序打破平局，并在报告中显示所有原始指标。若没有方法通过必选场景，比较结果不推荐默认方法。

15. 场景体系

15.1 端到端场景

端到端组使用现有 CoarsePathScenarioFactory 的固定基准入口，不读取实时 AMR：

场景	用途
`ExplicitEmpty`	直线不应被无意义扭曲
`RectangleDetour`	验证绕障转折和平滑切弯
`ManualAndTwoLeg`	验证多来源障碍和复杂路径
`ReverseGearSwitch`	验证方向分段和换向点保护

CacheHit 不增加新的平滑几何，不进入算法比较。NoFeasiblePath 没有成功粗路径，只保留为上游粗路径测试。

15.2 快速粗路径夹具

快速夹具覆盖：

straight
single-turn
s-bend
large-heading-change
rectangle-detour
multi-obstacle-detour
narrow-corridor
forward-reverse-switch

每个夹具保存：

稳定的英文场景 ID 和版本；
对应的固定地图重建信息；
车辆参数；
粗路径点和方向段；
生成该路径时的粗规划配置指纹；
期望的起终点、方向段数和换向点数。

快速实验重建地图但不调用 HybridAStarPlanner。夹具指纹与当前预期不一致时返回“夹具已过期”，不能静默继续，也不能在普通测试中自动覆盖夹具。

夹具更新必须通过显式开发入口完成，并在更新后运行端到端复核。

16. 比较报告

16.1 四条曲线

正常场景同时比较：

原始粗路径；
三次 B 样条；
局部三次 Bézier；
分段五次多项式。

路径图和曲率图使用相同的四方法顺序。某种方法产生完整但不可行的候选时，实验报告可以绘制其候选并在违规位置标记叉号，同时明确标注“不可行”。该候选只能存在于比较报告的只读调试数据中，不能进入正式 PathSmoothingResult.Path。

数值失败且没有完整候选时，保留图例和指标行，显示“无有效曲线”，不得伪造曲线。

16.2 IEEE 风格版式

每个场景输出一张双栏宽度图：

物理尺寸：7.16 × 5.2 in
PNG：2148 × 1560 px
分辨率：300 dpi

布局：

┌─────────────────────┬───────────────┐
│ (a) 四条路径对比     │ (b) 四条曲率  │
│ 地图、障碍物、起终点 │ κ(s) 与限制线 │
│                     ├───────────────┤
│                     │ (c) 指标表     │
└─────────────────────┴───────────────┘

路径面板约占宽度的 60%；
路径面板强制 X/Y 等比例；
曲率横轴使用各路径自身的累计弧长 s (m)；
曲率面板显示车辆最大正、负曲率限制；
不放大标题，只使用紧凑的 (a)、(b)、(c)；
路径面板不使用背景网格；
曲率面板只保留零线、必要刻度和曲率限制线；
指标表小数点对齐，失败项显示状态而非虚构数值；
图例在所有场景中位置、顺序、颜色和线型一致。

IEEE 官方图形指南建议使用颜色和线型共同编码、保持字体和字号一致，并优先使用高分辨率或矢量图：

16.3 固定色卡

采用色盲友好的科学绘图色卡：

内容	颜色	线型
原始粗路径	`#4D4D4D` 深灰	虚线
三次 B 样条	`#0072B2` 深蓝	实线
局部三次 Bézier	`#D55E00` 朱红	点划线
分段五次多项式	`#009E73` 蓝绿色	点线
曲率上下限	`#CC79A7` 紫红	细虚线
障碍物	`#D9D9D9` 填充、`#666666` 描边	实线

起点使用带黑色边框的黄色圆形，终点使用带黑色边框的紫色菱形。图中不使用渐变、阴影或依赖透明度才能辨认的叠色。

16.4 字体、字号和防乱码

用户指定：

中文：SimSun（宋体）
英文、数字、希腊字母和公式：Times New Roman

绘图实现必须：

在导出前按准确字体族名检查两种字体；
将混合文本按中文和非中文字符片段拆分；
使用字体度量统一片段基线；
使用同一排版模型生成 SVG 和 PNG；
字体缺失时返回明确的 FontUnavailable 导出失败，不能静默替换；
SVG 使用 UTF-8 XML；
CSV 使用带 BOM 的 UTF-8；
PNG 写入并验证对应 300 dpi 的物理分辨率元数据；
使用稳定英文文件名，中文只出现在图内和 CSV 内容中。

最终物理尺寸下建议：

坐标刻度：8 pt
坐标标题、图例和表格正文：9 pt
分图编号：10 pt

英文 Times New Roman 属于 IEEE 推荐字体。由于宋体不是 IEEE 通用英文字体清单的一部分，它只用于满足本项目中文图注需求；英文、数字和数学字符仍使用 Times New Roman。

16.5 输出格式

每个场景输出：

<scenario-id>-comparison.svg
<scenario-id>-comparison.png
<scenario-id>-metrics.csv

SVG 是矢量母版，PNG 用于直接查看和现有工作流，CSV 保存每种方法的完整指标。报告输出属于生成产物，不提交到源码目录。

绘图使用独立的不可变 SmoothingFigureModel。SVG 和 PNG 渲染器都消费该模型，以保证面板范围、曲线、颜色、字体和文本一致。

17. 错误隔离

单个算法的预期失败只影响对应比较项；
单个算法发生数值异常时，比较服务记录算法名、异常类型和安全诊断，然后继续其他算法；
取消优先于继续比较，取消后不再启动后续算法；
报告导出失败不改变平滑比较结果；
某个场景所有算法失败时仍可生成只包含原粗路径、失败状态和诊断的报告；
正式服务不捕获并隐藏调用方编程错误，但所有输入契约错误必须转换为稳定状态和诊断；
所有错误消息包含场景、方法和处理阶段，不依赖堆栈才能定位。

18. 测试设计

18.1 契约测试

空路径、空方向段、空地图、空车辆和空配置；
NaN、无穷值、负弧长和非法曲率；
非法重复点和不完整方向段；
成功和失败结果的只读集合；
失败状态不得携带可执行路径；
取消不得发布部分路径。

18.2 几何分析测试

直线的曲率为零；
已知半径圆弧的几何曲率和车辆曲率符合方向语义；
前进和倒车使用正确的曲率符号；
航向跨越 -π/π 时连续展开；
不跨换向重复点计算差分；
等弧长重采样保留精确段尾。

18.3 算法性质测试

三种方法分别验证：

直线路径不产生无意义横向偏移；
方向段首尾位置和航向保持；
换向点的位置、航向和顺序保持；
输出点数和间距符合配置；
重试强度严格递减且次数有限；
短段和退化控制点产生明确失败而非 NaN。

18.4 安全复核测试

平滑切入障碍物时拒绝；
相邻点之间扫掠碰撞时拒绝；
车体越过地图边界时拒绝；
最大车辆曲率超限时拒绝；
净空被高估时拒绝；
方向段或换向结构改变时拒绝；
原粗路径复核失败时禁止回退。

18.5 比较和夹具测试

三种方法接收同一份预处理输入；
单个方法失败不影响其他方法；
排序规则按固定字典序执行；
没有可行方法时不推荐默认方法；
快速夹具路径不调用 Hybrid A*；
配置指纹变化会使夹具明确过期；
四个现有端到端场景保持预期成功和方向结构。

18.6 绘图测试

固定色值、线型、图例顺序和面板编号；
路径图 X/Y 比例相等；
曲率限制线数值正确；
SimSun 与 Times New Roman 检测；
粗路径 κ(s) X (m) −π 等代表性混合文本完整渲染；
SVG 为有效 UTF-8，包含预期字体和所有四种方法；
PNG 具有正确像素尺寸、PNG 结构、CRC 和 300 dpi 元数据；
CSV 具有 UTF-8 BOM、稳定列顺序和不依赖区域设置的小数格式；
不使用依赖平台抗锯齿细节的脆弱逐像素金图测试；
至少对直线、绕障和换向三张代表图执行人工视觉检查，确认无乱码、遮挡、裁切和间距失衡。

19. 实施边界

第一轮实施只建立本设计中的接口、三种算法、统一验证、快速夹具和报告。正式 MovementTest 集成是后续独立工作，在默认方法通过离线比较并得到明确推荐后进行。

实验台不得让 CoarsePath 依赖 PathSmoothing。正式串联由更上层门面完成：

CoarsePathPlanningService
        ↓ 成功 PlanningResult
PathSmoothingService
        ↓ 成功或明确回退
后续 SQP

20. 验收标准

设计实施完成需满足：

成功粗路径能通过单一公共接口得到不可变平滑路径结果。
三种方法对同一粗路径独立运行，不串联、不修改输入。
起终点、方向段和换向结构在所有成功结果中保持。
所有成功结果通过完整车体碰撞、边界、曲率和输出契约复核。
不可行候选不会进入正式可执行路径。
八个快速夹具无需运行 Hybrid A* 即可完成三算法比较。
四个现有端到端场景能够从建图和 Hybrid A* 连接到平滑比较。
每个场景生成包含四路径、四曲率和指标表的 SVG、300 dpi PNG 与 UTF-8 CSV。
图内中文使用宋体，英文、数字和数学字符使用 Times New Roman，代表性图无乱码、遮挡或裁切。
比较结果使用公开的字典序规则推荐方法；没有合格方法时明确不推荐。
输出可以作为后续 SQP 的空间参考路径输入，但不提前引入时间、速度或控制字段。

25 KiB Raw Blame History Unescape Escape