当前位置：网站首页>学习笔记12--路径-速度分解法之局部路径搜索

学习笔记12--路径-速度分解法之局部路径搜索

2022-07-31 14:39:00 【FUXI_Willard】

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》。
此专栏是关于《自动驾驶汽车决策与控制》书籍的笔记.

2.汽车局部轨迹规划

2.3 路径-速度分解法

路径-速度分解法将路径规划与速度规划分开进行，百度Apollo中的EM算法，分别应用了动态规划算法进行路径规划和二次规划算法进行速度规划，算法架构如下图所示：

2.3.1 局部路径搜索

基本算法介绍
1. 启发式搜索算法
  1. $A^*$ 算法
    - $A^*$ 算法是一种启发式搜索算法，基于深度优先算法(Depth First Search，DFS)和广度优先算法(Breadth First Search，BFS)的一种融合算法，按照一定原则确定如何选取下一个结点；
    - 启发式搜索算法指：从起点出发，先寻找起点相邻的栅格，判断它是否是最好的位置，基于这个最好的栅格再往外向其相邻的栅格扩展，找到一个此时最好的位置，通过这样一步步逼近目标点，减少盲目的搜索，提高可行性和搜索效率；
    - 深度优先搜索算法思想：搜索算法从起点开始进行搜索(初始状态下待搜索区域内所有结点均未被访问)，与周围所有邻点进行比较，选择其中距离终点最近的点进行存储，然后再以该邻点为基础对比周围未被访问的所有邻点，仍然选择距离终点最近的邻点存储；若访问结点到达终点或访问完所有结点仍未到终点，则视为搜索失败，成功搜索所存储的结点连接而成的路径即为起点到终点的路径；
    - 广度优先搜索算法思想：从起始点出发依次访问与它相连接的邻点，访问完毕后再从这些邻点出发访问邻点的邻点，但要保证先被访问的邻点的邻点要比后访问的邻点的邻点先访问，直至图中所有已被访问的结点的邻点都被访问到；如果此时图中尚有未被访问的结点，则需要选取一个尚未被访问的结点作为一个新的结点，继续搜索访问，直到图中所有的结点都被访问一遍为止；
    - 深度优先算法优先选择离目标点最近的结点，广度优先算法选择离初始点最近的点；基于深度优先算法，能以最快的速度找到一条连接起始点到目标点的路径，但不能保证路径的最优性；广度优先搜索算法则必然能找到最短的路径，但由于需要遍历所有的结点，计算复杂度更大； $A^*$ 算法基于启发函数构建代价函数，既考虑了新结点距离初始点的代价，又考虑了新结点与目标点距离的代价；
    - $A^*$ 算法中包含了开启列表(OPEN表)和关闭列表(CLOSE表)；在OPEN表中存放的是还没有访问到的结点，CLOSE表中存放的是已访问的结点；算法首先将起点放入开启列表中进行扩展，然后对开启列表中的结点进行路径评分，从而给出从小到大的排列；路径评分需要一个代价函数判断出该结点是否为OPEN表中代价最小的结点，在算法中，采用的代价函数：
      $F = G + H$
      其中： $G$ 为当前结点到起始点的距离； $H$ 为当前结点到目标点的距离， $F$ 为两者的距离和；
      OPEN表对列表中的结点根据 $F$ 值进行从小到大的排列，将 $F$ 值最小的结点从OPEN中删除，将其添加到CLOSE表中；最开始只有起点一个结点，因此起点被放入CLOSE表中，并将起点设为当前结点；通过当前结点搜索当前结点邻近的结点，如果该结点所扩展的结点不在OPEN表中，则将这些结点添加到OPEN表中，之后对添加到OPEN表中的结点进行排序，选出 $F$ 值最小的结点，选该结点作为当前结点，并扩展其邻近结点；如果所扩展的结点在OPEN表中，以当前结点为父结点，重新计算 $G$ 值，且和之前的 $G$ 值进行比较，从而找出最小 $G$ 值进行更新，并重新计算 $F$ 值，再次排序，按照上述步骤循环，直至将目标点添加到OPEN表中，此时搜索算法结束；根据父结点一直找到起点，即可得到搜索到的最佳路径。
  2. Dijkstra算法
    - Dijkstra算法是典型基于启发式算法的最短路径算法，用于计算一个结点到其他所有结点的最短路径；
    - 该算法主要特点：以起始点为中心向外层扩展，直到扩展到终点为止；Dijkstra算法能得出最短路径的最优解，但由于该算法遍历计算的结点很多，因此效率低；
    - Dijkstra算法运行过程：
      1. 创建两个表：OPEN表和CLOSE表；
      2. OPEN表保存所有已生成而未考察的结点，CLOSE表记录已访问过的结点；
      3. 访问离起始点最近且没有被检查过的点，把这个点放入OPEN表中等待检查；
      4. 从OPEN表中找出距起始点最近的点，找出这个点的所有子结点，把这个点放到CLOSE表中；
      5. 遍历考察这个点的子结点，求出这些子结点距起始点的距离值，将子结点放到OPEN表中；
      6. 重复4、5两步，直到OPEN表为空，或找到目标点；
