【自动驾驶】决策规划算法 | 数学基础（二）凸优化与非凸优化

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【决策规划】系列，带您深入浅出，领略规划之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

引言

  各位小伙伴们大家好，本篇博客是自动驾驶决策规划算法数学基础的第二节，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  凸优化是比二次规划更宽泛的概念。本篇博客的讲解不像数学系讲得那么细，比如凸优化的来源、凸优化的定义、怎么求解凸优化问题、KKT 条件之类的问题不会讲。对于做自动驾驶的人来说，凸优化就是调包，把数据结构弄好，然后喂到包里面去，算结果就可以了。

  无论是 Matlab 还是 C++，都有成熟的计算凸优化的包，所以不用关心到底是怎么算的，但是需要关心有什么使用条件，有什么优缺点，在什么地方可以用，在什么情况下会出问题。所以本节只是在宏观方面讲凸优化。

  自动驾驶规划目标：计算出一条满足各种约束的最优轨迹，然后把轨迹发给控制模块执行。

  在学习规划系列之前，要先把控制这块给过了，控制学会了再学规划，否则学习效果会大大打折扣，因为规划算完了，没法控制，也不知道规划得好不好，没有反馈。

---

一、最优轨迹评价指标

那么问题来了，什么是 最优轨迹 呢？最优的标准是什么呢？

一般有以下几个指标：

• 平滑性
• 舒适性
• 长度短
• 耗时少

---

二、代价函数构建与约束条件

  车辆轨迹表达式如下：
$$s=f\left(t\right)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$$其中，$a_0,a_1,a_2,a_3,a_4.a_5$都是未知常数

1、构建代价函数

  将评价指标量化，衡量轨迹质量用代价函数表示，代价函数可以写成：

  其中，$w_1、w_2、w_3$ 是相应的权重。 $J$ 越小意味着这些导数越靠近 $0$，也就意味着轨迹越平滑、越舒适。

2、约束条件

当然也要满足各种各样的约束条件：

• 轨迹连续
• 规避碰撞
• 遵守交通规则
• 满足车辆动力学约束

  但是在这里不具体讲怎样根据指标建立具体的代价函数，在后续讲规划时会讲到。

  本篇博客只讲数学问题，就是求解代价函数在约束下的最小值问题，即如何计算 $y=f(x)$ 满足约束条件下的最小值。

---

三、高中数学求函数最值的方法回顾

  回忆高中时，解 $y=f(x)$ 在$x\in \left[a,b\right]$ 上最小值时怎么算（假定$f(x)$ 是连续可导函数）。

• 第一步：先算出端点值 $f(a)、f(b)$

• 第二步：对 $y$ 求导 $y^{\prime }=f^{\prime }\left(x\right)$

• 第三步：令导函数 $y^{\prime }=0$，解出 $f^{\prime }(x)=0$ 的根，设为 $x_1,x_2,...,x_n$

• 第四步：计算 $f(x_1),f(x_2),...,f(x_n)$，与端点值 $f(a),f(b)$ 比较，在$f(x_1),f(x_2),...,f(x_n),f(a),f(b)$ 中选最小值，就是 $y=f(x)$ 在$[a,b]$ 上的最小值

---

四、凸优化与迭代法的引入

1、高维复杂函数的最值问题

  上面这种方法无法快速求解高维复杂约束下复杂函数的最值问题。

  举个例子，比如以下函数：
$$y=\ln \sin \left(x+\frac{1}{x^2}\right)+e^{\cos \ln \left(x^2+1\right)}+x^5+\frac{1}{x^2+6}$$  看起来比较复杂，但是求导求 $\frac{dy}{dx}$ 还是不难的，但是想快速求导函数的零点比较困难。

  这还是未知数只有一元函数的求最值问题，都已经比较复杂了，如果是高维的多元函数，通过求导数算最小值，是非常困难的。

2、求导法的局限性

  而且就算求出了导函数的零点，还有其他的问题，比如这样的函数：

