本篇博客主要是用于准备决策规划面试的知识点，具体的原理讲解请详见专栏1和专栏2。

坐标系转换

1. Frenet坐标系介绍

1.1 Cartesian坐标系

• 一般情况下，我们使用Cartesian坐标系（笛卡尔坐标系）来描述物体的坐标，但对于车辆来说，笛卡尔坐标系并不是最佳选择。因为即使知道了笛卡尔坐标系下车辆的位置信息，也难以表达车辆与道路之间的相对位置，导致二者之间的相对关系不明确。

• 因此，传统规划算法在笛卡尔坐标系下规划出的路径对于开放道路有良好的效果，但是对于公路环境，忽略车道信息导致路径的自由度太高而容易违反道路交通规则。

1.2 Frenet坐标系

Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。

Frenet坐标系使用道路的中心线作为参考线，使用参考线的切线向量和法线向量建立坐标系。相比笛卡尔坐标系，Frenet坐标系简化了路径规划问题。

下图是自动驾驶车辆在全局坐标系与Frenet坐标系中的位置示意。

假设自动驾驶车辆在全局坐标系下的坐标为$(x,y)$，从车辆的位置$(x,y)$向参考线$T$作投影，投影点为$F$，则点$F$与车辆位置$(x,y)$的距离即为横向位移$d$(方向为参考线当前的法向，称为横向，Lateral)；从参考线的起始点到投影点$F$的曲线距离即为纵向位移$s$（方向沿着参考线，称为纵向，Longitudinal）。

基于Frenet坐标系，将自动驾驶车辆每时每刻的位置状态分解在$s$和$d$两个方向来描述车辆的运动状态，从而在轨迹曲线拟合时，减少处理坐标信息的工作量。

决策控制部分

1. Pure Pursuit（纯追踪）算法

1.1. 原理

纯追踪算法的原理很简单，就是单车模型通过调整前轮转向$\delta$运动，使得车辆后轴中心刚好可以经过当前规划的路点。换句话说，此时的后轴中心为圆弧切点，车辆纵向车身为切线。通过控制前轮转角$\delta$， 使车辆可以沿着一条经过目标路点（goal point）（或者叫预瞄点）的圆弧行驶。

该算法会根据机器人的当前位置在路径上移动预瞄点，直到路径的终点。可以想象成机器人不断追逐它前面的一个点。

总结如下：

基于当前车辆后轴中心位置,在参考路径上向$l_d$ (自定义，称为前视距离)的距离匹配一个预瞄点。假设车辆后轴中心点可以按照一定的转弯半径$R$行驶抵达该预瞄点,然后根据预瞄距离$l_d$、转弯半径$R$、车辆坐标系下预瞄点的朝向角$2\alpha$之间的几何关系确定前轮转角。

1.2. 模型适配

纯跟踪算法是把车看成一个阿克曼模型进行几何解算的，本质是不适合差速车或者麦轮车的

1.3. 前视距离调整

前视距离的调整需要根据机器人运行的实际情况来进行调整。

前视距离是整个Pure Pursuit控制器的重要参数。往前看的距离是机器人从当前位置应沿着路径观察的距离，以计算转角控制命令。

在低速的情况下合理调整前视距离可以实现较好的路径跟踪效果，较小的前视距离能使机器人更加精确地追踪路径，但可能会引起机器人控制的不稳定甚至震荡；

较大的前视距离可以使机器人跟踪路径更加平滑，但不能精确地跟踪原始的路径，在大转角处会出现曲率大、转向不足的情况。

2. PID算法

任何闭环控制系统的首要任务是要稳（稳定）、准（准确）、快（快速）的响应命令。PID的主要工作就是如何实现这一任务。

PID控制器的比例单元 ( P) 、积分单元(I)和微分单元(D)分别对应目前误差、过去累计误差及未来误差。若是不知道受控系统的特性，一般认为PID控制器是最适用的控制器.

• 增大比例环节将加快系统的响应，它的作用于输出值较快，但不能很好稳定在一个理想的数值，不良的结果是虽较能有效的克服扰动的影响，但有余差出现，过大的比例系数会使系统有比较大的超调，并产生振荡，使稳定性变坏。

• 积分环节能在比例的基础上消除余差，它能对稳定后有累积误差的系统进行误差修整，减小稳态误差。

