Chapter-2 低速非结构化场景中的单一车辆决策与规划方法

本章介绍在低速非结构化场景中的最优化规划问题的构建和决策规划的计算流程。作者着重介绍了最优控制问题的一般形式和数值求解方法，以及使用A*求解初始值。

最优化问题为连续Bolza型问题，通过离散化通过非线性求解器计算。由于初始解既可以提高非线性规划问题的求解效率，又可以避免非线性规划限于局部最优解，因此先使用全局规划方法得到粗糙的轨迹作为决策轨迹，并作为非线性规划的初始解，非线性规划的结果最为最终规划的轨迹。

1. 轨迹规划命题的构建

车辆的轨迹规划任务是指在车辆的起始运动状态与终止时刻运动状态之间计算出满足符合约束调节的行驶轨迹，其中约束条件主要包括车辆内在的运动能力限制以及与外部环境相关的碰撞躲避限制。另一方面，满足上述约束条件的轨迹往往不止一条，应通过某一指标筛选优质的轨迹作为最终结果。鉴于轨迹规划任务存在硬约束条件以及用于寻优的指标式，因此适合采用最优控制问题的形式来描述。采用Bolza型最优控制问题形式。

$$J=\Phi (x(0),x(t_f),t_f)+{\int }_0^{t_f}L(x(t),u(t),t)dt\text{\hspace{1em}(1-1)}$$

其中，约束条件有：

• 系统动态方程约束；
• 两点边值约束；
• 流形约束(状态变量约束、控制变量约束、碰撞躲避约束)；

比如在泊车轨迹规划中，对终止时刻的车辆姿态角$\theta (t_f$