目标 给点起点和终点 先求出在起点做一次state lattice planning 的轨迹 再求在轨迹基础上从轨迹终点到给定终点的OBVP问题
1. state lattice
将控制量a离散化 离散成discretize_step份
分别再xyz上进行积分求得p v,得到轨迹
2. OBVP
将第一段的轨迹终点作为OBVP的起点 给定终点作为OBVP终点 进行最优边界值问题求解
起点 p v
终点p
OBVP求解过程
- 建模:系统建模 问题建模(优化目标函数) 状态表示 输入表示
- 求解:定义哈密尔顿方程 用庞特里亚金极小值原理求解最小代价函数
- 求解细节问题
- 求 alpha1 alpha2 alpha3 beta1 beta2 beta3
- 建立矩阵方程 param = H × deltaPv;
- J 已经推导出 直接将参数带入 J
- 求得最优T的解
- 回到算法中 将T带入参数中所有alpha beta J都可求得