• 微分环节具有超前作用，对于具有容量滞后的控制通道，引入微分参与控制，在微分项设置得当的情况下，对于提高系统的动态性能指标，有着显著效果，它可以使系统超调量减小，减小震荡，稳定性增加，动态误差减小。

在调整的时候，我们要做的任务就是在系统结构允许的情况下，在这三个参数之间权衡调整，达到最佳控制效果，实现稳、准、快的控制特点。

在实际应用中，主要有以下不足：

1. 在实际工业生产过程往往具有非线性、时变不确定，难以建立精确的数学模型，常规的PID控制器不能达到理想的控制效果；

2. 在实际生产现场中，由于受到参数整定方法烦杂的困扰，常规PID控制器参数往往整定不良、效果欠佳，对运行工况的适应能力很差。

3. Stanly算法

前轮反馈控制(Front wheel feedback)也就是常说的Stanley方法，其核心思想是基于车辆前轴中心点的路径跟踪偏差量对方向盘转向控制量进行计算。

Stanley方法是一种基于横向跟踪误差的非线性反馈函数,并且能实现横向跟踪误差指数收敛于0。

前轮转角控制变量$\delta$由两部分构成:

• 一部分是航向误差引起的转角,即当前车身方向与参考轨迹最近点的切线方向的夹角${\theta }_e$;
• 另一部分是横向误差引起的转角,即前轮速度方向与参考轨迹最近点的切线方向所成的夹角${\delta }_e$。

小结：Pure pursuit与Stanley 算法简单对比

Pure pursuit 与 Stanley 两个方法都是基于对前轮转角进行控制来消除横向误差。

Pure pursuit算法的关键在于预瞄点的选取：其距离越短，控制精度越高，但可能会产生震荡；预瞄距离越长，控制效果趋于平滑，震荡减弱。实际调试只需根据上述规则以及应用需求调整预瞄系数即可。

Stanley 算法的控制效果取决于控制增益，它缺少Pure pursuit算法的规律性，调试时需要花一定精力去寻找合适的控制增益值。

4. 后轮位置反馈

后轮反馈控制（Rear wheel feedback）是利用后轮中心的跟踪偏差来进行转向控制量计算的方法。它属于Frenet坐标系的一个应用。通过选择合适的李雅普诺夫函数设计控制率，只要设计的李雅普诺夫函数能够满足稳定性判据即可。

选择不同的系统方程，和最终不同的 李雅普诺夫形式，可以得到不同的控制率，这些不同的形式都可以使系统李雅普诺夫稳定。得到控制率​后，再考虑如何通过车辆方向盘转角进行控制，使得车辆实际的控制量趋近于期望控制量，可以选择前馈加反馈的形式，前馈就是那个artctan（wL/R）之类的式子，反馈就是pid就行。

5. LQR控制算法

最优控制理论主要探讨的是让动力系统以在最小成本来运作，若系统动态可以用一组线性微分方程表示，而其成本为二次泛函，这类的问题称为线性二次（LQ）问题。此类问题的解即为线性二次调节器，简称LQR。

LQR（Linear quadratic regulator，线性二次型调节器），它假设模型是locally linear 和 time-varied的。

LQR的目标就是找到一组控制量$u_0,u_1,...$，使得同时有系统状态$x_0,x_1,...$能够快速、稳定地趋近于零，并保持平衡(系统达到稳定状态)，$u_0,u_1,...$尽可能小(控制量尽量小的变化)。由此可以设计出代价函数。

这是一个典型的多目标优化最优控制问题，选取代价函数（目标函数）为
$$J=\frac{1}{2}{\int }_0^{\infty }{x}^{T}Qx+u^{T}Rudt$$
其中，Q、R分别是需要设计的半正定矩阵和正定矩阵。

代价函数 $J$需要达到最小值，那么在 $t$趋近于无穷时，状态向量 $x(t)$肯定趋近于0，即是达到了系统稳态；同理， $t$趋近于无穷时，控制向量 $u(t)$也会趋近于0，意味着，随着时间的推移，需要对系统施加的控制量会越来越小，意味着使用最小的控制量使得系统达到了最终控制目标，反映的是控制能量的损耗优化。

