# AMR 非结构化道路 Hybrid A* 粗路径规划总体技术方案

### 目录

0. 总体概览 1. 任务边界 2. 地图表达 3. 运动模型与曲率 4. 搜索节点状态 5. 运动原语 6. 原语终止与积分 7. 碰撞检测与模板 8. 代价与启发 9. 终点与Reeds–Shepp 10. 回溯与稠密点 11. 重采样 12. 平滑与校验 13. 性能优化TODO 14. 接口与验收 附录A. 推荐参数 附录B. 总体伪代码 附录C. 开发里程碑

## 0. 总体概览

车式运动
车头/车尾方向运动，不考虑蟹行
前进 + 倒车
允许在原语边界切换方向
空间粗路径
不包含速度、加速度、时间戳

### 0.1 总体职责边界

模块 | 主要职责 | 不负责
--- | --- | ---
Hybrid A* | 拓扑可达、无碰撞、基本运动学可行、前进/倒车结构、目标位置与航向接近 | 最终速度、加速度、时间、四轮舵角与轮速
路径后处理 | 回溯、稠密点恢复、关键点保留、弧长重采样、快速曲线平滑、完整复核 | 动态约束和最终轨迹时间参数化
后续SQP | 终点精确收敛、速度/加速度/角速度/时间戳、换向点停车、动态可跟踪性 | 重新决定绕障侧或凭空增加倒车拓扑
底盘逆解与控制 | 四轮舵角、轮速分配和轨迹跟踪 | 全局绕障搜索

### 0.2 总体数据流

感知障碍物 / 点云 → 过滤、投影到二维
二维占据栅格 + 障碍物距离场
起点、终点、车辆参数、规划配置
二维Dijkstra启发图 + Hybrid A*搜索
机会式/强制 Reeds–Shepp 终点连接
父节点回溯 + 0.05 m内部积分点恢复
关键点保留 + 分方向弧长重采样
三次B样条优先，局部Bézier/五次多项式，局部QP兜底
最终碰撞、净空、曲率、换向结构校验
输出空间粗路径 → 后续SQP
第一版核心原则：先保证算法完整跑通、路径正确、可解释、可复现；性能优化整体放入第十三步TODO，待前12步稳定后再开展。

## 1任务边界与输出定义 已确认

### 1.1 车辆运动模式

目标平台为四舵轮AMR，但第一阶段只使用车式运动能力：

- 车辆沿车头或车尾方向运动；
- 允许前进、倒车以及换向；
- 暂不考虑蟹行、纯横移和任意方向全向运动；
- 车体航向角始终表示车头朝向，即使车辆正在倒车。

### 1.2 Hybrid A*最小输出

输出空间路径点序列，至少包含：

    位置：x, y
    车体航向：heading
    运动方向：Forward / Reverse
    累计弧长：s
    参考/几何曲率：kappa
    换向标记：isGearSwitchPoint
    安全净空：clearance

不输出速度、加速度、角速度、时间戳、四个舵轮角度与轮速。

### 1.3 成功判据

- 路径全程无碰撞且不进入未知区域；
- 粗路径基本满足最大曲率约束；
- 包含正确绕障侧、前进/后退和换向结构；
- 终点位置与航向满足第九步终止规则；
- 可作为后续SQP的有效初值。

## 2地图表达与环境输入 已确认

### 2.1 地图生成流程

原始点云 / 障碍信息 ↓ 过滤噪声、地面、离群点 投影到二维平面 ↓ 二维占据栅格 Occupancy Grid ↓ 障碍物距离场 Distance Field / EDT

### 2.2 栅格定义

- 地图分辨率初值：`0.05 m`；
- 状态：Free、Occupied、Unknown；
- 第一版中Unknown按障碍物处理；
- 地图包含Origin、Resolution、Width、Height及Version；
- 算法内部不得写死分辨率，统一读取地图参数。

### 2.3 车体碰撞模型