就算求出极值点，但仍存在以下问题：

• 极值点非常多，难以快速判断哪个是最小的极值点，还要和端点比较
• 约束复杂，是许多不连续小区间的并集，处理麻烦

  所以一般求复杂函数在复杂约束下的最小值问题，都采用迭代法求解，而不采用求导法。

3、迭代法求解的优势

  迭代法的计算速度比求导法快很多，在自动驾驶里计算速度是非常重要的指标。在自动驾驶的功能不可能让慢慢地算，让慢慢地规划，这是不可能的，等算完了，说不定车都已经撞上去发生交通事故了，所以必须要有很高的执行速度。

4、常见迭代算法概览

常见的迭代算法：

• 牛顿法：在《数值分析》这本书里面有
• 梯度下降法：在机器学习中非常经典
• 高斯牛顿法：在《视觉Slam 十四讲》书里提到过

  在这里不具体讲迭代算法是怎么工作的，如果对这些算法不是特别了解的，可以去看相关资料。在人工智能特别是深度学习中，数值求解代价函数最小值是非常经典的问题，网上相关的资料非常多，所以在这里就不细讲具体实现过程，只讲一下大概原理。

---

五、梯度下降法原理与示例

1、梯度下降法基本原理

  以梯度下降法为例，讲一下大概过程。

  比如这样的函数：

  首先给初值 $x_0$，梯度下降法的大概的原理是根据初值的导数判断下一步迭代的方向，如果是一元函数就是导数，如果是多元函数就是梯度，所以叫梯度下降。的原理就是按照梯度的反方向迭代。

2、迭代过程示例

  比如， $x_0$ 的梯度为负，所以就往正方向迭代。迭代出 $x_1$，$x_1$ 导数也为负，那就再往前一步到 $x_2$，再迭代到 $x_3$，梯度为正，就往后迭代成 $x_4$，其梯度为是，再迭代叠到 $x_5$，直到收敛。就按梯度反方向迭代，直到找到最小值。

  梯度下降法，下一步的迭代方向取决于上一步的导数的正负。如果导数为负，就往正方向迭代；如果导数为正，就往负方向迭代。

3、梯度下降法收敛性

梯度下降法算法如何收敛？

  因为迭代不仅有方向，还有大小，就是到底移动了多少，大小按照导数的大小判断，如果导数为负，但绝对值比较大，步长移动相对较长，如果导数为正，但相对较小，那么移动步长就比较小。

  在本例从 $x_4$ 到 $x_5$ 时， $x_4$ 虽然导数为正，应该向后迭代，但导数已经比较小了，所以从 $x_4$ 到 $x_5$ 的步长不会特别大，而 $x_5$ 的导数几乎为 $0$ ，下一步迭代就几乎不怎么动，所以最终收敛。

  梯度下降法收敛，要么直接收敛到极小值点，要么收敛到约束边界上。

  在带约束的求函数最小值问题，并且在约束的空间内，函数是单调的，即在约束空间内并没有极值点的话，梯度下降法最终会收敛到约束边界上。

4、梯度下降法与其他迭代算法的比较

  虽然只讲了梯度下降法这一种迭代方法，但实际上大多数迭代方法，比如牛顿法、高斯牛顿法，原理都和梯度下降法大同小异：

• 梯度下降法：用到的就是一阶导数迭代
• 牛顿法：不仅用了一阶导数，还用了二阶导数
• 高斯牛顿法：是对牛顿法进行一些相关的改进，使计算起来更快一点。

---

六、迭代法的缺点与初值敏感性

1、迭代法对初值的敏感性

  先看这样的问题：

  约束界是两个小区间，如果用迭代法，以 $x_0$ 为迭代初值，最终会迭代到 $x_1$，如果以 $x_0^{\prime }$ 为迭代初值，最终会得到 $x_1^{\prime }$。

  迭代法对初值比较敏感，即使约束比较简单（不是破碎空间，是比较完整的空间），也有可能收敛到局部极小值，比如下图：