$Q$为半正定的状态加权矩阵, $R$为正定的控制加权矩阵，两者通常取为对角阵。$Q$矩阵元素变大意味着希望状态量能够快速趋近于零； $R$ 矩阵元素变大意味着希望控制输入能够尽可能小，它意味着系统的状态衰减将变慢。比如， $Q_{11}$​选取较大的值，会让 $x_1$​很快的衰减到0；所以，$Q、R$的选取，要综合看具体的实际应用场景来调节。

在轨迹跟踪中，前一项优化目标表示跟踪过程路径偏差的累积大小，第二项优化目标表示跟踪过程控制能量的损耗，这样就将轨迹跟踪控制问题转化为一个最优控制问题。

连续LQR的算法步骤如下：

• 选择参数矩阵Q,R（分别满足半正定和正定）
• 求解Riccati方程得到矩阵P
• 根据P计算增益$K=R^{-1}{B}^{T}P$
• 计算控制量 $u^{\ast }=-Kx$

离散情况下的LQR推导有最小二乘法和动态规划算法。

离散时间下的LQR算法步骤

采用LQR算法进行控制率求解的步骤概括为：

1. 确定迭代范围 $N$
2. 设置迭代初始值 $P_N=Q_f$，其中$Q_f=Q$
3. 循环迭代， 从后往前$t=N,\dots ,1$
   $$P_{t-1}=Q+A^T{P}_{t}A-A^T{P}_{t}B{\left(R+B^T{P}_{t}B\right)}^{-1}{B}^T{P}_{t}A$$
4. 从 $t=0,\dots ,N-1$，循环计算反馈系数 $K_t={\left(R+B^T{P}_{t+1}B\right)}^{-1}{B}^T{P}_{t+1}A$
5. 最终得优化的控制量 $u_t^{\ast }=-K_t{X}_t$

6. MPC算法

• 模型预测控制（MPC）的核心思想就是以优化方法求解最优控制器，其中优化方法大多时候采用二次规划（Quadratic Programming）。

• MPC控制器优化得到的控制输出也是系统在未来有限时间步的控制序列。 当然，由于理论构建的模型与系统真实模型都有误差，所以，实际上更远未来的控制输出对系统控制的价值很低，故MPC仅执行输出序列中的第一个控制输出。

MPC利用一个已有的模型、系统当前的状态和未来的控制量，来预测系统未来的输出，然后与我们期望的系统输出做比较，得到一个损失函数（代价函数），即：

$$损失函数=(未来输出(模型，当前状态，未来控制量)-期望输出{)}^2$$

由于上式中模型、当前状态、期望输出都是已知的，因此只有未来控制量一个自变量。采用二次规划的方法求解出某个未来控制量，使得损失函数最小，前面提到，这个未来控制量的第一个元素就是当前控制周期的控制量。

6.1 MPC vs optimal control

最优控制（optimal control）指的是在一定的约束情况下达到最优状态的系统表现，其中约束情况通常是实际环境所带来的限制，例如汽车中的油门不能无限大等。

最优控制强调的是“最优”，一般最优控制需要在整个时间域上进行求优化(从0时刻到正无穷时刻的积分)，这样才能保证最优性，这是一种很贪婪的行为，需要消耗大量算力。

最优控制常用解法有： 变分法，极大值原理，动态规划，LQR（LQR可以参考博客）。

MPC仅考虑未来几个时间步，一定程度上牺牲了最优性。

6.2 MPC优点

• MPC善于处理多输入多输出系统（MIMO）；

• MPC可以处理约束，如安全性约束，上下阈值；

• MPC是一种向前考虑未来时间步的有限时域优化方法（一定的预测能力）

  最优控制要求在整个时间优化

  实际上MPC采用了一个折中的策略，既不是像最优控制那样考虑整个时域，也不是仅仅考虑当前，而是考虑未来的有限时间域。

6.3 整体流程

将预测状态估计的部分称为预测区间（Predictive Horizon）,指的是一次优化后预测未来输出的时间步的个数。

将控制估计的部分称为控制区间（Control Horizon），在得到最优输入之后，我们只施加当前时刻的输入u(k)，即控制区间的第一位控制输入。

过小的控制区间，可能无法做到较好的控制，而较大的控制区间，比如与预测区间相等，则会导致只有前一部分的控制范围才会有较好的效果，而后一部分的控制范围则收效甚微，而且将带来大量的计算开销。

模型预测控制在k时刻共需三步；

• 第一步：获取系统的当前状态；