AMR使用旋转矩形车体碰撞模型，不能只用中心点或单圆近似。安全余量主要加到车体矩形上：

    CheckLength = VehicleLength + 2 × SafetyMargin
    CheckWidth  = VehicleWidth  + 2 × SafetyMargin

初始安全余量为`0.03 m`。避免地图膨胀与车体扩大同时重复计算同一余量。

## 3车辆运动模型与最大曲率 已确认

### 3.1 参考点

车辆状态参考点采用AMR几何中心。

### 3.2 基于路径弧长的运动学

    dx/ds = cos(theta)
    dy/ds = sin(theta)
    dtheta/ds = kappa

其中`theta`为车头航向角，`kappa`为车体中心路径曲率。

### 3.3 最大曲率来源

必须同时支持两种来源：

1. 外部直接输入`MaxCurvature`，或输入`MinTurningRadius`并换算`kappa_max = 1 / R_min`；
2. 由四轮转向几何调用`ComputeMaxCurvature(VehicleKinematicParameters)`计算。

推荐三种模式：

    ExternalOnly
    GeometryOnly
    ConservativeMinimum

当两种来源同时存在时，建议取更保守的较小值。若均无有效值，则返回配置错误。

曲率变化率不在本步骤强制建模。第一版先保证曲率不超过上限；渐变曲率原语和实车舵轮速率约束作为后续升级。

## 4Hybrid A*搜索节点状态 已确认

### 4.1 连续状态

    (x, y, theta, direction, curvatureIndex)

- `x, y, theta`保持连续值用于运动积分；
- `direction`为Forward或Reverse；
- `curvatureIndex`表示当前离散曲率等级。

### 4.2 Closed Set键

    (ix, iy, iHeading, direction, curvatureIndex)

推荐初始离散：

- 位置Closed Set分辨率：`0.05–0.10 m`；
- 航向离散：`5°`；
- 方向和曲率等级均进入键，避免把运动状态不同的节点错误合并。

### 4.3 搜索元数据

节点还应保存G/H/F代价、父节点索引、生成当前节点的原语积分点、最小净空、终止原因等。这些属于搜索管理信息，不属于车辆物理状态。

## 5运动原语与扩展规则 已确认

### 5.1 第一版原语集合

    { -kappa_max, -0.5 kappa_max, 0, 0.5 kappa_max, kappa_max }

每种曲率均支持Forward与Reverse。

### 5.2 曲率切换限制

相邻原语曲率等级最多变化1级。例如：

    允许：0 → 0.5κmax
    允许：0.5κmax → κmax
    不允许：κmax → -κmax

该限制可减少“左打死后立即右打死”的不合理跳变。

### 5.3 换向规则

- 前进/倒车只能在运动原语边界切换；
- 起始曲率若上层可提供则使用实际值，否则默认0；
- 第一版采用恒定曲率原语。

TODO：当恒定曲率原语造成曲率跳变过大、平滑后仍不可跟踪或实车舵轮变化速度不足时，增加线性渐变曲率原语。

## 6原语长度、积分与终止 已确认

### 6.1 固定参数

    PrimitiveLength = 0.50 m
    IntegrationStep = 0.05 m

完整原语最多包含10个内部积分点。只有原语终点进入Open List，内部积分点用于碰撞、净空和最终路径恢复。

### 6.2 终止条件

条件 | 处理
--- | ---
累计长度达到0.50 m | 生成正常候选节点
任一积分点碰撞 | 原语无效，立即终止
越界或进入未知区域 | 原语无效
中途满足目标规则 | 提前终止并保存实际有效段
Reeds–Shepp连接成功 | 结束搜索

TODO：后续根据障碍物距离、目标距离和窄通道自动调整原语长度；碰撞积分步长仍保持0.05 m。

## 7碰撞检测与航向栅格模板 已确认

### 7.1 两级碰撞检测

1. 距离场快速放行：若车辆中心到最近障碍物距离大于扩大车体外接圆半径及离散补偿，则该点必然安全；
2. 旋转矩形模板精确检测：距离不足以保证安全时，检查当前航向下扩大车体覆盖的所有栅格。