2. 随机采样算法
  基于随机采样的运动规划算法的基本思路：通过对状态空间均匀随机采样来构建一个连通图，当初始、目标状态都在图中或都可以连接到图中时，问题得到解决；基于随机采样的算法不需要对状态空间自由区域进行显式建模，由碰撞检测来验证轨迹的可行性；
  典型的基于随机采样的算法：概率路图法(Probabilistic Road Map，PRM)和快速随机扩展树法(Rapidly Random Tree，RRT)；
  1. PRM构建
    - PRM是一种图搜索方法，将连续空间转换成离散空间，再利用 $A^*$ 等搜索算法在路线图上寻找路径，以提高搜索效率；这种方法能用相对少的随机采样点来找到一个解；
    - 将运动规划环境抽象成一个二维封闭平面空间，记为 $C$ ，可以自由移动的区域为自由空间，记为 $C_{free}$ ，障碍物区域记为 $C_{obs}$ ；基本PRM算法思想：在自由空间区域随机采样路标点，以一种规则连接各路标点，形成一个随机的网络，这一随机网络即为概率地图；
    - 概率地图是一种无向图，用 $G = (V, E)$ 表示， $V$ 表示结点集，即自由空间的随机采样点， $E$ 表示边集；然后利用搜索算法在路径图搜索一条可行的路径；算法总体上分为两个阶段：构图阶段(Construction Phase)和查询阶段(Query Phase)：
      - 构图阶段：在自由空间中随机采样，构成路标点集合 $F$ ，然后由局部规划器检索每个路标点的邻近点，并连接路标点和邻近点；局部规划器可以设定最邻近的 $K$ 个点或与路标点距离小于 $d$ 的点作为邻近点；碰撞检测要求随机采样点不能处于绝对碰撞区内，路标点和邻近点的连线不能与障碍区相交；
      - 查询阶段：将起点和终点添加到轨迹点集合 $v$ ，局部规划器为其寻找最近的邻居点并建立连接，若起点到终点能形成连通图，则轨迹规划有解；基于某种图搜索算法求出连接起点和终点的最优轨迹；
    - PRM算法的伪代码：
  2. RRT的构建
    - 快速扩展随机树算法(Rapid-exploration Random Tree，RRT)；该算法是从起始点开始向外拓展一个树状结构，树状结构的拓展方向是通过在规划空间内随机采点确定；
    - 树的初始化：初始化树的结点集和边集，结点集只包含起始状态，边集为空；
    - 树的生长：对状态空间随机采样，当采样点落在状态空间安全区域时，选择当前树中离采样点最近的结点，将其向采样点扩展(或连接)；若生成的轨迹不与障碍物发生碰撞，则将该轨迹加入树的边集，该轨迹的终点加入到树的结点集；
    - 重复上述步骤，直至扩展到目标状态集中；
    - RRT构建的是初始状态作为根节点，目标状态作为叶结点的树结构；
    - RRT算法伪代码：