函数有多个极小值，对迭代法的初值比较敏感。比如：

• 初值为 $x_0$，最终会迭代到 $x_n$
• 初值为 $x_0^{\prime }$，最终会迭代到 $x_n^{\prime }$
• 初值为 $x_0^{\prime \prime }$，最终会迭代到 $x_n^{\prime \prime }$

  即选不同的初值，会迭代到不同的局部极小值上。有可能运气好，能找到最小值，但是有可能运气不好，找到的是局部极小值。

2、性质较好的问题

  当然也有性质比较好的问题，比如这样的函数：

  在这样的约束空间下，只有极值点，且为极小值点，并且约束空间是一块完整的空间，不破碎。

  或者是这样的问题：

  没有极值点，但在整个约束空间中是单调的，而且约束空间也是完整的、不破碎的空间。

---

七、凸优化问题的定义与性质

  这种问题的迭代法对初值就不敏感，取什么初值都能迭代到最小值，而且迭代法收敛的解必然是全局最小，即在约束条件下的全局最小值。

  这种性质比较好的问题，叫做 凸优化。

1、凸优化问题的性质

凸优化必然有两个性质：

• 凸函数：代价函数只有单个极值点，且为极小值
  这样表述其实不严谨，因为代价函数可能没有极值点，为单调函数。
• 凸空间：约束空间是一块完整的、不破碎的空间

求凸函数在凸空间的最小值问题被称为凸优化问题。

2、凸空间的定义

为什么完整的空间叫凸空间呢？

  凸空间的严谨定义由凸多边形衍生而来，凸多边形和凹多边形如下：

凸多边形：对于多边形内部任意的点 $x、y$，都有 $\frac{x+y}{2}$ 表示的点也在多边形内部。

凹多边形：存在两个点 $x、y$，使得 $\frac{x+y}{2}$ 表示的点不在多边形内部。

凸空间：完整的空间，比如下图：

  区间内任选两点 $x、y$，点 $\frac{x+y}{2}$ 也在区间里。

非凸空间：破碎的空间，比如下图：

  区间内存在两点 $x、y$，中点 $\frac{x+y}{2}$ 不在区间内。

  注意：多边形有凸多边形和凹多边形，但空间只有凸空间和非凸空间，没有凹空间，这不是专门的学术名词，只有凸空间和非凸空间，因为凸和非凸数学上已经远远超过几何意义了，是更抽象的东西。

---

八、凸优化与自动驾驶规划算法的关系

随之而来的两个问题：

• 为什么要讲凸优化？
• 凸优化和自动驾驶规划算法有什么联系？

1、凸优化的重要性

为什么讲凸优化呢？

  因为凸优化是最简单的非线性规划方法，也是人类唯一掌握的，唯一的研究的比较明白、比较彻底的一种非线性优化方法。凸优化是所有复杂优化方法的基石，很多非凸问题都是想办法去转化成凸问题，然后求凸优化的解。

2、凸优化与自动驾驶规划的关系

自动驾驶和凸优化有什么关系？

  自动化驾驶的规划的求解是凸优化问题。

3、凸优化的基本因素

凸优化有两个因素：

• 代价函数
• 约束空间

4、自动驾驶中的避障约束空间

