# AMR 非结构化道路轨迹规划技术路线（面向 Agent）

## 1. 项目目标

开发一套面向非结构化道路四舵轮 AMR 的轨迹规划系统。

第一阶段采用车式运动模型，具体边界如下：

- 支持前进、倒车和换向；
- 暂不支持蟹行、纯横移和原地旋转；
- 车体航向始终表示车头朝向；
- 规划结果必须满足完整车体无碰撞、最大曲率约束和目标位姿要求；
- 最终输出可供底盘控制器跟踪的带时间轨迹。

整体技术路线分为四个主要阶段：

```mermaid
flowchart LR
    A[输入与地图构建] --> B[Hybrid A* 粗路径规划]
    B --> C[路径平滑与可行性处理]
    C --> D[SQP 轨迹优化]
    D --> E[输出可跟踪轨迹]
```

---

## 2. 总体模块划分

```text
Module 1：输入与地图构建
Module 2：Hybrid A* 粗路径规划
Module 3：路径平滑与可行性处理
Module 4：SQP 轨迹优化
Module 5：轨迹输出与底盘接口
```

各模块必须通过明确的数据结构传递结果，不允许后级模块直接读取前级模块内部状态。

---

# 3. Module 1：输入与地图构建

## 3.1 模块目标

根据起点、终点、车辆参数和环境障碍物信息，构建一张供全局搜索使用的任务级二维栅格地图。

地图范围至少覆盖：

- AMR 起点；
- 目标位置；
- 起点与目标之间的规划区域；
- 周围必要的安全边界。

当前阶段继续使用已有 `GridMapData` 作为地图数据来源，并通过适配器转换为 Hybrid A* 使用的规划地图。

---

## 3.2 输入数据

### 3.2.1 起点位姿

```text
StartPose:
    X
    Y
    Heading
    StartDirection
    StartVehicleCurvature
```

字段定义：

- `X、Y`：AMR 几何中心在世界坐标系中的位置；
- `Heading`：车头航向；
- `StartDirection`：`Forward`、`Reverse` 或 `Both`；
- `StartVehicleCurvature`：车辆当前转向曲率，未提供时默认为 0。

---

### 3.2.2 终点位姿

```text
GoalPose:
    X
    Y
    Heading
    GoalDirectionConstraint
```

目标进入方向约束：

```text
Any
Forward
Reverse
```

其中：

- `GoalHeading` 始终表示目标车头朝向；
- `GoalDirectionConstraint` 只约束最后一个连续运动段的运动方向。

---

### 3.2.3 车辆参数

```text
VehicleParameters:
    VehicleLength
    VehicleWidth
    MaxVehicleCurvature
    MinTurningRadius
```

第一阶段车辆参考点统一采用 AMR 几何中心。

---

### 3.2.4 环境障碍物

当前已接入：

```text
TwoLegDetect 两腿检测结果
```

后续扩展：

```text
点云障碍物
激光雷达障碍物
静态障碍物
动态障碍物
禁行区域
```

---

### 3.2.5 地图配置

```text
MapConfiguration:
    GridResolution
    MapMarginX
    MapMarginY
    ObstacleInflation
    MaximumCellCount
```

---

## 3.3 当前地图构建逻辑

```mermaid
flowchart TD
    A[读取 AMR 位姿与车身尺寸] --> B[读取目标位置与地图参数]
    B --> C[计算任务级地图边界]
    C --> D[创建 GridMapData]
    D --> E[读取 TwoLegDetect 障碍物]
    E --> F[车体系坐标转世界坐标]
    F --> G[圆形障碍物栅格化]
    G --> H[输出环境占据栅格]
```

当前地图边界由起点与目标位置共同决定：

```text
XMin = min(StartX, GoalX) - MapMarginX
XMax = max(StartX, GoalX) + MapMarginX

YMin = min(StartY, GoalY) - MapMarginY
YMax = max(StartY, GoalY) + MapMarginY
```