3. DP算法
  DP(Dynamic Programming)算法称为动态规划算法；
  DP路径算法基本思路：基于给定的一条中心路径(Reference Line，参考线)和车道边界条件，每隔一定间隔的纵向距离(称为不同级上的 $S$ 值)对道路进行均匀采样(与中心点的横向偏移值称为 $L$ 值)，这样得到采样点( $Wa y p o in t$ ，这些采样点称为航迹点)数组；
  道路中心线用于建造 $S - L$ 坐标系， $S$ 代表沿中心线方向， $L$ 代表与中心线正交的方向；
  DP算法是一种聚合重复的计算过程，分解得到的子问题是相互重叠的，子问题依赖于子子问题，子子问题又进一步依赖下一级的子子问题，这样不断循环直至抵达不需要再分解的初始条件；
  $S - L$ 轨迹可表示为：
  $trajectory=l(s)≈(l_1,l_2,l_3,\dots,l_n)\tag{12}$
  计算复杂度为： $O(k^n)$ ；
  最优化过程为：
  $min{Cost}(l_n|l_{n-1})=min{Cost}(l_{n-1})+Cost(l_{n-1},l_n)\tag{13}$
  $min{Cost(l_n)}=min_{k_n}(minCost(l_{n-1})+Cost(l_{n-1},l_n))\tag{14}$
  计算复杂度为： $O(k^{2n})$ ；
  基于DP算法，通过将汽车行驶过程中前方一定长度区域分解为可采样的诸多分段路径，对其进行撒点采样并逐级优化目标函数，即可得到一条可通行的规划路径；
  路径规划过程的优化目标函数可以考虑以下因素进行构建：与道路中心线的偏差、路径曲率与曲率连续性、与障碍物应保持的合理距离、路径曲率符合汽车运动特性等；
局部路径平滑
- 通过路径搜索算法产生的路径能避开障碍，但因为结点连接起来是折线，不利于汽车跟踪，需要对路径进行平滑处理；拟合是路径平滑中常用的方法，常见的拟合方法：B样条曲线法、QP算法等；
- 拟合曲线根据曲线与给定点的关系分为："点点通过"式、"平均通过"式；
- “点点通过”：当给定的离散点位置比较精确的时候，拟合曲线通过所有给定的点，根据通过对给定的离散点拟合得到的曲线方程以足够小的步长获取相邻离散点间若干个数据点的坐标，并用直线连接；求解相邻离散点间若干数据点的过程称为插值；
- “平均通过”：当给定的数据点存在误差的时候，拟合曲线不通过所有的数据点，曲线的走势反映了这些数据点的变化趋势；曲线拟合的目标是使得给定的数据点与曲线的"距离"总和最小，得到的曲线方程要尽量"逼近"给定的数据点；
- "点点通过"式的曲线称为插值曲线，"平均通过"式的曲线称为逼近曲线；
- 当给定的数据点较多时，对所有的点拟合曲线方程有所困难，即使得到曲线方程，方程亦会十分复杂；可以通过分段进行拟合，这种方式称为样条拟合；分段拟合需要考虑段与段之间连接点的光滑度问题；
样条曲线和QP算法在路径平滑中的应用：
1. 三阶B样条曲线的研究
  汽车侧向运动的动力学模型传递函数近似表示为：
  $G_m(s)=\frac{b_1s^2+b_2s+b_3}{s^3(s+a_1)(s+a_2)}\tag{15}$
  进一步将动力学模型约束系统简化为： $1/s^3$ ，依据此简化结果进行轨迹规划， $G_m(s)$ 中的剩余部分通过自主汽车底层侧向跟踪控制模块进行补偿；关于B样条性质的一个结论： $k + 1$ 阶B样条曲线是形如下式的线性系统的一条轨迹：
  $\frac{d^k}{dt^k}y(t)=u(t)\tag{16}$
