Skip to content

规划的一些感想

本人在研究时对于规划的一些感悟

1.规划的配置空间：

对于无人驾驶车辆来讲，最简单的位形描述需要使用3个变量：车辆某个参照点的坐标(x, y)，以及车辆的朝向θ来表达车辆的构型，这也是很多参考文献中，对于非和谐车辆系统的位形表达方式。对于我们专门为有人乘用的无人驾驶车辆作轨迹规划的问题来讲，更好的位形组成在上面的3个变量基础上加入车辆的即时转向曲率κ：如果车辆的导向轮保持一定的角度，车辆会做圆周运动。这个圆周运动的半径就是即时转向曲率 κ。加入κ，有助于控制模块获得更准确的控制反馈，设计更为精细平稳的控制器

2.轨迹规划问题的难点

  • 预测的不确定性
  • 求解空间的非凸性、高维、非线性约束。
  • 高频

轨迹规划是一个复杂的问题，首先，规划上加入了速度与时间的信息，增加了规划的维度。其次，由于车辆是非和谐系统，具有特殊的运动学约束条件。举个例子，车辆不能独立的横向移动，需要纵向移动的同时才能获得横向偏移。躲避障碍物，特别是高速动态障碍物，也是一个比较困难的问题。对于搭载乘客的无人驾驶车辆来说，非常重要的一个要求（在本人看来可能是最重要的）是舒适性，规划出的轨迹必须做到平滑，将影响乘客舒适度的因素，比如加速度，向心加速度等等，保持在能够容忍的范围。

另外，对于无人驾 驶车辆来讲，为了处理多变的行车环境，轨迹规划算法需要以短周期高频率运行，对算法的计算效率也提出了要求。

3.轨迹规划策略

在 Apollo 平台中，为了解决规划问题，尤其是从降低计算的复杂性上，我们采取了对轨迹的路径与速度规划分离的方式。即先确定路径，也就是轨迹的几何形状，然后在已知路径的基础上，计算速度分配。使用这种策略，我们将一个高维度的轨迹规划问题，转换成了两个顺序计算的低维度规划问题：路径规划，借助中间变量路径的累计长度 s，先求解 s 映射到几何形状(x, y, θ, κ)的路径函数；速度规划，再求解时间 t，映射到中间变量 s,与 v，a 的速度函数。

4.路径规划的目标

前面提到 Apollo 轨迹规划的策略，我们先看路径规划在这种策略下需要解决的问题：

路径需要有足够的灵活度，用来对复杂城市环境中的障碍物进行避让。 路径的几何特性需要平顺变化，这样车辆在沿路径行驶时才能够保证舒适性。 路径的规划需要遵守车辆的运动学限制条件，比如，路径的曲率、曲率变化率需要限制在符合目标车辆属性的范围之内。

由于我们在 Apollo 平台中采用了路径/速度分离的策略，在路径规划时也需要考虑到当前车辆的运动状态而适当的调整车辆的几何形状。比如在做较大的横向运动的时候，低速和高速下，我们需要不同的几何形状来保证横向运动的舒适性。

5.路径规划方法

在路径规划中，我们借助于弗莱纳（frenet）坐标系，将当前车辆的运动状态(x, y, θ, κ, v, a)做分解。使用弗莱纳坐标系的前提是具有一条光滑的指引线。一般情况下，我们可以将指引线理解为道路中心线，车辆在没有障碍物情况下的理想运动路径。我们这里的光滑的指引线是指指引线的几何属性可以到曲率级别的光滑（曲率可导）。指引线的光滑性至关重要，因为其直接决定了在弗莱纳坐标系下求得轨迹的平顺性。在 Apollo 平台上，我们实现了多种平滑指引线的计算方法，在今天的分享中就不展开介绍了，有兴趣的同学请查阅相关代码（或者我们再进行一次以指引线为主题的分享。

在给定一条光滑指引线上，我们按照车辆位置，将其投影到指引线上，以投射点为基础，将地图坐标系下当前车辆的运动状态(x, y, θ, κ, v, a)进行分解，获得沿着指引线方向的位置，速度，加速度，以及相对于指引线运动的位置， “速度”， “加速度”。这里打引号的原因是横向的“速度”、“加速度”并非通常意义上位移对时间的一阶/二阶导数，而是横向位移对纵向位移的一阶/二阶导数，它们描述了几何形状的变化趋势。

6 frenet坐标系的优缺点

优点： - 能够把一些非凸的道路解空间转成凸的（比如十字路口，T字路口就是非凸的），也就是说一些非线性的约束可以转成线性约束。从而能够很好的把问题formulation成凸优化问题甚至是QP问题，有全局最优解，求解更稳定。采用一些比较成熟的求解器求解，效率更高。 - 更重要的是，可以把一个三维的（x,y,t）问题转化为两个二维的(s,l),(s,t)问题进行求解，能够简化问题，使轨迹规划问题的实时性更高。