栅格尺寸：

```text
Cols = ceil((XMax - XMin) / Resolution)
Rows = ceil((YMax - YMin) / Resolution)
```

---

## 3.4 当前地图适配要求

现有地图格式为：

```text
单位：mm
数组：Cells[row, col]
状态：0 = 未标记占据，1 = Occupied
```

Hybrid A* 内部统一使用：

```text
单位：m
角度：rad
曲率：1/m
```

因此需要增加：

```text
PlanningMapAdapter
```

适配流程：

```mermaid
flowchart TD
    A[GridMapData] --> B[校验边界、尺寸与分辨率]
    B --> C[深拷贝 Occupancy]
    C --> D[毫米转换为米]
    D --> E[生成障碍物距离场]
    E --> F[输出 PlanningGridMap]
```

---

## 3.5 地图构建阶段的关键规则

### 3.5.1 规划地图不得包含 AMR 自身占据层

AMR 自身不能写入环境占据地图，否则 Hybrid A* 起点会直接被判定为碰撞。

正确关系：

```text
环境地图：
    只包含外部障碍物

AMR 自身：
    通过 VehiclePose、VehicleLength、VehicleWidth 单独描述
```

---

### 3.5.2 障碍物膨胀量必须明确

建议将：

```text
SafetyMarginMm
```

改为：

```text
ObstacleInflationMm
```

其唯一含义是：

> 障碍物写入地图前向外膨胀的距离。

必须避免：

```text
障碍物已经膨胀
+
车辆碰撞矩形再次增加同样余量
```

---

### 3.5.3 区分地图创建成功与地图可用于规划

```text
Succeeded：
    地图数据结构是否创建成功

PlanningReady：
    当前地图是否满足规划输入要求

PlanningBlockReason：
    地图为何不能用于规划
```

例如：

```text
TwoLegDetect 正常：
    Succeeded = true
    PlanningReady = true

TwoLegDetect 无结果：
    Succeeded = true
    PlanningReady = false
```

---

## 3.6 地图输出

```text
PlanningGridMap:
    XMin
    XMax
    YMin
    YMax
    Resolution
    Rows
    Cols
    Occupancy[row, col]
    ObstacleDistance[row, col]
    ObstacleInflation
    InputSource
    PlanningReady
    MapVersion
```

当前第一版状态定义：

```text
Occupancy = 0：
    当前地图层未标记为占据

Occupancy = 1：
    占据

地图外部：
    按占据处理
```

---

# 4. Module 2：Hybrid A* 粗路径规划

## 4.1 模块目标

在任务级栅格地图中搜索一条：

- 从起点到目标附近；
- 完整车体无碰撞；
- 满足最大曲率限制；
- 支持前进、倒车和换向；
- 具有正确绕障拓扑；
- 可作为后续平滑和 SQP 优化初值；

的空间粗路径。

Hybrid A* 负责决定：

```text
从哪里走
绕障碍物哪一侧走
什么时候前进
什么时候倒车
在哪里换向
以什么车头航向接近目标
```

Hybrid A* 不负责：

```text
速度
加速度
角速度
时间戳
舵轮角度
轮速
```

---

## 4.2 搜索状态

连续状态：

```text
State:
    x
    y
    theta
    direction
    curvatureIndex
```

字段定义：

- `x、y`：AMR 几何中心位置；
- `theta`：车头航向；
- `direction`：`Forward` 或 `Reverse`；
- `curvatureIndex`：当前离散车辆曲率等级。

Closed Set 键：

```text
GridKey:
    ix
    iy
    iHeading
    direction
    curvatureIndex
```

---

## 4.3 运动模型

定义：

```text
directionSign = +1：Forward
directionSign = -1：Reverse
```

运动积分：

```text
dx/ds = directionSign × cos(theta)
dy/ds = directionSign × sin(theta)
dtheta/ds = directionSign × VehicleCurvature
```

其中：

```text
Heading：
    始终表示车头朝向

ArcLength：
    始终非负并单调增加
```

