1195 lines
17 KiB
Markdown
1195 lines
17 KiB
Markdown
|
# 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 解决“以什么速度和时间走”，底盘控制解决“四个舵轮和车轮如何执行”。
|