缺点： - 大曲率下可能投影不唯一 - 很难建模车辆的动力学（自行车模型不再成立） - frenet需要把自车和障碍物都投影到frenet下，可能造成形状的失真。 - Frenet高精度坐标转化需要使用优化算法，否则就可能存在厘米级误差，这会被城市场景中的决策判断放大。在Frenet下的障碍物筛选也没有XY下精度高，为了安全，只能提高冗余量，牺牲决策判断。

6.1 lattice

lattic一般是在frenet下的 - 自己有幸使用Lattice算法做过量产，发现该算法从理论上讲就至少有3个理论缺陷。该算法上手快，但是后期性能提升和泛化能力很差。对于轨迹生成，我认为在XY坐标系下，基于车辆运动学或再加动力学得到轨迹的方法更优。通过减少求解信息，达到减少耗时，提高限定时间内成功求解的概率；减少坐标系转换带来的损失。

7 碰撞检测

碰撞检测一般分为粗检测和精确检测：粗检测阶段主要是为了筛选出没有碰撞可能的物体，而选择出非常有可能发生碰撞的物体，而精确检测阶段接着上一步进行更加精确的检测，最终确定物体是否有碰撞。

碰撞检测分为两大类做法： - box与box的碰撞检测：不简化车辆的形状，还是认为是矩形（粗检测和精确检测） - box与点的碰撞检测：把车辆用2-3个圆包围起来，然后检测两个圆心的距离。

分2个阶段的好处主要是为了减少运算量。

7.1 粗检测（AABB-BOX）

AABB的近似过于粗糙（检测到发生碰撞，实际没有发生碰撞），但是计算速度很快，可以通过AABB检测快速的筛选掉没有发生碰撞的障碍物。找到可能发生碰撞的障碍物，然后做更精细的检测。如果要判断n个物体是否发生碰撞，需要检测\(c_n^2\)

粗检测阶段主要是结合以下3种方法： - 对物体模型做简化，一般是采用包围盒 - 对空间做索引，筛除掉不必要的检测 - Sweep and Prune

7.1.1 包围盒

（1）AABB-box AABB-box简单点说就是垂直于坐标轴的包围盒，搞过物体识别的标注的童鞋都知道，这种包围盒不会旋转（当物体旋转的时候，就用更大的包围盒去包围它），因此使用起来非常简单。

可以看到AABB-box在物体旋转之后，还是平行于横纵坐标没有发生旋转，为了继续包围住小车，大小发生了变化。

（2）与AABB-box对应的是OBB-box, OBB-box会跟着物体的朝向一起旋转，而大小不会改变，相对来说比AABB-box更加准确，但是检测碰撞的运算量会比AABB-box大一点。

可以看到采用包围盒之后简化了需要检测碰撞的模型，用其它更加简单的形状去做粗校验，避免采用更复杂的多边形去检测碰撞。

7.1.2 空间索引

空间索引是为了对空间做切分，省略不必要的比对，比如一条路上有n辆车，如果需要检测碰撞，则需要O(n*n)次，如果对空间做切分那么可以降低到O(nlogn)次。

空间索引的算法同样非常多，大体上归为3类：

  • 树形结构：Binary-tree，Rtree，Quadtrees，KDtree，aabbtree。
  • 栅格图：对空间进行网状划分
  • 空间填充曲线：geohash，以及google最新的s2geometry

7.1.3 Sweep and Prune

下面我们主要介绍一种对AABB-box快速进行碰撞检测的算法，我们首先把路上的汽车都用盒子包围。如果汽车有旋转的情况，用AABB-box代替OBB-box。

图5 用AABB-box包围汽车

这样我们就得到了一系列的矩阵，检测2个AABB-box是否碰撞很简单。我们先看下图

如果2个AABB-box重叠，那么它在x和y轴上的投影一定都重叠，如果2个AABB-box没有重叠，那么它可能在某一个坐标轴上有重叠，也可能没有重叠。

也就是说如果有N个AABB-box我们只需要在某一个轴上检测出有重叠的区域来判断AABB-box是否发生碰撞。那么问题简化成N个线段，检测这些线程是否有重叠区域，这样就把2维的问题简化到1维来了。

我们先对这些线段的起点和终点组成一个数组，对这个由起点和终点组成的数组进行排序，排序好之后，遍历上述数组，找到可能发生碰撞的盒子，那么我们如何查找呢？

对线段进行排序，得到排序号的数组

从上图中可以看出，我们对线段的起点和终点进行排序，得到最下面的数组，遍历数组。

  1. 发现线段3的起点b3，则把3放入激活数组，又发现b6，则把6放入数组，这时候数组中有[3, 6] 2个元素，继续遍历，发现b1，则把1放入数组，这时候数组中有[3, 6, 1]。
  2. 接着发现e6，由于是终点，则从数组中移除6，同时把(3, 6)和(6,1) 这2个可能碰撞的AABB-box保存到结果。这时候激活数组中剩下[3, 1]，然后遇到e1，激活数组移除1，同时把(3,1)保存到结果。激活数组剩下[3]
  3. 接着遇到b5，激活数组[3, 5]，然后遇到b2，激活数组[3, 5, 2]然后遇到e3，移除3，把(3, 5)和(3, 2)保存结果，激活数组剩下[5, 2]。
  4. 继续遍历遇到b4，激活数组[5, 2 ,4]，遇到e5，保存(5, 2)和(5, 4)，激活数组剩下[2, 4]，遇到e4把4移除，同时保存(2, 4)到结果，最后遇到e2，移除2。
  5. 至此遍历结束，得到结果(3, 6)，(6,1)，(3,1)，(3, 5)，(3, 2)，(5, 2)，(5, 4)，(2, 4)，找到所有有碰撞可能的组合。

