论文阅读220403_Autonomous Driving on Curvy Roads Without Reliance on Frenet Frame: A Cartesian-Based

Autonomous Driving on Curvy Roads Without Reliance on Frenet Frame: A Cartesian-Based Trajectory Planning Method 2021

0、摘要

         主要工作：提出了一种迭代计算框架来累积处理复杂约束。每次迭代求解一个中间问题，其中包含线性和可处理尺度的避碰约束和软化的运动学约束。

         开源链接 ： https://github.com/libai1943/CartesianPlanner.

1、介绍

         在弯曲的道路上，由于道路趋势的曲率较大，因此需要仔细规划轨迹，确保其符合自我车的运动学，很少有能够很好地处理弯曲道路场景的方法。本文研究的是弯曲路面上的轨迹规划问题。

1.1 相关工作

          Frenet 坐标系的缺点：1、无法模拟自我车的真实运动学。2、车辆的变形被无视。3、无法保证轨迹的连续性。4、无法进行多车的合作规划。

          我认为，第一条只能说是在高阶方法上存在一定的误差，Frenet坐标系仍然可以通过引入真实的曲率约束实现真实的运动学。车辆变形被无视，但是膨胀之后依然可以认为是安全的。第四条是确实挺难的。毕竟F坐标系是个非线性变换的坐标系，甚至于，不是一个连续映射。所以在弯曲的路上确实会出现很多问题。

1.2 动机与贡献

          提出了一种快速准确而且能够实现在弯曲道路上的最优轨迹的生成。主要实现了一种迭代计算的框架，解决了两个最重要痛点：一个是维度灾难。

2、问题声明

          将轨迹生成转化为OCP问题。

2.1 总体的结构

 

相关变量1：x：状态量，包括x，y两个坐标、朝向、速度、加速度、打角。 

相关变量2：u：控制量，加加速度、打角的角速度哦。

相关常量： 代表状态与控制量的允许的范围。 

目标函数：一些关于位置和控制量的函数

约束1：状态量与控制量的传递关系

约束2：状态量可控制量的范围，T代表一个规划的平面

约束3：终点位置约束、碰撞约束（ST图？）。

T 是一个常量还是变量勒。 

2.2 Cost function

相当于偏移量与控制量的加权和。其中偏移量部分可以被细化分为：

相当于位置偏移量与角度偏移量的加权和。

2.3 约束施加

轨迹规划任务中的约束通常包括运动学约束、两点边界约束和避碰约束。

1）运动学约束：

因为在弯曲的道路上，我们往往认为车辆走的不会特别的快，所以他们引入的变量：

theta 代表车辆前进的方向，phi是打角的方向，Lw是车前后轮轴中心的距离

2）两点边界约束

起始点： 

终止点:

将车变为三个圆：

这样实现了将车辆的边界进行平滑。

其中圆的中点是：

圆的半径是

这样就可以引入约束了:

在所预计的时间中，划分成NFE个等距的时间空间。然后约束就在这些空间里面进行施加了。

 

在上述的OCP模型中，我们使用了走廊，这会导致问题的出现：因为它们是根据粗略搜索的参考轨迹来制定的，因此很容易省去运动学可行性所需的自由空间。感觉这句话也能说明一下走廊存在的问题.

3、路径规划的方法：

本节介绍了我们提出的道路轨迹规划器。首先介绍了规划的原则，然后对方案进行了分析。

3.1 Principle

输入：粗的参考地图

输出：一条最优的轨迹

STEP 1：加载采样和搜索粗导出的参考轨迹轨迹，然后返回数值求解(12)所需的所有决策变量。

STEP 2：初始化惩罚变量，迭代次数、清空目标的traj。

STEP 3：依照所设定的迭代次数进行循环：

          1 - 使用加彩出来的参考轨迹放入到OCP中。这一步中，期望与运动学相关的约束在合并到Cost函数之前被软化为外部惩罚成本。

          2 - 优化生成一个新的轨迹。

          3 - 确定可信度，找到0到T时刻的总的惩罚

                    3-1- 如果惩罚很小了，我们就将这个轨迹返回。

                    3-2- 反之，则更新一下惩罚稀疏，并且迭代次数增加一次。

RETURN；

OCP的更新：

 

惩罚函数的设定： 

 

一个是f的动力学的参考，还有一个是有关相关圆的内容。

         SolveNLP（OCP，x）详细的步骤包括将OCP离散为一个非线性规划问题，然后通过一个基于梯度的局部优化器来解决它。详细的步骤包括将OCP离散为一个非线性规划问题，然后通过一个基于梯度的局部优化器来解决它。

          MeasureInfeasibility()，测量的运动学不可行性程度。使用的就是f_penalty

3.2 性能分析

         这一部分该分段利用了所提议的轨迹规划器的几个特性。

         1、我们的算法通过优化轨迹不停地更新走廊是由轴向对齐的局部盒子组成的，因此自由空间不可避免地会被部分遗漏，从而导致优化甚至可行性的丧失。在迭代期间更新走廊可以有效地减少遗漏的空闲空间。在迭代过程中，参考轨迹的可行性/最优性得到改善。2、其次，在迭代过程中，参考轨迹的可行性/最优性不断提高，为前文提到的更新走廊策略的效率奠定了基础。3、最新的走廊内约束是围绕最初的猜测构建的。4、虽然问题是非凸的，但是我们的约束是线性的，所以解的会比较快。而且我们的初始值也会比较好。

          目前，自动驾驶轨迹规划领域普遍采用简化OCP的方法，将其离散化为二次规划(QP)问题，而非非凸性的NLP问题。然而，制定QP通常需要线性化运动学约束。当泰勒展开点质量较差时，线性化的运动学约束容易引起误解。在这种情况下，需要更多的迭代，尽管每个迭代中的CPU运行时间很小。

          5、选择轴线对齐的盒子来形成走廊，我们进行的走廊构建更快。6、达到第九行的时候我们认为获得了最优的节点，随着迭代次数的增加，惩罚会越来越少。迭代过程中wpenalty会不断增加，这样既可以加速着一个过程。7、如果对CPU运行时间有严格的限制，整个迭代优化框架可以以任anytime style实现。即使所提出的规划器在预定义的运行时间限制或迭代中没有找到有效的最优值，只要最终轨迹的近期段在运动学上是可行的，那么结果仍然可以传递给下一个规划框架进行进一步的改进。