• 第二步：基于$u(k),u(k+1),u(k+2)...u(k+m)$进行最优化处理；

  离散系统的代价函数可以参考

  $$J=\sum _k^{m-1}{E}_k^{T}Q{E}_k+u_k^{T}R{u}_k+E_N^{T}F{E}_N$$

  其中$E_N$表示误差的终值，也是衡量优劣的一种标准。

• 第三步：只取$u(k)$作为控制输入施加在系统上。

在下一时刻重复以上三步，在下一步进行预测时使用的就是下一步的状态值，我们将这样的方案称为滚动优化控制（Receding Horizon Control）。

---

---

路径规划部分

7. Dijkstra算法

迪杰斯特拉算法是从一个节点遍历其余各节点的最短路径算法，解决的是有权图中最短路径问题。它的主要特点是以起始点为中心向外层层扩展(广度优先遍历思想)，直到扩展到终点为止。是图搜索算法的一种。

1. 引进两个点集S和U。初始时S中只有一个起点，S的作用是记录已求出最短路径的节点(以及相应的最短路径长度)；而U则是记录还未确定最短路径的节点(以及该节点到起点D的距离)。
2. 初始时，数组S中只有起点D，而数组U中是除起点D之外的节点集合，并且数组U中记录各节点到起点D的距离。如果节点与起点D不相邻，距离设为无穷大。
3. 然后，从数组U中找出路径最短的节点K，并将其加入到数组S中；同时，从数组U中移除节点K。接着，更新数组U中的各节点到起点D的距离。
4. 不断重复这个过程，直到遍历完所有节点。

7.1 算法说明

Dijkstra算法过程包括了三个循环，第一个循环的时间复杂度为$O(n)$，第二、三个循环为循环嵌套，时间复杂度为$O(n^2)$。

可以看出，Dijkstra最短路径算法的执行时间和占用空间与图(或网)中结点数目有关，当结点数目较大时，Dijkstra算法的时间复杂度急剧增加。当图规模较大时，直接应用该算法就会存在速度慢或空间不够的问题。所以在大的城市交通网络图中直接应用Dijkstra最短路径算法是很困难的。路径规划作为无人驾驶汽车导航系统的重要功能模块，其算法的优劣是非常重要的，评价该算法的主要性能指标是它的实时性和准确性。Dijkstra算法作为经典的路径规划算法，在实验地图数据量较小情况下会得到很好的规划结果，但在实验地图数据量较大情况下很难满足路径规划的实时性要求。

7.2 最短路径的最优子结构性质

如果 P ( i , j ) = { V i … V k … V m … V j } P(i,j)=\{V_i…V_k…V_m…V_j\} P(i,j)={Vi​…Vk​…Vm​…Vj​}是从顶点$i$到$j$的最短路径，$k$和$m$是这条路径上的一个中间顶点，那么$P(k,m)$必定是从$k$到$m$的最短路径。

证明：反证法

假设 P ( i , j ) = { V i … V k … V m … V j } P(i,j)=\{V_i…V_k…V_m…V_j\} P(i,j)={Vi​…Vk​…Vm​…Vj​}是从顶点$i$到$j$的最短路径，则有$P(i,j)=P(i,k)+P(k,m)+P(m,j)$。而$P(k,m)$不是从$k$到$m$的最短距离，那么必定存在另一条从$k$到$m$的最短路径$P^{\prime }(k,m)$，那么$P(i,j)=P(i,k)+P^{\prime }(k,m)+P(m,j)<P(i,j)$。则与$P(i,j)$是从$i$到$j$的最短路径相矛盾。因此该性质得证。

8. A星算法

8.1 算法原理

A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法，广泛应用于室内机器人路径搜索、游戏动画路径搜索等。它是图搜索算法的一种。

A*算法是一种启发式的搜索算法，它是基于深度优先算法(Depth First Search, DFS)和广度优先算法(Breadth First Search, BFS)的一种融合算法，按照一定原则确定如何选取下一个结点。

启发式搜索算法指的是，从起点出发，先寻找起点相邻的栅格，判断它是否是最好的位置，基于这个最好的栅格再往外向其相邻的栅格扩展，找到一个此时最好的位置，通过这样一步一步逼近目标点，减少盲目的搜索，提高了可行性和搜索效率。

