从约束出发：轨迹规划问题的求解及失败处理

z他山之石

于 2024-12-15 20:19:00 发布

阅读量1k

点赞数 22

分类专栏：自动驾驶文章标签：人工智能算法机器学习

本文链接：https://blog.youkuaiyun.com/weixin_39739999/article/details/144491339

版权

自动驾驶专栏收录该内容

11 篇文章

订阅专栏

对于轨迹规划问题，经过建立适当的物理模型和数学模型，最后通常都能转换成一个优化问题，本文首先梳理了优化问题的基础知识和轨迹规划问题的求解，最后从约束的角度探讨了优化问题失败后的处理。

优化问题基础

下面是标准的优化问题，其中，x为优化变量，f(x)为目标函数，g(x)为不等式约束，h(x)为等式约束；

根据是否存在h和g，优化问题可分为无约束优化问题和有约束优化问题。对于无约束优化问题，有很成熟的求解算法，如各种梯度类算法。带约束优化问题的求解会复杂很多，而且没有通用的解决方法，一种简单常用的求解思路是把约束问题转化为一系列无约束问题，用一系列无约束优化问题的解来逼近约束问题的解。典型的如惩罚函数法，将对偏离约束的惩罚加到原来的目标函数组成新目标函数，从而转换成无约束目标函数。罚函数方法又可以分为外点罚函数法和内点罚函数法，两者的区别在于对约束的处理方式不同。

外点罚函数法

等式约束用 $h^2(x)$ 惩罚，不等式约束用 $max\{g(x),0\}$ 惩罚，所以新的目标函数为

$\mathop{f}\limits_{}^-(x)=f(x)+(\sum_{i=1}^{m}{\sigma_i}h_i^2(x)+\sum_{i=1}^p{\sigma_i}[max\{g_i(x),0\}]^2)$

只要无约束问题的解是约束问题的可行点就是原问题的极小值点，而且惩罚因子sigma越大，对无约束极小值点不在可行域内的惩罚越大，所以也越可能落在可行域内。求解过程中，算法迭代进行，只要找到的无约束解不可行就增大罚因子，然后继续求解无约束极值点。这种方法的特点是，1. 不要求初始点在可行域内；2. 无法保证结果一定严格满足可行性条件；3. 随着惩罚因子的增大，在新目标函数中目标函数占的比例越来越小，约束惩罚占的比例越来越大，使得优化问题的适定性变得越来越差。

内点罚函数法

内点罚函数法又称为障碍函数法，使用对数函数、指数函数或倒数函数等作为惩罚函数处理不等式约束，当近似极小值点接近可行域边界时惩罚函数变得很大。内点罚函数法的特点是，1. 不能处理含有等式约束的问题；2. 要求可行域含有内点；3. 由于可行域边界处的高墙的存在，内点罚函数法在近似点靠近可行域边界时罚函数会变得病态，往往难以求得最小值点。

轨迹规划问题

将轨迹规划问题转成标准的优化问题形式，目标函数f(x)为期望的轨迹所具有的属性，一般包括安全性、舒适性和通行效率；等式约束h(x)除包括起始点和终止点约束外，大部分的等式约束为相邻两点的跳转方程，即建立的物理模型；在不等式约束g(x)中，定义了优化变量的可行域，一般包括安全边界，车辆的物理操作边界等。

目标函数和状态方程方面的讨论可以参考从描述驾驶体感到建立状态方程，本文主要讨论下轨迹规划中的约束问题及其求解方法；

轨迹规划问题中的约束主要包括状态量约束、控制量约束、以及可行驶区域约束。状态量和控制量的约束很好表示，都是典型的框约束，边界值也很好确定。但是可行驶区域，即位置(x,y)约束的表示和处理就比较复杂，其边界包括车道线、规则障碍物以及无语义障碍物。

对于优化问题的求解，一种方法是使用各种求解器（如ipopt、osqp等）来直接对优化问题进行求解，如Apollo使用osqp求解器，这些经过验证的成熟求解器稳定、可靠，有方便调用的接口，可以大大减小了开发工作量，但是其缺点也很明显，求解过程对开发者处于黑盒状态，开发者对其干预程度很低，难以根据需要提前结束求解或者使用中间结果。如果开发者自己开发求解器，虽然可白盒控制求解过程，但是工作量和开发难度都很大。

自动驾驶中的轨迹规划问题有其特点，1.实时性很强，对计算时间有严苛的要求；2.对结果的最优性要求不高；3. 必须要有轨迹输出，换句话说，没有最优的轨迹，求解器必须及时输出一条可行的轨迹。

智能驾驶技术从有图模式逐渐向无图模式过渡，轨迹规划问题从由参考线跟踪占主导的问题转变为障碍物避障为主导的问题，对应的求解方法也从传统的基于QP求解向基于ilqr求解。lqr方法的求解精度可能比不上梯度类算法，由于lqr是少有的可以通过解析方法来求极值的算法，所以其求解速度非常快，通常只有求解器求解时间的1/3～1/6。这个特点也是其在规划问题中使用越来越广泛的原因，而且其运算过程对开发者而言也是白盒状态。在ilqr的求解方法中，对于约束的处理，都是将其以惩罚项的形式写到目标函数中，然后进行lqr迭代求解，关于ilqr的基本知识可以参考从描述驾驶体感到建立状态方程。根据内外罚函数的特点，以及轨迹规划问题的特性，ilqr算法建议选择外罚函数法来处理约束条件。

约束及失败处理

约束可分为硬约束(hard-constraint)和软约束(soft-constraint)两种，硬约束在任何时候都必须满足，比如规划的轨迹不能与其他物体发生碰撞（安全性），比如规划的轨迹不能超过车辆的可执行能力（可控性）。软约束是尽可能满足的约束，比如轨迹需要尽可能在车道线中间，规划的速度尽可能接近驾驶员设定的速度。硬约束确保了解的有效性，而软约束则帮助我们在满足硬约束的基础上，优化解的质量。合理的约束对问题的求解至关重要，约束过多（过约束）会导致问题求解失败，约束过少（欠约束）会导致解的质量偏离我们的预期。约束的优先级划分对问题求解失败后的处理至关重要，轨迹规划问题的约束优先级为可控性>安全性>软约束。安全性也可根据碰撞目标来进行划分优先级，例如vru目标大于无语义的护栏等。

由于外点罚函数法无法确保结果满足硬约束，所以在实际开发中，需要对ilqr输出的轨迹进行可行性检查，主要包括安全性和可控性。如果ilqr的输出轨迹不满足硬约束，必须要进行规划失败的后处理，下面讨论规划失败后的处理问题。