4、实验验证

【T-ITS】Trajectory Planning for Autonomous Driving on Curvy Roads_哔哩哔哩_bilibili

4.1 仿真的setup：

          使用的是IPOPT在MATLAB+AMPL中实现。

4.2 对提出的Planner 的验证

 

这意味着要求每个中间OCP纯粹包含框约束是减少运行时的有效方法。使用了另外的一种硬约束来求解这个问题，发现求解时间明显变长了。

4.3 与EM Planner的对比

         EM规划器只消耗0.046秒完成，这是相当快的。但是这种基于Frenet的方法无视了本车的一些运动学定律，这些定律实际上存在于现实世界的笛卡尔坐标系中。

4.4 与基于MPC的方法的对比

         基于mpc的规划器通过解耦名义上耦合在一起的车辆运动学约束和避碰约束，为轨迹规划过程提供了便利。该方法对非线性运动学约束进行线性化处理，通过引入额外松弛变量来松弛避碰约束，从而构建了松弛变量最小的OCP。对OCP进行迭代求解，直到收敛。但是处理时常需要1s比我们这种方法要长地多。

          基于mpc的规划器是在迭代过程中松弛变量会改变同伦类，从而带来了使规划轨迹与高层模块决策不一致的风险。

4.5 实车实验

         实验在Quanser公司生产的小型自动驾驶汽车平台QCar上进行。

          在从启动实例到结束的规划帧中，DP搜索过程平均消耗0.732ms, Alg. 1平均消耗273.630ms。两者的运行时间都远低于整个规划水平长度，这表明所提出的规划器能够处理现实世界中的道路轨迹规划任务。

          也就是说，这玩意不是个实时的，也就组不成大的控制闭环了。但是又能够向前发展的希望。

5、结论

          这变文章通过把问题转化成一个快速而简单问题的迭代的形式从而加速在笛卡尔坐标系下的求解，规避了一些Frenet坐标系的限制，基于笛卡儿的规划器如果设计得好，有望迅速提供准确的解决方案。

          Frenet 的优势：值得强调的是，基于frenet的规划师在描述道路[24]上与交通规则相关的约束方面有其天然的优势，而对基于笛卡尔的规划师来说，对这些约束进行建模并不容易。我们未来的工作重点是如何在基于笛卡尔轨迹规划器的道路自动驾驶场景中描述空间约束。此外，还需要开发一种统一的方法，将笛卡尔框架和弗雷内特框架结合起来，并在道路场景变化时自适应地在它们之间切换。

994、阅读总结

          一种挺好的思路，将复杂问题转化为简单问题的迭代，但是实现的是一个非实时的系统，参考轨迹的生成的速度要远远地低于车辆的控制速度。但是在未来有望转化成不错的东西。

999、词语积累

aforementioned adj. 上述的

curvy adj. 弯曲的

dimensionality n. 维度

drastically adv. 猛烈地

equidistant adj.等距的

prevalent adj. 盛行的

skeleton n. 骨骼

tackle v. 应对

oriented adj. 以……为方向的，重视……的

inherently adv. 内在的，固有的

intermediate adj. 中间的

irregular adv.不平整的

参考文献

[1]–[3]  城市交通系统中的安全、舒适和机动性

[4] 道路规划主要关心如何使轨迹的曲率有限和连续，同时保持轨迹不发生碰撞

[5] 一个停车导向的规划集中在如何通过一些机动驾驶车辆到一个期望的配置

[6] 关于道路轨迹规划的出版物

[7] Frenet框架又称曲线框架，已广泛应用于道路轨迹规划任务的建模

[8] 将原耦合运动学约束解耦为纵向和横向尺寸上的独立多项式,在Frenet框架中采样候选轨迹并将其正则化，然后在笛卡尔框架中评估其代价

[9]-[14] QP

[15] 采用虚拟控制器对采样后的轨迹基元进行快速探索随机树(RRT)搜索跟踪

[16][17] 通过模型预测控制方法对RRT原语进行后处理，增强了每个候选轨迹原语在笛卡尔坐标系下的运动学可行性。

[18][19] 基于优化规划的方法能够在连续解空间中找到无限候选解的最优解，比基于选择的方法具有更大的灵活性

[21] 由于自我车的轮廓不是光滑的（矩形的），碰撞避免约束即道路障碍物和障碍物是名义上不可微的。

[22]  将自我车身划分为多个部分，并为每个部分构造廊道，但廊道内约束下的伪距离函数仍然是非凸的，因此产生了较大的运行时间

[23] 提出了一种凸可行集方法使走廊内约束线性化，但走廊内约束的维数不固定，从而使OCP求解的性能发生变化。

[24][25] 用一系列与参考轨迹对齐的矩形构建走廊

[26]假设曲率轮廓是参数化的三次样条曲线

[27] 建立迭代框架，反复求解线性化运动学约束的QP问题;一次迭代得到的最优值作为下一次迭代的一阶泰勒展开点

[28][29] 通过动态规划在抽象的状态空间中进行搜索，得到参考轨迹。

[30] bicycle模型可以充分描述车辆的机动性

endl;