深度优先搜索算法的思想是，搜索算法从起点开始进行搜索(初始状态下待搜索区域内所有结点均未被访问)，与周围所有邻点进行比较，选择其中距离终点最近的点进行存储，然后再以该邻点为基础对比其周围未被访问的所有邻点，仍然选择距离终点最近的邻点存储。若访问结点到达终点或访问完所有结点仍未到达终点，则视为搜索失败。成功搜索所存储的结点连接而成的路径即为起点到终点的路径。

广度优先搜索的原理是，从初始点出发依次访问与它相连接的邻点，访问完毕后再从这些邻点出发访问邻点的邻点,但是要保证先被访问的邻点的邻点要比后被访问的邻点的邻点先访问，直至图中所有已被访问的结点的邻点都被访问到。如果此时图中尚有未被访问的结点，则需要选取一个尚未被访问的结点作为个新的初始点,继续搜索访问，直到图中所有的结点都被访问一遍为止。

因此深度优先算法与广度优先搜索算法从过程上存在较大差异。深度优先算法优先选择离目标点最近的结点，而广度优先搜索算法优先选择离初始点最近的点。基于深度优先算法,能以最快的速度找到一条连接初始点到目标点的路径，但不能保证路径的最优性(例如以路径最短为标准)；广度优先搜索算法则必然能找到最短的路径，但由于需要遍历所有的结点，其计算复杂程度更大。基于这两种算法的优缺点，A*算法基于启发函数构建了代价函数，既考虑了新结点距离初始点的代价，又考虑了新结点与目标点距离的代价。

• A*算法使用一个路径优劣评价公式为：
  $$f(n)=g(n)+h(n)$$

  • f(n) 是从初始状态经由状态n到目标状态的代价估计，
  • g(n) 是从初始状态到状态n的实际代价，
  • h(n) 是从状态n到目标状态的最佳路径的估计代价。

• A*算法需要维护两个状态表，分别称为openList表和closeList表。openList表由待考察的节点组成， closeList表由已经考察过的节点组成。

8.2 算法步骤

1. 首先把起点加入 openList 。

2. 重复如下过程：

   • 遍历 openList ，查找 F 值最小的节点，把它作为当前要处理的节点。

   • 把这个节点移到 closeList 。

   • 对当前方格的 8 个相邻方格：

     ◆ 如果它是不可抵达的或者它在 closeList 中，忽略它；

     ◆ 如果它不在 openList 中，则把它加入 openList ，并且把当前方格设置为它的父节点，记录该方格的$f,g,h$值。

     ◆ 如果它已经在 openList 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 $g$ 值作参考，更小的 $g$ 值表示这是更好的路径。如果$g$值更小，把该节点的父节点设置为当前方格，并重新计算它的 $g$ 和 $h$ 值。

3. 停止搜索的情况有两种：

   • 把终点加入到了openList 中，此时路径已经找到了

   • 查找终点失败，并且 openList 是空的，此时没有路径。

4. 保存路径。使用回溯的方法，从终点开始，每个方格沿着父节点移动直至起点，这就是最终搜索到的路径。

9. RRT算法

9.1 基于随机采样的路径规划算法

• 基于随机采样的路径规划算法适用于高维度空间，它们以概率完备性(当时间接近无限时一定有解)来代替完备性，从而提高搜索效率。

• • 基于随机采样的运动规划算法的基本思路是：通过对状态空间均匀随机采样来构建一个连通图，当初始、目标状态都在图中或者都可以连接到图中时，则问题得以解决。基于随机采样的算法不需要对状态空间自由区域进行显式建模，由碰撞检测来验证轨迹的可行性即可。

• 基于随机采样的路径规划算法又分为单查询算法(single-query path planning)以及渐近最优算法(asymptotically optimal path planning)，前者只要找到可行路径即可，侧重快速性，后者还会对找到的路径进行逐步优化，慢慢达到最优，侧重最优性。单查询方法包括概率路图算法(Probabilistic Road Map, PRM)、快速随机扩展树算法(Rapidly-exploring Random Tree, RRT)、RRT-Connect算法等，渐近最优算法有RRT*算法等。

9.2 算法原理

• RRT算法是一种单查询算法，目标是尽可能快的找到一条从起点到终点的可行路径。

• RRT算法模拟树木生长时树根不断向四周扩散的过程。