2. 三阶B样条曲线在路径平滑中的应用
  利用B样条曲线平滑路径需要样条曲线通过给定的路径点，即路径点不能作为B样条曲线的控制点，要作为型值点被样条曲线穿过，但B样条曲线是通过控制点坐标来构建参数方程的，这需要通过型值点坐标来求解控制点坐标；通过型值点求解控制点的方式称为反算；
  在B样条曲线中反算的目的在于利用B样条曲线对已知点进行插值，步骤依次为：利用型值点求解控制点、利用控制点构造B样条曲线方程、利用B样条曲线方程计算插值点；
  假设当前得到的路径点为 $Q_i(i=1,2,3,\dots,n)$ ，求三阶B样条曲线的控制点 $P_i(i=0,1,2,\dots,n+1)$ ；因为曲线段与段间首尾连接，即第 $i$ 段曲线的终点同时是第 $i + 1$ 段曲线的起点，且三阶B样条曲线二阶导数连续，光滑连接，因此每个型值点可看作每段三阶B样条曲线的起点；
  三阶B样条曲线的型值点与控制点的位置存在如下关系：
  $p_i(t=0)=\frac{P_{i-1}+4P_i+P_{i+1}}{6}=Q_i，i=1,2,\dots,n\tag{17}$
  由上式得到 $n$ 个方程，但从 $P_0$ 到 $P_{i+1}$ 有 $n + 2$ 个未知数，因此需要将起点和终点两个边界条件考虑进去解出所有的未知数；
  对于B样条曲线边界处理，此要求曲线过 $Q_1$ 和 $Q_n$ ，即 $P_1=Q_1，P_n=Q_n$ ，可得线性方程组：
  $\begin{bmatrix} 6 & 0 &&&&\\ & 1 & 4 & 1 &&\\ &&&\dots&&\\ &&1 & 4 & 1 & \\ &&&&0&6 \end{bmatrix} \begin{bmatrix} P_1\\ P_2\\ \vdots\\ P_{n-1}\\ P_n \end{bmatrix}= 6\begin{bmatrix} Q_1\\ Q_2\\ \vdots\\ Q_{n-1}\\ Q_n \end{bmatrix}\tag{18}$
  还需要增加 $P_0=2P_1-P_2$ 和 $P_{n+1}=2P_n-P_{n-1}$ 两个条件，即曲线起点和终点分别与 $P_0P_1、P_nP_{n-1}$ 相切，这样构造的三阶B样条曲线在两端曲率为零；