### 7.2 外接圆仅用于“安全放行”

    R_outer = 0.5 × sqrt(CheckLength² + CheckWidth²)

`D > R_outer`表示安全；`D ≤ R_outer`只表示“无法确定”，不等于碰撞，必须进入矩形模板检查。

### 7.3 航向栅格模板的准备

航向分辨率5°时，在初始化阶段自动生成72组模板：

    0°, 5°, 10°, ... , 355°

每个模板保存旋转后的扩大车体矩形与地图栅格相交所对应的相对栅格偏移：

    GridOffset { Dx, Dy }

运行时把当前车辆中心转换为栅格坐标，选择最近航向模板，将模板中的相对偏移平移到当前位置并查询占据状态。

### 7.4 模板生成规则

- 模板以车体几何中心为原点；
- 车体尺寸包含安全余量与少量离散补偿；
- 只要栅格方块与旋转矩形有交集，就加入模板；
- 不能只判断栅格中心是否在矩形内部，否则可能漏掉边角碰撞；
- 真实车体始终是矩形，模板呈阶梯状只是栅格化结果。

### 7.5 每个积分点的处理

    读取距离场
      ↓
    可安全放行？——是→继续
      ↓否
    选择航向模板
      ↓
    平移模板并查询Occupied / Unknown / 越界
      ↓
    任一命中→碰撞；全部通过→安全

## 8搜索代价与启发函数 已确认

### 8.1 节点排序

    f(n) = g(n) + w_h × h(n)

第一版`HeuristicWeight = 1.3`。

### 8.2 累计代价

    g_new =
    g_parent
    + C_length
    + C_reverse
    + C_switch
    + C_curvature
    + C_curvatureChange
    + C_obstacle

- 路径长度是基础代价；
- 后退系数初值`ReversePenalty = 1.3`；
- 换向固定惩罚初值`GearSwitchPenalty = 2.0`；
- 曲率代价轻度抑制长期极限转向；
- 曲率变化代价偏好平缓动作序列；
- 障碍物代价让路径尽量远离障碍物。

### 8.3 双启发

    h = max(h_2D, h_RS)

- `h_2D`：从目标反向运行二维Dijkstra得到的绕障距离；
- `h_RS`：基于当前位姿、目标位姿和最小转弯半径的Reeds–Shepp距离。

所有权重必须外部可配置。

## 9终点判定、SQP精修与Reeds–Shepp 已确认

### 9.1 基本终点容差

    GoalPositionTolerance = 0.15 m
    GoalHeadingTolerance  = 5°

在每个0.05 m内部积分点检查位置和航向误差，避免越过目标。

### 9.2 容差到达与SQP

Hybrid A*可以在容差范围内结束，由后续SQP通过终端约束精确收敛到目标。但必须满足：

- 粗路径已包含正确绕障、倒车和换向拓扑；
- 当前终点、目标位姿及末端局部区域无碰撞；
- 终点附近存在足够局部调整空间；
- SQP只负责局部修正，不负责创造新的换向或改变绕障侧。

### 9.3 目标进入方向

    GoalDirectionConstraint = Any | Forward | Reverse

默认Any。该字段约束最后一段运动方向，不改变目标车头航向定义。

### 9.4 Reeds–Shepp模式

模式 | 连接失败后的行为 | 适用场景
--- | --- | ---
Disabled | 不尝试，满足容差即可结束 | 开阔终点、调试对照
Opportunistic | 失败继续搜索，满足容差仍可交SQP | 默认、普通导航
RequiredNearGoal | 失败不能按容差结束，必须继续找可连接节点 | 狭窄钻入、精确泊入

### 9.5 自动触发

    2–5 m：每扩展10个节点尝试一次
    ≤2 m：每扩展3个节点尝试一次
    采样碰撞步长：0.05 m

连接路径必须检查地图边界、Unknown、扩大车体碰撞、安全净空、最大曲率、方向约束及与当前曲率的衔接。

### 9.6 默认策略

    ReedsSheppMode = Opportunistic

无需人工每次开启，由程序自动触发。任务类型可覆盖默认模式。