• 如下图，

  • 算法通常将起点作为根节点$x_{init}$，加入到随机树的节点集合中。
  • 从可行区域内随机选取一个节点$x_{rand}$，并在已生成的树中利用欧氏距离判断距离$x_{rand}$最近的点$x_{near}$。
  • 从$x_{near}$与$x_{rand}$的连线方向上扩展固定步长$u$，得到新节点 $x_{new}$（如果$x_{near}$与$x_{rand}$间的距离小于步长，则直接将$x_{rand}$作为新节点$x_{new}$）。
  • 若$x_{new}$和$x_{near}$之间无障碍物，将$x_{new}$加入到随机树的节点集合中，同时将$x_{near}$作为$x_{new}$的父节点，将边$(x_{near},x_{new})$加入到随机树的边集中
  • 若这两个节点间有障碍物，则重新选择$x_{near}$并进行扩展。
  • 循环执行以上步骤，直到随机树的叶节点包含了目标点，并从中找出一条各节点连接成的从起点至终点的无碰撞路径。

上述是基础的RRT算法流程，它的采样过程是完全随机的，但是我们可以在采样时以一定的概率直接采样终点作为 $x_{rand}$ ，加快搜索速度。

9.3 RRT算法的优缺点

优点

RRT算法适用范围广、参数简单、高维空间规划性能优秀。既能够用于机械臂的运动力学规划，也可用于机器人或无人机等进行路径规划。

在使用 RRT算法进行路径规划时，若能够获得全局环境并进行建模，可进行全局路径规划。若无法获得全局环境，如自动驾驶汽车路径规划问题，能够在动态规划中对局部地图进行规划以生成局部路径，也为无人机等高维空间的路径规划提供了可行方案。

缺点

在扩展节点时从无障碍区域内随机选择节点，会导致产生部分无用节点，节点利用率低，增加算法随机性的同时也降低了算法的收敛速度。

由于随机树扩展时会判断 $x_{near}$和 $x_{rand}$连线方向上有无障碍物，若有障碍物则会放弃在该方向上扩展节点。 因此当路径中包含障碍物之间形成的狭窄通道时，使用RRT算法规划路径有一定几率无法规划出最优路径。

10. RRT-Connect算法

• RRT-Connect算法在RRT的基础上引入了双树扩展环节，分别以起点和目标点为根节点同时扩展随机树从而实现对状态空间的快速搜索。
• 当两棵树建立连接时可认为路径规划成功。
• 通过一次采样得到一个采样点 ，然后两棵搜索树同时向采样点​方向进行扩展，加快两棵树建立连接的速度。相较于单树扩展的RRT算法，RRT-Connect加入了启发式步骤，加快了搜索速度，对于狭窄通道也具有较好的效果。

10.1 算法步骤

• 每一次迭代中，开始步骤与原始的RRT算法一样，都是采样随机点然后进行扩展。
• 然后扩展完第一棵树的新节点$q_{new}$后，以这个新的目标点$q_{new}$作为第二棵树扩展的方向。
• 同时第二棵树扩展的方式略有不同，首先它会扩展固定步长得到$q{\prime }_{new}$，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者$q{\prime }_{new}=q_{new}$表示与第一棵树相连了，即connect了，整个算法结束。
• 每次迭代中必须考虑两棵树的平衡性，即两棵树的节点数的多少（也可以考虑两棵树总共花费的路径长度），交换次序选择“小”的那棵树进行扩展。

10.2 RRT-Connect的特点

这种双向的RRT技术具有良好的搜索特性，比原始RRT算法的搜索速度、搜索效率有了显著提高。

• 首先，Connect算法较之前的算法在扩展的步长上更长，使得树的生长更快；
• 其次，两棵树不断朝向对方交替扩展，而不是采用随机扩展的方式，特别当起始位姿和目标位姿处于约束区域时，两棵树可以通过朝向对方快速扩展而逃离各自的约束区域。
• 这种带有启发性的扩展使得树的扩展更加贪婪和明确，使得双树RRT算法较之单树RRT算法更加有效。

RRT-Connect和RRT一样，都是单查询算法，最终路径并不是最优的。

11. RRT*算法

RRT*算法是一种渐近最优算法，属于RRT算法的优化。

渐近最优的意思是随着迭代次数的增加，得出的路径是越来越优化的，因此要想得出相对满意的优化路径，需要一定的运算时间。

算法流程与RRT算法流程基本相同，不同之处主要在于两个地方：