3. QP算法在路径平滑中的应用
  使用QP算法(二次规划算法)需要限制条件：曲率和曲率连续性、贴近中心线、避免碰撞；
  路径定义在Station-Lateral坐标系中；参数 $s$ 的取值范围为汽车的当前位置到默认规划路径的长度；将路径划分为 $n$ 段，每段路径用一个多项式来表示，每个样条段 $i$ 都有沿着参考线的累加距离 $d_i$ ；每段的路径默认用5阶多项式表示：
  $l=f_i(s)=a_{i0}+a_{i1}·s+a_{i2}·s^2+a_{i3}·s^3+a_{i4}·s^4+a_{i5}·s^5(0≤s≤d_i)\tag{19}$
  定义每个样条段优化目标函数：
  $cost=\sum_{i=1}^n\left(\omega_1·\int_0^{d_i}f'_i(s)^2ds+\omega_2·\int_0^{d_i}f''_i(s)^2ds+\omega_3·\int_{0}^{d_i}f'''_i(s)^2ds\right)\tag{20}$
  将最小化优化目标函数的过程转换为最小化QP公式的过程，OP公式参考：
  $\begin{cases} &\min\frac{1}{2}·x^T·H·x+f^T·x\\ &st{\space}LB≤x≤UB\\ &A_{eq}x≤b_{eq}\\ &Ax≥b \end{cases}\tag{21}$
  其中：
  - $LU 、 U B$ 分别是 $s$ 的上界和下界；
  - $A_{eq}x≤b_{eq}、Ax≥b$ 分别为根据实际约束条件转换得到的约束公式；
  - $H$ 为由优化目标函数形式转换成 $QP$ 公式形式得到的对应矩阵；
  将优化目标函数转换为QP公式实例：
  $f_i(s)=|1\space s\space s^2\space s^3 \space s^4\space s^5|·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}$
  $f'_i(s)=|0\space 1\space 2s\space 3s^2 \space 4s^3\space 5s^4|·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}$
  $f'_i(s)^2=|a_{i0}\space a_{i1}\space a_{i2}\space a_{i3} \space a_{i4}\space a_{i5}|·\begin{vmatrix} 0\\ 1\\ 2s\\ 3s^2\\ 4s^3\\ 5s^4\\ \end{vmatrix}·|0 \space 1\space 2s \space 3s^2\space 4s^3\space 5s^4| ·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}$
  $\int_0^{d_i}f'_i(s)^2ds=|a_{i0}\space a_{i1}\space a_{i2}\space a_{i3} \space a_{i4}\space a_{i5}|·\int_{0}^{d_i}\begin{vmatrix} 0\\ 1\\ 2s\\ 3s^2\\ 4s^3\\ 5s^4\\ \end{vmatrix}·|0 \space 1\space 2s \space 3s^2\space 4s^3\space 5s^4|ds ·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}$
  可得：
  $\int_{0}^{d_i}f'_i(s)^2ds=|a_{i0}\space a_{i1}\space a_{i2}\space a_{i3} \space a_{i4}\space a_{i5}|· \begin{vmatrix} 0 & 0 & 0 & 0 & 0 & 0\\ 0 & d_i & d_i^2 & d_i^3 & d_i^4 & d_i^5\\ 0 & d_i^2 & \frac{4}{3}d_i^3 & \frac{6}{4}d_i^4 & \frac{8}{5}d_i^5 & \frac{10}{6}d_i^6\\ 0 & d_i^3 & \frac{6}{4}d_i^4 & \frac{9}{5}d_i^5 & \frac{12}{6}d_i^6 & \frac{15}{7}d_i^7\\ 0 & d_i^4 & \frac{8}{5}d_i^5 & \frac{12}{6}d_i^6 & \frac{16}{7}d_i^7 & \frac{20}{8}d_i^8\\ 0 & d_i^5 & \frac{10}{6}d_i^6 & \frac{15}{7}d_i^7 & \frac{20}{8}d_i^8 & \frac{25}{9}d_i^9 \end{vmatrix} ·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}$
  得到一个6阶的矩阵表示5阶样条插值的衍生开销；
  平滑过程应考虑的约束条件：初始点约束、终点约束、平滑点约束、点采样边界约束；
  - 初始点约束：假设第一个点为： $s_0,l_0)$ ，其中 $l_0$ 表示横向的偏移，对应的一阶、二阶导数为： $l'_0、l''_0$ ;规划路径的起始点衍生开销可由 $f_i(s)、f'_i(s)、f''_i(s)$ 计算得到；
    将上述约束转换为QP约束等式，使用等式： $A_{eq}x=b_{eq}$ ；
    转换的具体步骤：
    $f_i(s_0)= \begin{vmatrix} 1 \space s_0 \space s_0^2 \space s_0^3 \space s_0^4 \space s_0^5 \end{vmatrix}·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}=l_0$
    $f'_i(s_0)=|0\space 1\space 2s_0\space 3s_0^2 \space 4s_0^3\space 5s_0^4|·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}=l'_0$
    $f''_i(s_0)=|0\space 0\space 2\space 3\times2s_0\space 4\times3s_0^2 \space 5\times4s_0^3|·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}=l''_0$
    其中： $i$ 是包含的样条段 $s_0$ 的索引值；
  - 终点约束：终点 $s_e,l_e)$ 应当按照起始点计算方法生成约束条件，将起始点和终点组合在一起，得出约束条件为：
    $\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4 & s_0^5\\ 0 & 1 & 2s_0 & 3s_0^2 & 4s_0^3 & 5s_0^4\\ 0 & 0 & 2 & 6s_0 & 12s_0^2 & 20s_0^3\\ 1 & s_e & s_e^2 & s_e^3 & s_e^4 & s_e^5\\ 0 & 1 & 2s_e & 3s_e^2 & 4s_e^3 & 5s_e^4\\ 0 & 0 & 2 & 6s_e & 12s_e^2 & 20s_e^3 \end{vmatrix} ·\begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5}\\ \end{vmatrix}= \begin{vmatrix} l_0\\ l_0'\\ l_0''\\ l_e\\ l_e'\\ l_e'' \end{vmatrix}$
  - 平滑结点约束：该约束的目的是使样条的结点更加平滑；假设两个段 $seg_k$ 和 $seg_{k+1}$ 互相连接，且 $seq_{k}$ 按结点的累计值 $s$ 为 $s_k$ ，计算约束的等式为： $f_k(s_k)=f_{k+1}(s_0)$ ；
    计算的具体步骤为：
    $\begin{vmatrix} 1 \space s_k \space s_k^2 \space s_k^3\space s_k^4 \space s_k^5 \end{vmatrix}· \begin{vmatrix} a_{k0}\\ a_{k1}\\ a_{k2}\\ a_{k3}\\ a_{k4}\\ a_{k5} \end{vmatrix}= \begin{vmatrix} 1 \space s_0 \space s_0^2 \space s_0^3 \space s_0^4 \space s_0^5 \end{vmatrix}· \begin{vmatrix} a_{k+1,0}\\ a_{k+1,1}\\ a_{k+1,2}\\ a_{k+1,3}\\ a_{k+1,4}\\ a_{k+1,5} \end{vmatrix}$
    同样，可以为下述等式计算约束等式：
    $f'_k(s_k)=f'_{k+1}(s_0)，f''_k(s_k)=f''_{k+1}(s_0)，f'''_k(s_k)=f'''_{k+1}(s_0)$
  - 点采样边界约束：在路径上均匀地取 $m$ 个点，检查这些点上的障碍物边界；将这些约束转换为QP约束不等式，使用不等式： $A x \geq b$ ；
    先基于道路宽度和周围的障碍物找到点 $s_j,l_j)$ 的下边界 $l_{lb,j}$ ，且 $j\in[0,m]$ ；计算约束的不等式为：
    $\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4 & s_0^5\\ 1 & s_0 & s_0^2 & s_0^3 & s_0^4 & s_0^5\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots\\ 1 & s_m & s_m^2 & s_m^3 & s_m^4 & s_m^5\\ \end{vmatrix}· \begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5} \end{vmatrix}≥ \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ \vdots\\ l_{lb,m} \end{vmatrix}$
    上边界约束：
    $\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4 & s_0^5\\ 1 & s_0 & s_0^2 & s_0^3 & s_0^4 & s_0^5\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots\\ 1 & s_m & s_m^2 & s_m^3 & s_m^4 & s_m^5\\ \end{vmatrix}· \begin{vmatrix} a_{i0}\\ a_{i1}\\ a_{i2}\\ a_{i3}\\ a_{i4}\\ a_{i5} \end{vmatrix}≤ \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ \vdots\\ l_{ub,m} \end{vmatrix}$
    确定上述约束条件后，最小化QP目标函数后得到的曲线即为平滑后的路径；