---

## 4.4 运动原语

第一版离散曲率集合：

```text
-kappaMax
-0.5 × kappaMax
0
+0.5 × kappaMax
+kappaMax
```

每个曲率均支持：

```text
Forward
Reverse
```

第一版建议参数：

```text
PrimitiveLength = 0.50 m
IntegrationStep = 0.05 m
```

每条原语按 0.05 m 步长积分，并在每个内部积分点执行完整车体碰撞检测。

---

## 4.5 搜索约束

每个候选原语必须检查：

```text
地图边界
Occupied 栅格
完整车辆矩形碰撞
安全净空
最大车辆曲率
允许的运动方向
曲率等级切换规则
目标进入方向
```

第一阶段暂不直接约束：

```text
速度
加速度
曲率时间变化率
舵轮转向速度
```

但需要通过代价项减少极端曲率跳变。

---

## 4.6 搜索代价

```text
g =
    路径长度代价
  + 倒车代价
  + 换向代价
  + 曲率代价
  + 曲率变化代价
  + 障碍物距离代价
```

启发函数：

```text
h = max(h2D, hRS)
```

其中：

```text
h2D：
    基于二维 Dijkstra 的绕障距离

hRS：
    基于 Reeds–Shepp 模型的非完整约束距离
```

节点排序：

```text
f = g + HeuristicWeight × h
```

---

## 4.7 终点附近 Reeds–Shepp 连接

```mermaid
flowchart TD
    A[当前节点接近目标] --> B[生成 Reeds-Shepp 候选]
    B --> C[按 0.05 m 采样]
    C --> D[完整车体碰撞检查]
    D --> E[最大曲率与目标方向检查]
    E -->|成功| F[精确连接目标]
    E -->|失败| G[继续 Hybrid A* 搜索]
```

支持模式：

```text
Disabled
Opportunistic
RequiredNearGoal
```

停车、钻入和精确泊入任务优先使用：

```text
RequiredNearGoal
```

---

## 4.8 Hybrid A* 输出

```text
CoarsePathPoint:
    X
    Y
    Heading
    UnwrappedHeading
    ArcLength
    Direction
    VehicleCurvature
    GeometricCurvature
    BodyClearance
    IsGearSwitchPoint
    Source
```

路径按运动方向拆分：

```text
PathSegment:
    SegmentIndex
    Direction
    StartIndex
    EndIndex
    StartsAtGearSwitch
    EndsAtGearSwitch
```

---

# 5. Module 3：路径平滑与可行性处理

## 5.1 模块目标

消除 Hybrid A* 粗路径中的离散折线和不连续曲率，生成更平滑的空间参考路径。

该阶段只优化几何形状，不进行时间参数化。

不得改变：

```text
绕障侧
前进与倒车顺序
换向次数
方向段拓扑
目标进入方向
```

---

## 5.2 处理流程

```mermaid
flowchart TD
    A[Hybrid A* 稠密粗路径] --> B[删除重复点]
    B --> C[按 Forward/Reverse 分段]
    C --> D[保留关键点]
    D --> E[按弧长重采样]
    E --> F[几何平滑]
    F --> G[完整可行性检查]
    G --> H[平滑空间路径]
```

---

## 5.3 必须保留的关键点

```text
起点
终点
换向点
曲率等级切换点
直线与弯道切换点
Reeds–Shepp 连接点
局部最小净空点
明显航向变化点
```

禁止跨换向点进行单条曲线拟合。

---

## 5.4 平滑方法

### 5.4.1 贝塞尔曲线

适用于：

```text
局部短转角
折线拐点快速平滑
局部连接
```

特点：

```text
实现简单
计算速度快
局部形状易控制
```

---

### 5.4.2 五次多项式

适用于：

```text
末端短距离连接
具有明确起终点位置、航向和曲率的局部段
```

特点：

```text
可同时约束位置、航向和曲率
适合末端姿态调整
```