• 首先，重新为$x_{new}$选择父节点。

  • 不同于RRT中直接选择$x_{nearest}$作为$x_{new}$的父节点，我们需要重新为$x_{new}$选择父节点，使得$x_{new}$到起点的cost能够最小。至于cost的定义，可以是路径的长度。
  • 父节点的选择可以是该节点附近相连的所有点，一般是在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$，作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选
  • 我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价 加上 近邻节点 $X_{near}$ 到 $x_{new}$ 的路径代价，取路径代价最小的近邻节点$x_{min}$作为 $x_{new}$ 新的父节点

• 其次，就是在重新选完父节点后，为该节点的所有近邻节点重新布线，即rewire，布线的原则是使所有节点到起点的cost最小

示意过程如下：

12. PRM算法

12.1 算法原理

概率路图算法是一种典型的基于采样的路径规划方法。它主要分为两个阶段：学习阶段， 查询阶段。

学习阶段

应用PRM算法进行路径规划时，首先在将连续空间转换成离散空间后，在离散空间中采样一个无碰撞的点（随机撒点，剔除落在障碍物上的点），以该点为中心，在一定的半径范围内搜索其邻域点，并将其连接形成路径，随后进行碰撞检测，若无碰撞，则保留该路径。

查询阶段

当空间中所有采样点均完成上述步骤后，再应用图搜索算法搜索出可行路径。

12.2 PRM算法的优缺点

优点

该算法原理简单，容易实现，只需要调整参数即可实现不同场景的路径规划，且不需要对环境中的障碍物进行精确建模，在高维空间和动态环境中的路径规划有很大优势。

缺点

但该算法存在狭窄通路问题： 空白区域采样点密集，障碍物密集处采样点又相对较少，可能无法得到最短路径。而且参数的设置对路径规划的结果影响较大，采样点的数量、邻域的大小设置不合理均可能导致路径规划失败（如采样点设置过少导致生成的路径过少未覆盖起终点、邻域设置过大导致过多的路径无法通过碰撞检测）。

所以PRM是概率完备且不最优的算法。

13. 人工势场法

13.1 基本思想

• 人工势场法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场

• 被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。

• 更直观而言， 势场法是将障碍物比作是平原上具有高势能值的山峰， 而目标点则是具有低势能值的低谷。

• 引力势场主要与汽车和目标点间的距离有关, 距离越大, 汽车所受的势能值就越大; 距离越小, 汽车所受的势能值则越小

• 决定障碍物斥力势场的因素是汽车与障碍物间的距离, 当汽车未进入障碍物的影响范围时, 其受到的势能值为零; 在汽车进入障碍物的影响范围后, 两者之间的距离越大, 汽车受到的势能值就越小, 距离越小, 汽车受到的势能值就越大。

13.2 缺点与改进

1. 目标不可达的问题

由于障碍物与目标点距离太近，当汽车到达目标点时，根据势场函数可知，目标点的引力降为零，而障碍物的斥力不为零，此时汽车虽到达目标点， 但在斥力场的作用下不能停下来，从而导致目标不可达的问题。

2. 陷入局部最优的问题

车辆在某个位置时，无法向前搜索避障路径。

出现局部最优主要有两种情况：

• 汽车受到的障碍物的斥力和目标点的引力之间的夹角近似为180°，几乎在同一条直线上,就会出现汽车在障碍物前陷入局部最优的问题。

• 如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，智能汽车自身判断到达势能极小值的位置，但没有到达期望的目标点位置。由于合力为零，汽车就会陷在势能极小的位置，无法继续前进和转向，以致无法到达期望的目标点。

3. 改进障碍物斥力势场函数

通过改进障碍物斥力势场函数来解决局部最优和目标不可达的问题；在传统人工势场法的障碍物斥力场模型中加入调节因子，使斥力势场函数不仅跟汽车到障碍物的距离有关，还跟汽车与目标点的距离有关。 汽车只有到达目标点时, 斥力和引力才同时减小到零, 从而使局部最优和目标不可达的问题得到解决。

14. 样条曲线法

样条曲线法主要用于路径的平滑。

14.1 贝塞尔曲线法

贝塞尔曲线是应用于二维图形应用程序的数学曲线，由一组称为控制点的向量来确定，给定的控制点按顺序连接构成控制多边形，贝塞尔曲线逼近这个多边形，进而通过调整控制点坐标改变曲线的形状。控制点的作用是控制曲线的弯曲程度