基于汽车运动学和动力学的轨迹优化

根据二自由度转向模型可得汽车稳态横摆角速度增益为：
$\frac{w_r}{\delta}=\frac{u/L}{1+Ku^2}\tag{22}$
稳态横摆角速度为：
$w_r=\frac{u\delta}{L(1+Ku^2)}\tag{23}$
其中： $K=\frac{m}{L^2}\left(\frac{a}{k_2}-\frac{b}{k_1}\right)$ ， $m$ 为目标汽车质量， $a$ 为目标汽车质心距离前轴的距离， $b$ 为目标汽车距离后轴的距离， $L$ 为目标汽车轴距，即 $a$ 和 $b$ 之和， $k_1$ 为前轮侧偏刚度， $k_2$ 为后轮侧偏刚度， $u$ 为目标汽车在车身坐标系下沿 $x$ 轴方向的速度分量， $\delta$ 为前轮转角；
稳态时汽车转弯半径 $R$ 为：
$R=\frac{u}{w_r}=\frac{L(1+Ku^2)}{\delta}\tag{24}$
假设目标汽车当前时刻所处位置为 $X_0,Y_0)$ ，航向角为 $\theta_0$ ，经过时间 $t$ 后，目标汽车所处位置为 $X_t,Y_t)$ ，航向角为 $\theta_t$ ，则目标汽车航向角的变化量 $\theta$ 为：
$\theta=w_r·t\tag{25}$