## 10搜索回溯与原始稠密路径 已确认

### 10.1 回溯内容

搜索成功后从终止节点沿父节点索引回溯到起点，反转节点链，并恢复每条原语保存的0.05 m内部积分点。

### 10.2 不能只输出0.5 m搜索节点

只输出原语终点会丢失圆弧形状、碰撞检查细节和换向局部结构。因此必须复用搜索时已经计算的内部点。

### 10.3 路径点字段

    x, y
    heading, unwrappedHeading
    curvature
    direction
    arcLength
    clearance
    isGearSwitchPoint
    source

### 10.4 特殊处理

- 删除相邻原语重复边界点；
- 中途终止原语只保留真实有效积分点；
- Reeds–Shepp连接点追加到普通搜索路径之后；
- 机会式容差终点不允许在本步骤被“硬改”为目标点；
- 航向同时保存归一化值和连续解缠值；
- 累计弧长始终单调增加，方向单独保存；
- 按换向点划分Forward/Reverse连续段。

## 11关键点保留与弧长重采样 已确认

### 11.1 处理目标

原始0.05 m稠密路径不直接全部送入平滑和SQP。先保留关键结构，再按照累计弧长重采样。

### 11.2 必须保留的关键点

- 起点与终点；
- 所有换向点；
- 曲率等级变化点；
- 直线/弯道切换点；
- Reeds–Shepp分段和连接点；
- 障碍物附近最小净空点；
- 必要的明显航向变化点。

### 11.3 分方向重采样

每个Forward或Reverse段独立处理，不能跨越换向点插值。

### 11.4 推荐间距

    普通区域：0.10 m
    重点区域：0.05 m

以下区域采用0.05 m：

- 障碍距离小于0.50 m；
- |κ| ≥ 0.5 κmax；
- 目标2 m范围内；
- 换向点前后0.30 m；
- 曲率变化点前后0.25 m；
- Reeds–Shepp连接段与窄通道。

### 11.5 重采样后复核

新插值点必须重新执行车体碰撞检查。最终输出点可为0.10 m，但碰撞验证内部步长仍不得大于0.05 m。

## 12快速曲线平滑、局部QP兜底与最终校验 已确认

### 12.1 最终采用的分层方案

按Forward/Reverse方向段拆分
普通路径段：三次B样条快速平滑
局部短转角：分段三次Bézier
末端短连接：五次多项式（可选）
0.05 m完整碰撞、净空、曲率验证
失败局部：安全走廊约束QP
局部QP仍失败：回退第十一步原始可行路径

### 12.2 为什么不默认全路径QP

全路径多轮QP还需要走廊生成、曲率线性化和反复碰撞检测，可能与后续SQP功能重复并增加耗时。因此第一版采用快速曲线优先，仅对失败局部使用QP。

### 12.3 平滑结构约束

- 换向点固定，不能使用一条曲线跨越换向；
- 不改变绕障侧、前进/后退顺序和换向次数；
- Reeds–Shepp精确目标点固定；
- 容差终点不在本步骤强行修改为目标；
- 障碍物附近控制点移动范围收紧。

### 12.4 典型偏移范围

    普通区域最大偏移：0.10–0.15 m
    障碍物附近最大偏移：0.02–0.05 m
    局部QP问题区间：失败点前后各约1.0 m

### 12.5 航向与曲率

- 平滑后必须根据新几何路径重新计算切线与航向；
- 前进段车头航向与路径切线一致；
- 倒车段车头航向与路径切线相差π；
- 区分几何曲率与车辆控制曲率；
- 建议平滑后最大车辆曲率不超过`0.95 κmax`。

### 12.6 最终验证

- 0.05 m采样完整车体碰撞检测；
- Unknown与越界检查；
- 安全余量检查；
- 最大曲率与曲率变化检查；
- 起终点、换向点、方向分段和路径连续性检查。

## 13性能优化阶段 整体TODO

第十三步不作为第一版主流程实现要求。前12步完整跑通并建立正确性基线后，再进行性能剖析与针对性优化。