上述的方法需要先排序，算法复杂度O(nlogn)，然后需要遍历需要O(n)次，然后判断是否碰撞的组合有k个则需要O(k)，因此总的复杂度为O(nlogn)+O(n)+O(k)。相比O(n*n)的复杂度提高了不少。

7.2 精确检测（OBB-BOX、凸多边形等）

主要用两种方法，STA（分离轴定理）、和GJK算法

7.2.1 SAT

SAT（Separating Axis Theorem，分离轴定理）碰撞检测算法直观且高效。然而，它只适用于凸体的碰撞检测。它的原理清晰易懂，即若两个物体没有发生碰撞，则总会存在一条直线，能将两个物体分离。于是，我们把这条能够隔开两个物体的线称为分离轴。 向什么方向投影 ？？（简化，是向自然坐标系 XY 投影）

若两个凸多边形没有发生碰撞，则总会存在一条直线，两个多边形在这条直线上的投影没有重叠，如图6右侧图所示，蓝色线段和粉红色线段没有重叠部分，其中右侧图中灰色线就是一条分离轴（主观理解是下图左侧图的虚线应该叫分离轴，但是这是错误的）。

若两个多边形碰撞，则无法找到这样一条分离轴，能使两个多边形在直线上的投影不重叠，这种情况的实例如图7所示，两个多边形在任何一条轴上的投影都有重叠部分。

这个部分我们将如何获取分离轴，即上方伪代码中的Axis[] axises 的获取方式。SAT算法作者提出了一个非常棒的想法：选取多边形每条边的垂线。

7.2.2 GJK

2D Convex Collision Detection Algorithm_落羽归尘的博客-CSDN博客 GJK（Gilbert–Johnson–Keerthi）算法。它足够高效，且很容易了解它是如何进行碰撞检测的。同样的，它也只适用于 凸多边形 间的碰撞检测。 而 GJK 从 重叠 的角度来探索物体间的碰撞。

本质就是利用Minkowski差来判断2个几何体有没碰撞。因为如果碰撞了，那么2个几何体至少包含了同一个点，也就意味着它们的Minkowski差（第二幅图）必然包含原点：

7.3 点与box的碰撞检测

不同于7.1和7.2的box和box，主要讨论一个点是否在多边形内

7.3.1 射线法

判断一个点是否在多边形内，我们可以从该点引出一条水平射线（任意射线都可，但水平便于计算），观察射线与多变形的交点个数，如果交点个数为奇数，则该点在多边形内，如果为偶数则在多边形外。如下左图所示，判断点P和多边形的关系，从点P得到一个水平向右的射线，通过多边形的每两个相邻顶点可以得到边的直线方程，例如P 1 P 2 P_1P_2P1​P2​，有y 0 y_0y0​可以计算得到点B的坐标，就可以判断射线是否与P 1 P 2 P_1P_2P1​P2​相交了。此方法对多边形的凸凹性没有要求，但是如果点P在多边形边上或者顶点需要特殊处理。

7.3.2 轮廓六分圆

碰撞检测技术介绍_小作坊钳工的博客-CSDN博客

此方法是将矩形的碰撞检测转化为圆之间的碰撞检测，通过两个圆的半径和圆心之间的距离判断两个圆是否重叠。给定车辆矩形轮廓，该算法首先计算矩形轮廓的外接圆，然后将整个矩形区域分解成与四个角点对齐的同等大小的正方形，轮廓矩形区域剩下的部分再进一步分解成等大小的小矩形，最后计算每一个小矩形或正方形的外接圆[1]。

8 优化相关

https://zhuanlan.zhihu.com/p/105313548

凸优化求解方法

9 规划时如果坐标数值过大，求解不稳定怎么办

两个方法：

  • 采用local坐标系，把所有环境信息变换到以自车当前位置位基础的一个参考系内，这样所有的环境信息都是相对坐标
  • 对一些变量做缩放，算出结果后再变回去。

panning中基于采样的方法和基于搜索的方法各有什么优缺点？

1 基于采样的方法

1.1 随机性采样方法（RRT、RRT*）

优点：高维度时速度快 缺点：随机性较强，导致前端的解空间不稳定。没有办法保证最优性，只有概率完备性。不一定在有限时间内找到解。

1.2 确定性采样（lattice）

优点：更符合运动学特性，cost设计比较好时，解比较稳定 缺点：纬度高时速度慢，解的质量取决于零散化分辨率。

基于搜索的方法（A* dijkstra）

优点：全局最优性，解稳定 缺点：纬度高时速度慢，解的质量取决于零散化分辨率。