---

### 5.4.3 B 样条

适用于：

```text
较长的普通路径段
多点连续平滑
降低局部折线感
```

建议采用三次 B 样条作为第一版普通路径段的主平滑方法。

---

## 5.5 可行性检查

平滑后的路径必须重新检查：

```text
完整车体碰撞
地图边界
Unknown / Occupied
最小安全净空
最大车辆曲率
路径位置连续性
路径切线连续性
换向结构
目标进入方向
```

若平滑失败：

```text
局部重新调整
    ↓
局部 QP 兜底
    ↓
仍失败则回退原始可行粗路径
```

---

## 5.6 输出

```text
SmoothedPathPoint:
    X
    Y
    Heading
    ArcLength
    Direction
    VehicleCurvature
    GeometricCurvature
    BodyClearance
    Source
```

该输出仍然不包含：

```text
速度
加速度
时间戳
```

---

# 6. Module 4：SQP 轨迹优化

## 6.1 模块目标

以平滑空间路径作为参考线，通过 Frenet 坐标系和 SQP 优化，联合完成：

- 横向几何调整；
- 曲率优化；
- 纵向速度规划；
- 加速度约束；
- 换向停车约束；
- 终点精确收敛；
- 时间参数化。

最终生成可供车辆控制器跟踪的带时间轨迹。

---

## 6.2 Frenet 坐标转换

将平滑路径作为参考线，建立 Frenet 坐标系：

```text
s：
    沿参考线的累计弧长

l：
    相对于参考线的横向偏移
```

转换后，将原二维路径问题分解为：

```text
横向 l-s 问题
纵向 s-t 问题
```

---

## 6.3 横向 l-s 优化

主要优化变量：

```text
横向偏移 l
横向一阶导数 dl/ds
横向二阶导数 d²l/ds²
曲率
曲率变化
```

优化目标：

```text
尽量贴近参考线
避开障碍物
保持曲率平滑
满足最大曲率
满足安全走廊约束
精确到达目标横向位置
```

---

## 6.4 纵向 s-t 优化

主要优化变量：

```text
s(t)
速度 v(t)
加速度 a(t)
加加速度 jerk(t)
```

优化目标：

```text
满足速度上限
满足加速度约束
满足减速度约束
提高运动平顺性
减少总时间
在换向点停车
在终点停车
```

---

## 6.5 SQP 联合优化

SQP 负责迭代求解：

```text
路径位置
航向
曲率
速度
加速度
角速度
时间戳
```

约束至少包括：

```text
障碍物安全约束
车辆曲率约束
速度约束
加速度约束
角速度约束
换向点速度为 0
终点位置约束
终点航向约束
终点速度约束
```

SQP 不负责：

```text
重新决定绕障侧
凭空新增换向
改变 Hybrid A* 已确定的拓扑结构
```

---

## 6.6 SQP 输出

```text
TrajectoryPoint:
    X
    Y
    Heading
    Time
    ArcLength
    Direction
    Velocity
    Acceleration
    AngularVelocity
    VehicleCurvature
    GeometricCurvature
```

该轨迹可以直接提供给后续底盘控制与轨迹跟踪模块。

---

# 7. Module 5：轨迹输出与底盘接口

## 7.1 输出内容

```text
TrajectoryResult:
    PlanningStatus
    GoalReachType
    PathProcessingResult
    TrajectoryPoints
    DirectionSegments
    Diagnostics
```

---

## 7.2 底盘控制前的处理

轨迹规划器不直接输出四轮舵角和轮速。

底盘控制层负责：

```text
轨迹点
    ↓
车辆状态误差计算
    ↓
轨迹跟踪控制
    ↓
底盘运动学逆解
    ↓
四轮舵角与轮速
```

---

# 8. Agent 开发顺序

## 阶段 M0：数据结构与公共工具

实现：

```text
Pose2D
VehicleParameters
GoalPose
GridMapData Adapter
角度归一化
坐标变换
单位转换
```