### 13.1 第一版仅保留防失控措施

- 较宽松的搜索超时；
- 最大扩展节点和最大生成节点限制；
- 取消请求机制；
- 各阶段基础耗时与节点数量统计。

### 13.2 后续性能TODO

1. 统计地图处理、Dijkstra、Hybrid A*、碰撞检测、Reeds–Shepp、回溯和平滑耗时；
2. 根据剖析结果识别真实瓶颈；
3. 优化Open List、Closed Set、节点内存与对象分配；
4. 预计算运动原语、航向模板、距离场和启发图；
5. 地图裁剪、路径热启动、增量更新、多分辨率搜索；
6. 最终验证平均、P95、最坏时间和100 ms达成率。

开发顺序：正确性 → 可行性 → 可复现性 → 性能剖析 → 针对性优化 → 100 ms验收。

## 14输入输出接口、错误状态与验收 已确认

### 14.1 推荐模块

    HybridAStarPlanner
    ├── MapProcessor
    ├── VehicleModel
    ├── MotionPrimitiveGenerator
    ├── CollisionChecker
    ├── HeuristicProvider
    ├── ReedsSheppConnector
    ├── GoalChecker
    ├── PathBacktracker
    ├── PathResampler
    ├── PathSmoother
    ├── PathValidator
    └── PlanningDiagnostics

### 14.2 主接口

    public interface IHybridAStarPlanner
    {
        PlanningResult Plan(PlanningRequest request);
    }

### 14.3 输入

- 二维占据栅格；
- 起点位姿与可选起始方向、起始曲率；
- 目标位姿与目标进入方向；
- 车辆尺寸、安全余量、最大曲率/最小转弯半径；
- Hybrid A*配置、Reeds–Shepp模式和平滑配置。

### 14.4 输出

输出不带时间的空间路径点和分段信息：

    X, Y
    Heading, UnwrappedHeading
    ArcLength
    GeometricCurvature
    VehicleCurvature
    Direction
    BodyClearance
    IsGearSwitchPoint
    Source

### 14.5 终点到达类型

    ExactByReedsShepp
    ReachedWithinTolerance
    ExactAfterDirectSearch

### 14.6 主要返回状态

    Success
    SuccessWithToleranceGoal
    SuccessWithSmoothingFallback
    InvalidRequest / InvalidMap / InvalidVehicleParameters
    StartOutsideMap / StartInCollision / StartInUnknownArea
    GoalOutsideMap / GoalInCollision / GoalInUnknownArea
    InvalidCurvatureConfiguration
    SearchTimeout / SearchNodeLimitExceeded / NoFeasiblePath
    ReedsSheppRequiredButFailed
    BacktrackingFailed / ResamplingFailed / FinalValidationFailed
    Cancelled / InternalError

### 14.7 第一版验收重点

- 有效场景能输出完整路径；
- 起点一致，终点满足第九步规则；
- 完整车体全程无碰撞且不进入Unknown；
- 最大曲率不超过车辆上限；
- 前进、倒车和换向结构正确；
- 回溯无断点和异常重复；
- 平滑失败可以回退原路径；
- 失败场景返回明确状态；
- 后续SQP能够读取并收敛。

## 附录A：第一版推荐配置参数

