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

0. 总体概览

0.1 总体职责边界

0.2 总体数据流

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

1.1 车辆运动模式

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

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

1.2 Hybrid A*最小输出

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

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

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

1.3 成功判据

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

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

2.1 地图生成流程

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；
• 第一版采用恒定曲率原语。

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

6.1 固定参数

PrimitiveLength = 0.50 m
IntegrationStep = 0.05 m

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

6.2 终止条件

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模式

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 最终采用的分层方案

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达成率。

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：第一版推荐配置参数

附录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：建议开发里程碑