17 KiB
17 KiB
AMR 非结构化道路轨迹规划技术路线（面向 Agent）
1. 项目目标
开发一套面向非结构化道路四舵轮 AMR 的轨迹规划系统。
第一阶段采用车式运动模型，具体边界如下：
- 支持前进、倒车和换向；
- 暂不支持蟹行、纯横移和原地旋转；
- 车体航向始终表示车头朝向；
- 规划结果必须满足完整车体无碰撞、最大曲率约束和目标位姿要求；
- 最终输出可供底盘控制器跟踪的带时间轨迹。
整体技术路线分为四个主要阶段：
flowchart LR
A[输入与地图构建] --> B[Hybrid A* 粗路径规划]
B --> C[路径平滑与可行性处理]
C --> D[SQP 轨迹优化]
D --> E[输出可跟踪轨迹]
2. 总体模块划分
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 起点位姿
StartPose:
X
Y
Heading
StartDirection
StartVehicleCurvature
字段定义：
X、Y：AMR 几何中心在世界坐标系中的位置；Heading：车头航向；StartDirection：Forward、Reverse或Both；StartVehicleCurvature：车辆当前转向曲率，未提供时默认为 0。
3.2.2 终点位姿
GoalPose:
X
Y
Heading
GoalDirectionConstraint
目标进入方向约束：
Any
Forward
Reverse
其中：
GoalHeading始终表示目标车头朝向；GoalDirectionConstraint只约束最后一个连续运动段的运动方向。
3.2.3 车辆参数
VehicleParameters:
VehicleLength
VehicleWidth
MaxVehicleCurvature
MinTurningRadius
第一阶段车辆参考点统一采用 AMR 几何中心。
3.2.4 环境障碍物
当前已接入：
TwoLegDetect 两腿检测结果
后续扩展：
点云障碍物
激光雷达障碍物
静态障碍物
动态障碍物
禁行区域
3.2.5 地图配置
MapConfiguration:
GridResolution
MapMarginX
MapMarginY
ObstacleInflation
MaximumCellCount
3.3 当前地图构建逻辑
flowchart TD
A[读取 AMR 位姿与车身尺寸] --> B[读取目标位置与地图参数]
B --> C[计算任务级地图边界]
C --> D[创建 GridMapData]
D --> E[读取 TwoLegDetect 障碍物]
E --> F[车体系坐标转世界坐标]
F --> G[圆形障碍物栅格化]
G --> H[输出环境占据栅格]
当前地图边界由起点与目标位置共同决定：
XMin = min(StartX, GoalX) - MapMarginX
XMax = max(StartX, GoalX) + MapMarginX
YMin = min(StartY, GoalY) - MapMarginY
YMax = max(StartY, GoalY) + MapMarginY
栅格尺寸：
Cols = ceil((XMax - XMin) / Resolution)
Rows = ceil((YMax - YMin) / Resolution)
3.4 当前地图适配要求
现有地图格式为：
单位：mm
数组：Cells[row, col]
状态：0 = 未标记占据，1 = Occupied
Hybrid A* 内部统一使用：
单位：m
角度：rad
曲率：1/m
因此需要增加：
PlanningMapAdapter
适配流程：
flowchart TD
A[GridMapData] --> B[校验边界、尺寸与分辨率]
B --> C[深拷贝 Occupancy]
C --> D[毫米转换为米]
D --> E[生成障碍物距离场]
E --> F[输出 PlanningGridMap]
3.5 地图构建阶段的关键规则
3.5.1 规划地图不得包含 AMR 自身占据层
AMR 自身不能写入环境占据地图，否则 Hybrid A* 起点会直接被判定为碰撞。
正确关系：
环境地图：
只包含外部障碍物
AMR 自身：
通过 VehiclePose、VehicleLength、VehicleWidth 单独描述
3.5.2 障碍物膨胀量必须明确
建议将：
SafetyMarginMm
改为：
ObstacleInflationMm
其唯一含义是：
障碍物写入地图前向外膨胀的距离。
必须避免：
障碍物已经膨胀
+
车辆碰撞矩形再次增加同样余量
3.5.3 区分地图创建成功与地图可用于规划
Succeeded：
地图数据结构是否创建成功
PlanningReady：
当前地图是否满足规划输入要求
PlanningBlockReason：
地图为何不能用于规划
例如：
TwoLegDetect 正常：
Succeeded = true
PlanningReady = true
TwoLegDetect 无结果：
Succeeded = true
PlanningReady = false
3.6 地图输出
PlanningGridMap:
XMin
XMax
YMin
YMax
Resolution
Rows
Cols
Occupancy[row, col]
ObstacleDistance[row, col]
ObstacleInflation
InputSource
PlanningReady
MapVersion
当前第一版状态定义：
Occupancy = 0：
当前地图层未标记为占据
Occupancy = 1：
占据
地图外部：
按占据处理
4. Module 2：Hybrid A* 粗路径规划
4.1 模块目标
在任务级栅格地图中搜索一条：
- 从起点到目标附近；
- 完整车体无碰撞；
- 满足最大曲率限制；
- 支持前进、倒车和换向；
- 具有正确绕障拓扑；
- 可作为后续平滑和 SQP 优化初值；
的空间粗路径。
Hybrid A* 负责决定：
从哪里走
绕障碍物哪一侧走
什么时候前进
什么时候倒车
在哪里换向
以什么车头航向接近目标
Hybrid A* 不负责：
速度
加速度
角速度
时间戳
舵轮角度
轮速
4.2 搜索状态
连续状态：
State:
x
y
theta
direction
curvatureIndex
字段定义：
x、y：AMR 几何中心位置；theta：车头航向；direction：Forward或Reverse；curvatureIndex：当前离散车辆曲率等级。
Closed Set 键：
GridKey:
ix
iy
iHeading
direction
curvatureIndex
4.3 运动模型
定义：
directionSign = +1：Forward
directionSign = -1：Reverse
运动积分：
dx/ds = directionSign × cos(theta)
dy/ds = directionSign × sin(theta)
dtheta/ds = directionSign × VehicleCurvature
其中：
Heading：
始终表示车头朝向
ArcLength：
始终非负并单调增加
4.4 运动原语
第一版离散曲率集合：
-kappaMax
-0.5 × kappaMax
0
+0.5 × kappaMax
+kappaMax
每个曲率均支持：
Forward
Reverse
第一版建议参数：
PrimitiveLength = 0.50 m
IntegrationStep = 0.05 m
每条原语按 0.05 m 步长积分，并在每个内部积分点执行完整车体碰撞检测。
4.5 搜索约束
每个候选原语必须检查：
地图边界
Occupied 栅格
完整车辆矩形碰撞
安全净空
最大车辆曲率
允许的运动方向
曲率等级切换规则
目标进入方向
第一阶段暂不直接约束：
速度
加速度
曲率时间变化率
舵轮转向速度
但需要通过代价项减少极端曲率跳变。
4.6 搜索代价
g =
路径长度代价
+ 倒车代价
+ 换向代价
+ 曲率代价
+ 曲率变化代价
+ 障碍物距离代价
启发函数：
h = max(h2D, hRS)
其中：
h2D：
基于二维 Dijkstra 的绕障距离
hRS：
基于 Reeds–Shepp 模型的非完整约束距离
节点排序：
f = g + HeuristicWeight × h
4.7 终点附近 Reeds–Shepp 连接
flowchart TD
A[当前节点接近目标] --> B[生成 Reeds-Shepp 候选]
B --> C[按 0.05 m 采样]
C --> D[完整车体碰撞检查]
D --> E[最大曲率与目标方向检查]
E -->|成功| F[精确连接目标]
E -->|失败| G[继续 Hybrid A* 搜索]
支持模式：
Disabled
Opportunistic
RequiredNearGoal
停车、钻入和精确泊入任务优先使用：
RequiredNearGoal
4.8 Hybrid A* 输出
CoarsePathPoint:
X
Y
Heading
UnwrappedHeading
ArcLength
Direction
VehicleCurvature
GeometricCurvature
BodyClearance
IsGearSwitchPoint
Source
路径按运动方向拆分：
PathSegment:
SegmentIndex
Direction
StartIndex
EndIndex
StartsAtGearSwitch
EndsAtGearSwitch
5. Module 3：路径平滑与可行性处理
5.1 模块目标
消除 Hybrid A* 粗路径中的离散折线和不连续曲率，生成更平滑的空间参考路径。
该阶段只优化几何形状，不进行时间参数化。
不得改变：
绕障侧
前进与倒车顺序
换向次数
方向段拓扑
目标进入方向
5.2 处理流程
flowchart TD
A[Hybrid A* 稠密粗路径] --> B[删除重复点]
B --> C[按 Forward/Reverse 分段]
C --> D[保留关键点]
D --> E[按弧长重采样]
E --> F[几何平滑]
F --> G[完整可行性检查]
G --> H[平滑空间路径]
5.3 必须保留的关键点
起点
终点
换向点
曲率等级切换点
直线与弯道切换点
Reeds–Shepp 连接点
局部最小净空点
明显航向变化点
禁止跨换向点进行单条曲线拟合。
5.4 平滑方法
5.4.1 贝塞尔曲线
适用于：
局部短转角
折线拐点快速平滑
局部连接
特点：
实现简单
计算速度快
局部形状易控制
5.4.2 五次多项式
适用于：
末端短距离连接
具有明确起终点位置、航向和曲率的局部段
特点：
可同时约束位置、航向和曲率
适合末端姿态调整
5.4.3 B 样条
适用于：
较长的普通路径段
多点连续平滑
降低局部折线感
建议采用三次 B 样条作为第一版普通路径段的主平滑方法。
5.5 可行性检查
平滑后的路径必须重新检查：
完整车体碰撞
地图边界
Unknown / Occupied
最小安全净空
最大车辆曲率
路径位置连续性
路径切线连续性
换向结构
目标进入方向
若平滑失败：
局部重新调整
↓
局部 QP 兜底
↓
仍失败则回退原始可行粗路径
5.6 输出
SmoothedPathPoint:
X
Y
Heading
ArcLength
Direction
VehicleCurvature
GeometricCurvature
BodyClearance
Source
该输出仍然不包含：
速度
加速度
时间戳
6. Module 4：SQP 轨迹优化
6.1 模块目标
以平滑空间路径作为参考线，通过 Frenet 坐标系和 SQP 优化，联合完成：
- 横向几何调整；
- 曲率优化；
- 纵向速度规划；
- 加速度约束；
- 换向停车约束；
- 终点精确收敛；
- 时间参数化。
最终生成可供车辆控制器跟踪的带时间轨迹。
6.2 Frenet 坐标转换
将平滑路径作为参考线，建立 Frenet 坐标系：
s：
沿参考线的累计弧长
l：
相对于参考线的横向偏移
转换后，将原二维路径问题分解为：
横向 l-s 问题
纵向 s-t 问题
6.3 横向 l-s 优化
主要优化变量：
横向偏移 l
横向一阶导数 dl/ds
横向二阶导数 d²l/ds²
曲率
曲率变化
优化目标：
尽量贴近参考线
避开障碍物
保持曲率平滑
满足最大曲率
满足安全走廊约束
精确到达目标横向位置
6.4 纵向 s-t 优化
主要优化变量：
s(t)
速度 v(t)
加速度 a(t)
加加速度 jerk(t)
优化目标：
满足速度上限
满足加速度约束
满足减速度约束
提高运动平顺性
减少总时间
在换向点停车
在终点停车
6.5 SQP 联合优化
SQP 负责迭代求解：
路径位置
航向
曲率
速度
加速度
角速度
时间戳
约束至少包括：
障碍物安全约束
车辆曲率约束
速度约束
加速度约束
角速度约束
换向点速度为 0
终点位置约束
终点航向约束
终点速度约束
SQP 不负责：
重新决定绕障侧
凭空新增换向
改变 Hybrid A* 已确定的拓扑结构
6.6 SQP 输出
TrajectoryPoint:
X
Y
Heading
Time
ArcLength
Direction
Velocity
Acceleration
AngularVelocity
VehicleCurvature
GeometricCurvature
该轨迹可以直接提供给后续底盘控制与轨迹跟踪模块。
7. Module 5：轨迹输出与底盘接口
7.1 输出内容
TrajectoryResult:
PlanningStatus
GoalReachType
PathProcessingResult
TrajectoryPoints
DirectionSegments
Diagnostics
7.2 底盘控制前的处理
轨迹规划器不直接输出四轮舵角和轮速。
底盘控制层负责：
轨迹点
↓
车辆状态误差计算
↓
轨迹跟踪控制
↓
底盘运动学逆解
↓
四轮舵角与轮速
8. Agent 开发顺序
阶段 M0：数据结构与公共工具
实现：
Pose2D
VehicleParameters
GoalPose
GridMapData Adapter
角度归一化
坐标变换
单位转换
完成标准：
单元测试通过
毫米与米转换无误
航向角定义统一
阶段 M1：地图适配与碰撞检测
实现：
PlanningMapAdapter
PlanningGridMap
距离场
旋转矩形碰撞检测
地图边界检测
完成标准：
AMR 自身不会出现在环境占据图中
不同航向下完整车体碰撞检测正确
阶段 M2：最小 Hybrid A*
实现：
只前进
无障碍
恒曲率运动原语
基础启发函数
完成标准：
稳定从起点搜索到目标
阶段 M3：障碍物、倒车和换向
实现：
Dijkstra 启发
Forward / Reverse
换向代价
方向分段
完成标准：
单障碍绕行成功
需要倒车的场景成功
换向点正确输出
阶段 M4：目标终止与 Reeds–Shepp
实现：
目标位置容差
目标航向容差
目标进入方向
Reeds–Shepp 三种模式
完成标准：
普通目标和狭窄目标场景均能正确终止
阶段 M5：路径后处理
实现：
父节点回溯
内部积分点恢复
关键点保留
弧长重采样
完成标准：
路径无断点
换向结构明确
方向段正确
阶段 M6：平滑与可行性检查
实现：
B 样条
贝塞尔曲线
五次多项式
完整碰撞复核
失败回退
完成标准：
路径曲率明显改善
不改变原有拓扑
平滑失败可回退
阶段 M7：SQP 轨迹优化
实现：
Frenet 坐标转换
l-s 优化
s-t 优化
速度和加速度约束
换向停车约束
终点精确约束
完成标准：
输出带时间的可跟踪轨迹
终点位置与航向满足要求
换向点速度为 0
阶段 M8：性能优化
在前述流程完整跑通后再开展：
性能剖析
Open List 优化
Closed Set 优化
节点内存优化
地图裁剪
预计算运动原语
预计算碰撞模板
启发图复用
路径热启动
增量规划
9. 核心技术边界
Hybrid A* 负责
拓扑
绕障侧
前进与倒车结构
换向位置
基本运动学可行性
粗目标位姿接近
路径平滑负责
消除折线
改善局部曲率
保持拓扑不变
输出连续空间参考路径
SQP 负责
终点精确收敛
速度规划
加速度规划
角速度规划
时间参数化
动态可跟踪性
底盘控制负责
四轮舵角
四轮轮速
轨迹跟踪
执行器控制
10. 最终技术路线总结
起点位姿 + 目标位姿 + 车辆参数 + 障碍物数据
↓
构建任务级环境占据地图
↓
通过 PlanningMapAdapter 转换为规划地图
↓
Hybrid A* 搜索满足运动学约束的粗路径
↓
Reeds–Shepp 处理终点位姿连接
↓
回溯、重采样和路径方向分段
↓
B 样条 / 贝塞尔 / 五次多项式进行几何平滑
↓
重新执行完整车体碰撞和曲率检查
↓
建立 Frenet 参考坐标系
↓
横向 l-s 优化路径形状和曲率
↓
纵向 s-t 优化速度、加速度和停车决策
↓
SQP 联合求解可跟踪轨迹
↓
输出位置、航向、速度、加速度、角速度和时间戳
↓
底盘控制器进行四轮舵角和轮速逆解
该技术路线的核心原则是：
Hybrid A* 解决“怎么走”，路径平滑解决“走得是否顺”，SQP 解决“以什么速度和时间走”，底盘控制解决“四个舵轮和车轮如何执行”。