考虑问题：自动驾驶中的避障约束空间是不是凸空间？

  显而易见，不是凸空间。

  举例如下：

  避障的约束空间是红色区域，除了树附近不能去之外，其他地方都可以去，那么这块空间很显然是一块破碎的空间，因为中间有个洞，作为车来说，可以从上面绕过，也可以从下面绕过，但是上面和下面加起来除以 $2$ 这条绿色线，就不可以，上面这条线满足避障约束，下面也满足避障约束，但是和上向下加起来除以 $2$ 就不满足避障约束，所以不是凸空间。

  对于动态障碍物也是一样的。

  比如下图车辆规避横穿马路的行人场景，如果规划车速较快，人到道路上时车已经远远超过人，满足避障约束。

  如果规划车速较慢，车还没到人的位置，也是满足避障约束。

  如果车速快 $+$ 车速慢除以 $2$，人和车就贴在一起，发生交通事故。

  上面避让树是静态避障的案例，下面避让人是动态避障的案例。

  自动驾驶规划中静态和动态避障的约束空间都不是凸空间，所以规划算法相对比较复杂、比较难做。因为解空间或约束空间是非凸的，当然难点远不止于空间是非凸的，还有其他难点，后面再说。

  显然，虽然不知道代价函数是不是凸函数，但是至少解空间和约束空间肯定不是凸空间，所以肯定是非凸问题。

5、凸优化与非凸优化的对比

  对于凸问题，只要给初值，然后迭代就可求解，迭代的最后迭代收敛的值一定有全局最小值。这是凸问题的性质，是好性质。

  对于非凸问题，可能存在多个极值点，或者空间破碎，结论非常依赖于初值，初值选的不好就会收敛到局部极小值。

如何求解非凸问题的最小值？

  可以很遗憾地说，目前为止没有完美解决所有的非凸问题的算法，只是针对不同情况有不同的考虑。

---

九、非凸优化问题的求解思路与策略

1、非凸问题求解思路

  求解非凸问题的主要思路就是找非凸问题中的凸结构。

  比如这样的问题：

函数是凸的，但约束空间不是凸空间，要怎样求解这样的问题？

  主流方法是启发式算法，首先随机在约束空间里采样一些离散函数值，比大小，然后取值最小的作为迭代初值。

2、函数与约束空间非凸的情况

  对于非凸函数以及非凸空间的最小值问题，方法也差不多，比如这样的函数：

  先离散地在约束空间中采样取点，比较大小，从中选最小的作为初值迭代。用梯度下降法、高斯牛顿法这类方法迭代，先在约束空间里采样，然后再找到采样点的最小值。本质上是连续空间离散后，在离散约束空间的最优解。

3、非凸问题的求解流程

  举个例子，在连续的约束空间中采样撒点，想要在哪个点选最小值，本质上是把连续的约束空间转换为一个个离散点：

  即最优解只能在这些点中寻找，采样比大小，然后求出最小值，最小值本质上是连续约束空间离散化后，在离散约束空间的最优解，是粗略的解。

  对于非凸空间，求解过程如下：

  首先离散化，然后再采样得到粗解，再对粗解进行迭代，得到最终解，粗解起到基点的作用，以粗解为基础，然后得到最终解。

  求解思想：先离散地采样撒点，看一下最小值最有可能的位置，然后再根据最有可能位置作为初值迭代，从而找到最优解。

4、非凸问题求解的局限性

  当然方法也不是尽善尽美，比如这样的非凸问题：

  如果采样点较少，像这样采样点并没有落在最优解所在的约束空间，就容易陷入局部极小值。所以采样点越少就越容易收敛到局部最优，而不是整体得最值，但采样多容易发生维度灾难。

---

十、维度灾难

• 一维空间中，在前后两个方向上采样：
  

• 二维空间中，在前后左右方向上采样：
  

• 三维空间中，在前后左右上下方向上采样。

  如果一维空间采样 $100$ 个点，那么二维空间可能要采样 $1$ 万个点，三维的话就要还要乘 $100$，就是采 $100$ 万个点，所以非凸优化没有尽善尽美的解决方案，只能根据不同问题调整。

---

十一、总结

  本篇博客主要从宏观层面介绍了凸优化在自动驾驶规划中的应用，讲解了代价函数构建、约束条件处理、迭代法求解以及非凸优化问题的处理策略。

  下一节将介绍自然坐标系和直角坐标系之间的转化关系及相关推导。

---

参考资料

  自动驾驶决策规划算法第一章第二节 凸优化与非凸优化

---

后记：

🌟 感谢您耐心阅读这篇关于 凸优化与非凸优化 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