参数 | 推荐初值 | 说明
--- | --- | ---
MapResolution | 0.05 m | 读取地图配置，不写死
HeadingResolution | 5° | 72个航向模板
SafetyMargin | 0.03 m | 主要加在车体矩形
PrimitiveLength | 0.50 m | 第一版固定
IntegrationStep | 0.05 m | 碰撞与积分步长
CurvatureLevels | -1,-0.5,0,0.5,1 × κmax | 5级
GoalPositionTolerance | 0.15 m | Hybrid A*基础容差
GoalHeadingTolerance | 5° | Hybrid A*基础容差
HeuristicWeight | 1.3 | Weighted A*
ReversePenalty | 1.3 | 允许倒车但略微惩罚
GearSwitchPenalty | 2.0 | 减少频繁换向
ReedsSheppMode | Opportunistic | 默认自动机会式
AnalyticExpansionDistance | 5.0 m | 进入后周期尝试
NearGoalDistance | 2.0 m | 提高尝试频率
AnalyticExpansionInterval | 10节点 | 2–5 m
NearGoalInterval | 3节点 | ≤2 m
NormalResampleSpacing | 0.10 m | 普通区域
FineResampleSpacing | 0.05 m | 重点区域
FineObstacleDistance | 0.50 m | 小于此值加密
FineGoalDistance | 2.0 m | 目标附近加密
GearSwitchDenseRange | ±0.30 m | 换向点附近
CurvatureTransitionDenseRange | ±0.25 m | 曲率切换附近
CurvatureLimitRatio | 0.95 | 平滑后留控制余量
SearchTimeLimitMs | 3000–5000 ms | 第一版防失控，不是性能指标

## 附录B：总体伪代码

    PlanningResult Plan(request)
    {
        ValidateRequest(request);
        ValidateMap(request.Map);
        ResolveVehicleCurvatureLimit(request.Vehicle);
        ValidateStartAndGoal();

        PrepareDistanceFieldIfNeeded();
        PrepareHeadingFootprintTemplatesIfNeeded();
        BuildGoalDijkstraHeuristic();

        startNode = CreateStartNode();
        PushOpen(startNode);

        while (OpenList not empty)
        {
            CheckCancellationAndSafetyLimits();

            current = PopBestValidNode();

            if (ShouldTryReedsShepp(current))
            {
                rsPath = TryReedsShepp(current, goal);
                if (ValidateAnalyticPath(rsPath))
                    return BuildFinalResult(current, rsPath);
            }

            if (GoalChecker.CanTerminateByTolerance(current))
                return BuildFinalResult(current, noAnalyticPath);

            foreach (primitive in GenerateAllowedPrimitives(current))
            {
                integrated = IntegratePrimitive(
                    current,
                    primitive,
                    step = 0.05 m,
                    maxLength = 0.50 m);

                if (!integrated.Valid)
                    continue;

                child = CreateChildNode(integrated);
                if (!ImproveBestCost(child))
                    continue;

                PushOpen(child);
            }
        }

        return Failure(NoFeasiblePath);
    }

    BuildFinalResult(goalNode, analyticPath)
    {
        rawDense = BacktrackAndRestorePrimitivePoints(goalNode);
        AppendAnalyticPathIfAny(rawDense, analyticPath);

        segmented = SplitByMotionDirection(rawDense);
        resampled = PreserveKeyPointsAndResample(segmented);

        smoothed = TryFastSplineSmoothing(resampled);
        if (!ValidatePath(smoothed))
            smoothed = TryLocalCurveOrQPFallback(resampled);

        if (!ValidatePath(smoothed))
            smoothed = resampled;

        if (!ValidatePath(smoothed))
            return Failure(FinalValidationFailed);

        return Success(smoothed, diagnostics);
    }

## 附录C：建议开发里程碑

阶段 | 范围 | 完成标准
--- | --- | ---
M0 数据与工具 | Pose、地图、车辆参数、角度/坐标工具 | 单元测试通过
M1 运动学与碰撞 | 恒曲率积分、航向模板、矩形碰撞 | 可视化验证不同角度无漏检
M2 最小Hybrid A* | 无障碍、只前进、基础启发 | 稳定从起点到终点
M3 障碍与倒车 | Dijkstra、前进/倒车、换向代价 | 绕障与一次倒车场景通过
M4 终点模块 | 容差、GoalDirection、Reeds–Shepp三模式 | 普通与狭窄目标场景通过
M5 后处理 | 回溯、0.05 m稠密点、分段、重采样 | 路径结构无断点、换向明确
M6 平滑与复核 | B样条、Bézier、局部QP兜底 | 不改变拓扑，失败可回退
M7 接口与集成 | PlanningRequest/Result、状态、诊断 | 可接入SQP
M8 性能TODO | 剖析、优化、100 ms目标 | 在正确性基线后执行