图4.20中未来 $t$ 时间后目标汽车所处位置在 $x$ 轴和 $y$ 轴上的增量为 $\Delta{x}、\Delta{y}$ ，则：
$\begin{aligned} &\Delta{x}=R\sin\theta=\frac{L(1+Ku^2)}{\delta}\sin\left(\frac{u\delta}{L(1+Ku^2)}t\right)\\ &\Delta{y}=R-R\cos\theta=\frac{L(1+Ku^2)}{\delta}\left(1-\cos\left(\frac{u\delta}{L(1+Ku^2)}t\right)\right) \end{aligned}\tag{26}$
在 $t$ 时刻后，目标汽车所处位置为：
$\begin{cases} &X_t=X_0+\Delta{x}\\ &Y_t=Y_0+\Delta{y} \end{cases}\tag{27}$
航向角为：
$\theta_t=\theta_0+\theta\tag{28}$
可积分的运动约束与几何约束在物理实质上没有区别，合称为完整约束；不可积的运动约束即不能化为几何约束的运动约束，在物理实质上不同于几何约束，称为非完整约束；
无人驾驶系统速度项无法通过积分变换转化为系统对应的空间位置，使得系统的控制变量个数少于系统位姿自由度，因此无人驾驶系统是一个典型的非完整性约束系统；由于无人驾驶系统是一个受非完整性约束的非线性系统，因此汽车进行路径规划的时候不仅仅要满足几何约束和运动学约束，还要满足汽车动力学约束；
在车速一定的情况下，汽车行驶轨迹的曲率与汽车的侧向加速度成正比，汽车在实际运动过程中产生的侧向加速度受多个因素限制，如：发动机动力水平、轮胎特性、地面附着系数等，且为了乘坐人员提供舒适的驾乘体验，侧向加速度不宜过大，且越小越好，因此侧向加速度 $a_{lat}$ 存在如下约束：
$|a_{lat}|≤a_{max\_{lat}}\tag{29}$
其中： $a_{max\_lat}$ 为最大侧向加速度；
$\begin{aligned} &a_{lat}=\frac{u^2}{R}=\frac{u^2\delta}{L(1+Ku^2)}\\ &\delta=a_{lat}\frac{L(1+Ku^2)}{u^2}\\ &-a_{max\_lat}\frac{L(1+Ku^2)}{u^2}≤\delta≤a_{max\_lat}\frac{L(1+Ku^2)}{u^2} \end{aligned}\tag{30}$
由上式可知，前轮转角与汽车侧向加速度正相关，前轮转角与方向盘转角的关系由转向系传动比决定；一定车速下方向盘转角越大，侧向加速度越大，侧向加速度的约束可以转化为一定车速下对方向盘转角范围的约束，同时由于受转向系统机械结构的限制，方向盘转角变化连续且有界；
根据上一时刻前轮的实际转角为参考，在其一定范围内均匀离散产生一组 $(n 个)$ 转角，但要确保转角在要求的范围内，由这一组转角再推断经过 $m$ 个规划周期汽车的位置；
通过下式计算指标 $w$ 来评价每条局部路径与规划路径的接近程度：
$w=\sum_{i=1}^m[(x_g^t-x_l^t)^2+(y_g^t-y_l^t)^2]\tag{31}$
然后从这一组 $w_1,w_2,w_3,\dots,w_n$ 中找出最小的 $w$ 所对应的局部路径，即为最优局部路径；