完成标准：

```text
单元测试通过
毫米与米转换无误
航向角定义统一
```

---

## 阶段 M1：地图适配与碰撞检测

实现：

```text
PlanningMapAdapter
PlanningGridMap
距离场
旋转矩形碰撞检测
地图边界检测
```

完成标准：

```text
AMR 自身不会出现在环境占据图中
不同航向下完整车体碰撞检测正确
```

---

## 阶段 M2：最小 Hybrid A*

实现：

```text
只前进
无障碍
恒曲率运动原语
基础启发函数
```

完成标准：

```text
稳定从起点搜索到目标
```

---

## 阶段 M3：障碍物、倒车和换向

实现：

```text
Dijkstra 启发
Forward / Reverse
换向代价
方向分段
```

完成标准：

```text
单障碍绕行成功
需要倒车的场景成功
换向点正确输出
```

---

## 阶段 M4：目标终止与 Reeds–Shepp

实现：

```text
目标位置容差
目标航向容差
目标进入方向
Reeds–Shepp 三种模式
```

完成标准：

```text
普通目标和狭窄目标场景均能正确终止
```

---

## 阶段 M5：路径后处理

实现：

```text
父节点回溯
内部积分点恢复
关键点保留
弧长重采样
```

完成标准：

```text
路径无断点
换向结构明确
方向段正确
```

---

## 阶段 M6：平滑与可行性检查

实现：

```text
B 样条
贝塞尔曲线
五次多项式
完整碰撞复核
失败回退
```

完成标准：

```text
路径曲率明显改善
不改变原有拓扑
平滑失败可回退
```

---

## 阶段 M7：SQP 轨迹优化

实现：

```text
Frenet 坐标转换
l-s 优化
s-t 优化
速度和加速度约束
换向停车约束
终点精确约束
```

完成标准：

```text
输出带时间的可跟踪轨迹
终点位置与航向满足要求
换向点速度为 0
```

---

## 阶段 M8：性能优化

在前述流程完整跑通后再开展：

```text
性能剖析
Open List 优化
Closed Set 优化
节点内存优化
地图裁剪
预计算运动原语
预计算碰撞模板
启发图复用
路径热启动
增量规划
```

---

# 9. 核心技术边界

## Hybrid A* 负责

```text
拓扑
绕障侧
前进与倒车结构
换向位置
基本运动学可行性
粗目标位姿接近
```

## 路径平滑负责

```text
消除折线
改善局部曲率
保持拓扑不变
输出连续空间参考路径
```

## SQP 负责

```text
终点精确收敛
速度规划
加速度规划
角速度规划
时间参数化
动态可跟踪性
```

## 底盘控制负责

```text
四轮舵角
四轮轮速
轨迹跟踪
执行器控制
```

---

# 10. 最终技术路线总结

```text
起点位姿 + 目标位姿 + 车辆参数 + 障碍物数据
        ↓
构建任务级环境占据地图
        ↓
通过 PlanningMapAdapter 转换为规划地图
        ↓
Hybrid A* 搜索满足运动学约束的粗路径
        ↓
Reeds–Shepp 处理终点位姿连接
        ↓
回溯、重采样和路径方向分段
        ↓
B 样条 / 贝塞尔 / 五次多项式进行几何平滑
        ↓
重新执行完整车体碰撞和曲率检查
        ↓
建立 Frenet 参考坐标系
        ↓
横向 l-s 优化路径形状和曲率
        ↓
纵向 s-t 优化速度、加速度和停车决策
        ↓
SQP 联合求解可跟踪轨迹
        ↓
输出位置、航向、速度、加速度、角速度和时间戳
        ↓
底盘控制器进行四轮舵角和轮速逆解
```

该技术路线的核心原则是：

> Hybrid A* 解决“怎么走”，路径平滑解决“走得是否顺”，SQP 解决“以什么速度和时间走”，底盘控制解决“四个舵轮和车轮如何执行”。