贝塞尔曲线只需要很少的控制点就能够生成较复杂的平滑曲线。该方法能够保证输入的控制点与生成的曲线之间的关系非常简洁、明确。

对于车辆系统，规划的轨迹应满足以下准则： 轨迹连续；轨迹曲率连续； 轨迹容易被车辆跟随，且容易生成。

贝塞尔曲线是参数化曲线，n次贝塞尔曲线由n+1个控制点决定。

14.2 贝塞尔曲线性质

贝塞尔曲线具有许多性质，这里就列举几个在自动驾驶运动规划中的常见性质。

• 性质1： $P_0$和$P_n$分别位于贝塞尔曲线的起点和终点；

• 性质2：几何特性不随坐标系的变换而变化；

• 性质3：起点和终点处的切线方向与和特征多边形的第一条边及最后一条边分别相切，换句话说，可根据曲线的起始点和终止点的切线方向确定车辆起始点姿态和目标点姿态；

• 性质4：若要求两端弧线拼接在一起依然是曲率连续的，必须要求两段弧线在连接处的曲率是相等的；

• 性质5： 至少需要三阶贝塞尔曲线（四个控制点）才能生成曲率连续的路径。

1. 城市环境下局部路径规划，如贝塞尔曲线能够拟合直道和弯道，在曲率变化较大的地方可以选用两个贝塞尔曲线来拟合。
2. 无人驾驶车辆的运动规划，目标轨迹曲率是连续的且轨迹的曲率不超过车辆可行驶轨迹曲率的限制。

15. B样条曲线法

贝塞尔曲线有以下缺陷：

1. 确定了多边形的顶点数（n+1个），也就决定了所定义的Bezier曲线的阶次（n次），这样很不灵活。

2. 当顶点数（ n+1 ） 较大时， 曲线的次数较高，曲线的导数次数也会较高，因此曲线会出现较多的峰谷值。

3. 贝塞尔曲线无法进行局部修改。

B样条曲线除了保持Bezier曲线所具有的优点外，还弥补了上述所有的缺陷。即： 可以指定阶次； 移动控制点仅仅改变曲线的部分形状，而不是整体。

16. 曲线插值法

• 曲线插值的方法是按照车辆在某些特定条件（安全、快速、高效）下， 进行路径的曲线拟合，常见的有多项式曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、 B样条曲线等。

• 曲线插值法的核心思想就是基于预先构造的曲线类型，根据车辆期望达到的状态（比如要求车辆到达某点的速度和加速度为期望值），将此期望值作为边界条件代入曲线类型进行方程求解，获得曲线的相关系数（简单地说就是待定系数法！）。

• 曲线所有的相关系数一旦确定，轨迹规划随之完成。

多项式曲线分为三次多项式曲线、 五次多项式曲线、 七次多项式曲线

• 针对三次多项式曲线，最多能确定每一个期望点的两个维度的期望状态，一般来说就是位置和速度。

{ x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 y ( t ) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 (1) \tag{1} \left\{

\right. {x(t)=a0​+a1​t+a2​t2+a3​t3y(t)=b0​+b1​t+b2​t2+b3​t3​(1)

• 针对五次多项式曲线，最多能确定每一个期望点的三个维度的期望状态，一般来说就是位置、速度、加速度。

{ x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 y ( t ) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 + b 4 t 4 + b 5 t 5 (2) \tag{2} \left\{

\right. {x(t)=a0​+a1​t+a2​t2+a3​t3+a4​t4+a5​t5y(t)=b0​+b1​t+b2​t2+b3​t3+b4​t4+b5​t5​(2)

• 针对七次多项式曲线，最多能确定每一个期望点的四个维度的期望状态，一般来说就是位置、速度、 加速度、加加速度（加加速度称为jerk，加加加速度称为snap，无人机轨迹规划中有用到snap）

{ x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 + a 6 t 6 + a 7 t 7 y ( t ) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 + b 4 t 4 + b 5 t 5 + b 6 t 6 + b 7 t 7 (3) \tag{3} \left\{

\right. {x(t)=a0​+a1​t+a2​t2+a3​t3+a4​t4+a5​t5+a6​t6+a7​t7y(t)=b0​+b1​t+b2​t2+b3​t3+b4​t4+b5​t5+b6​t6+b7​t7​(3)