写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！📚 系列专栏：【决策规划】系列，带您深入浅出，领略规划之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、最优轨迹评价指标
- 二、代价函数构建与约束条件
- 三、高中数学求函数最值的方法回顾
- 四、凸优化与迭代法的引入
- 五、梯度下降法原理与示例
- 六、迭代法的缺点与初值敏感性
- 七、凸优化问题的定义与性质
- 八、凸优化与自动驾驶规划算法的关系
- 九、非凸优化问题的求解思路与策略
- 十、维度灾难
- 十一、总结
- 参考资料
引言
各位小伙伴们大家好，本篇博客是自动驾驶决策规划算法数学基础的第二节，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。
凸优化是比二次规划更宽泛的概念。本篇博客的讲解不像数学系讲得那么细，比如凸优化的来源、凸优化的定义、怎么求解凸优化问题、KKT 条件之类的问题不会讲。对于做自动驾驶的人来说，凸优化就是调包，把数据结构弄好，然后喂到包里面去，算结果就可以了。
无论是 Matlab 还是 C++，都有成熟的计算凸优化的包，所以不用关心到底是怎么算的，但是需要关心有什么使用条件，有什么优缺点，在什么地方可以用，在什么情况下会出问题。所以本节只是在宏观方面讲凸优化。
自动驾驶规划目标：计算出一条满足各种约束的最优轨迹，然后把轨迹发给控制模块执行。
在学习规划系列之前，要先把控制这块给过了，控制学会了再学规划，否则学习效果会大大打折扣，因为规划算完了，没法控制，也不知道规划得好不好，没有反馈。
一、最优轨迹评价指标
那么问题来了，什么是 最优轨迹 呢？最优的标准是什么呢？
一般有以下几个指标：
- 平滑性
- 舒适性
- 长度短
- 耗时少
二、代价函数构建与约束条件
车辆轨迹表达式如下：
s = f ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 s=f\left( t \right) =a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 s=f(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5其中， a 0 , a 1 , a 2 , a 3 , a 4 . a 5 a_0,a_1,a_2,a_3,a_4.a_5 a0,a1,a2,a3,a4.a5都是未知常数
1、构建代价函数
将评价指标量化，衡量轨迹质量用代价函数表示，代价函数可以写成：
其中， w 1 、 w 2 、 w 3 w_1、w_2、 w_3 w1、w2、w3 是相应的权重。 J J J 越小意味着这些导数越靠近 0 0 0，也就意味着轨迹越平滑、越舒适。
2、约束条件
当然也要满足各种各样的约束条件：
- 轨迹连续
- 规避碰撞
- 遵守交通规则
- 满足车辆动力学约束
但是在这里不具体讲怎样根据指标建立具体的代价函数，在后续讲规划时会讲到。
本篇博客只讲数学问题，就是求解代价函数在约束下的最小值问题，即如何计算 y = f ( x ) y=f(x) y=f(x) 满足约束条件下的最小值。
三、高中数学求函数最值的方法回顾
回忆高中时，解 y = f ( x ) y=f(x) y=f(x) 在 x ∈ [ a , b ] x\in \left[ a,b \right] x∈[a,b] 上最小值时怎么算（假定 f ( x ) f(x) f(x) 是连续可导函数）。
-
第一步：先算出端点值 f ( a ) 、 f ( b ) f(a)、f( b) f(a)、f(b)
-
第二步：对 y y y 求导 y ′ = f ′ ( x ) y'=f'\left( x \right) y′=f′(x)
-
第三步：令导函数 y ′ = 0 y'=0 y′=0，解出 f ′ ( x ) = 0 f'(x)=0 f′(x)=0 的根，设为 x 1 , x 2 , . . . , x n x_1,x_2,...,x_n x1,x2,...,xn
-
第四步：计算 f ( x 1 ) , f ( x 2 ) , . . . , f ( x n ) f(x_1),f(x_2),...,f(x_n) f(x1),f(x2),...,f(xn)，与端点值 f ( a ) , f ( b ) f(a),f(b) f(a),f(b) 比较，在 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) f(x1),f(x2),...,f(xn),f(a),f(b) 中选最小值，就是 y = f ( x ) y=f(x) y=f(x) 在 [ a , b ] [a,b] [a,b] 上的最小值
四、凸优化与迭代法的引入
1、高维复杂函数的最值问题
上面这种方法无法快速求解高维复杂约束下复杂函数的最值问题。
举个例子，比如以下函数：
y = ln sin ( x + 1 x 2 ) + e cos ln ( x 2 + 1 ) + x 5 + 1 x 2 + 6 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} y=lnsin(x+x21)+ecosln(x2+1)+x5+x2+61 看起来比较复杂，但是求导求 d y d x \frac{dy}{dx} dxdy 还是不难的，但是想快速求导函数的零点比较困难。
这还是未知数只有一元函数的求最值问题，都已经比较复杂了，如果是高维的多元函数，通过求导数算最小值，是非常困难的。
2、求导法的局限性
而且就算求出了导函数的零点，还有其他的问题，比如这样的函数：
就算求出极值点，但仍存在以下问题：
- 极值点非常多，难以快速判断哪个是最小的极值点，还要和端点比较
- 约束复杂，是许多不连续小区间的并集，处理麻烦
所以一般求复杂函数在复杂约束下的最小值问题，都采用迭代法求解，而不采用求导法。
3、迭代法求解的优势
迭代法的计算速度比求导法快很多，在自动驾驶里计算速度是非常重要的指标。在自动驾驶的功能不可能让慢慢地算，让慢慢地规划，这是不可能的，等算完了，说不定车都已经撞上去发生交通事故了，所以必须要有很高的执行速度。
4、常见迭代算法概览
常见的迭代算法：
- 牛顿法：在《数值分析》这本书里面有
- 梯度下降法：在机器学习中非常经典
- 高斯牛顿法：在《视觉Slam 十四讲》书里提到过
在这里不具体讲迭代算法是怎么工作的，如果对这些算法不是特别了解的，可以去看相关资料。在人工智能特别是深度学习中，数值求解代价函数最小值是非常经典的问题，网上相关的资料非常多，所以在这里就不细讲具体实现过程，只讲一下大概原理。
五、梯度下降法原理与示例
1、梯度下降法基本原理
以梯度下降法为例，讲一下大概过程。
比如这样的函数：
首先给初值 x 0 x_0 x0，梯度下降法的大概的原理是根据初值的导数判断下一步迭代的方向，如果是一元函数就是导数，如果是多元函数就是梯度，所以叫梯度下降。的原理就是按照梯度的反方向迭代。
2、迭代过程示例
比如， x 0 x_0 x0 的梯度为负，所以就往正方向迭代。迭代出 x 1 x_1 x1， x 1 x_1 x1 导数也为负，那就再往前一步到 x 2 x_2 x2，再迭代到 x 3 x_3 x3，梯度为正，就往后迭代成 x 4 x_4 x4，其梯度为是，再迭代叠到 x 5 x_5 x5，直到收敛。就按梯度反方向迭代，直到找到最小值。
梯度下降法，下一步的迭代方向取决于上一步的导数的正负。如果导数为负，就往正方向迭代；如果导数为正，就往负方向迭代。
3、梯度下降法收敛性
梯度下降法算法如何收敛？
因为迭代不仅有方向，还有大小，就是到底移动了多少，大小按照导数的大小判断，如果导数为负，但绝对值比较大，步长移动相对较长，如果导数为正，但相对较小，那么移动步长就比较小。
在本例从 x 4 x_4 x4 到 x 5 x_5 x5 时， x 4 x_4 x4 虽然导数为正，应该向后迭代，但导数已经比较小了，所以从 x 4 x_4 x4 到 x 5 x_5 x5 的步长不会特别大，而 x 5 x_5 x5 的导数几乎为 0 0 0 ，下一步迭代就几乎不怎么动，所以最终收敛。
梯度下降法收敛，要么直接收敛到极小值点，要么收敛到约束边界上。
在带约束的求函数最小值问题，并且在约束的空间内，函数是单调的，即在约束空间内并没有极值点的话，梯度下降法最终会收敛到约束边界上。
4、梯度下降法与其他迭代算法的比较
虽然只讲了梯度下降法这一种迭代方法，但实际上大多数迭代方法，比如牛顿法、高斯牛顿法，原理都和梯度下降法大同小异：
- 梯度下降法：用到的就是一阶导数迭代
- 牛顿法：不仅用了一阶导数，还用了二阶导数
- 高斯牛顿法：是对牛顿法进行一些相关的改进，使计算起来更快一点。
六、迭代法的缺点与初值敏感性
1、迭代法对初值的敏感性
先看这样的问题：
约束界是两个小区间，如果用迭代法，以 x 0 x_0 x0 为迭代初值，最终会迭代到 x 1 x_1 x1，如果以 x 0 ′ x'_0 x0′ 为迭代初值，最终会得到 x 1 ′ x'_1 x1′。
迭代法对初值比较敏感，即使约束比较简单（不是破碎空间，是比较完整的空间），也有可能收敛到局部极小值，比如下图：
函数有多个极小值，对迭代法的初值比较敏感。比如：
- 初值为 x 0 x_0 x0，最终会迭代到 x n x_n xn
- 初值为 x 0 ′ x'_0 x0′，最终会迭代到 x n ′ x'_n xn′
- 初值为 x 0 ′ ′ x''_0 x0′′，最终会迭代到 x n ′ ′ x''_n xn′′
即选不同的初值，会迭代到不同的局部极小值上。有可能运气好，能找到最小值，但是有可能运气不好，找到的是局部极小值。
2、性质较好的问题
当然也有性质比较好的问题，比如这样的函数：
在这样的约束空间下，只有极值点，且为极小值点，并且约束空间是一块完整的空间，不破碎。
或者是这样的问题：
没有极值点，但在整个约束空间中是单调的，而且约束空间也是完整的、不破碎的空间。
七、凸优化问题的定义与性质
这种问题的迭代法对初值就不敏感，取什么初值都能迭代到最小值，而且迭代法收敛的解必然是全局最小，即在约束条件下的全局最小值。
这种性质比较好的问题，叫做 凸优化。
1、凸优化问题的性质
凸优化必然有两个性质：
- 凸函数 ：代价函数只有单个极值点，且为极小值
这样表述其实不严谨，因为代价函数可能没有极值点，为单调函数。 - 凸空间：约束空间是一块完整的、不破碎的空间
求凸函数在凸空间的最小值问题被称为凸优化问题。
2、凸空间的定义
为什么完整的空间叫凸空间呢？
凸空间的严谨定义由凸多边形衍生而来，凸多边形和凹多边形如下：
凸多边形 ：对于多边形内部任意的点 x 、 y x、y x、y，都有 x + y 2 \frac{x+y}{2} 2x+y 表示的点也在多边形内部。
凹多边形 ：存在两个点 x 、 y x、 y x、y，使得 x + y 2 \frac{x+y}{2} 2x+y 表示的点不在多边形内部。
凸空间 ：完整的空间，比如下图：
区间内任选两点 x 、 y x、y x、y，点 x + y 2 \frac{x+y}{2} 2x+y 也在区间里。
非凸空间 ：破碎的空间，比如下图：
区间内存在两点 x 、 y x 、y x、y，中点 x + y 2 \frac{x+y}{2} 2x+y 不在区间内。
注意：多边形有凸多边形和凹多边形，但空间只有凸空间和非凸空间，没有凹空间，这不是专门的学术名词，只有凸空间和非凸空间，因为凸和非凸数学上已经远远超过几何意义了，是更抽象的东西。
八、凸优化与自动驾驶规划算法的关系
随之而来的两个问题：
- 为什么要讲凸优化？
- 凸优化和自动驾驶规划算法有什么联系？
1、凸优化的重要性
为什么讲凸优化呢？
因为凸优化是最简单的非线性规划方法，也是人类唯一掌握的，唯一的研究的比较明白、比较彻底的一种非线性优化方法。凸优化是所有复杂优化方法的基石，很多非凸问题都是想办法去转化成凸问题，然后求凸优化的解。
2、凸优化与自动驾驶规划的关系
自动驾驶和凸优化有什么关系？
自动化驾驶的规划的求解是凸优化问题。
3、凸优化的基本因素
凸优化有两个因素：
- 代价函数
- 约束空间
4、自动驾驶中的避障约束空间
考虑问题：自动驾驶中的避障约束空间是不是凸空间？
显而易见，不是凸空间。
举例如下：
避障的约束空间是红色区域，除了树附近不能去之外，其他地方都可以去，那么这块空间很显然是一块破碎的空间，因为中间有个洞，作为车来说，可以从上面绕过，也可以从下面绕过，但是上面和下面加起来除以 2 2 2 这条绿色线，就不可以，上面这条线满足避障约束，下面也满足避障约束，但是和上向下加起来除以 2 2 2 就不满足避障约束，所以不是凸空间。
对于动态障碍物也是一样的。
比如下图车辆规避横穿马路的行人场景，如果规划车速较快，人到道路上时车已经远远超过人，满足避障约束。
如果规划车速较慢，车还没到人的位置，也是满足避障约束。
如果车速快 + + + 车速慢除以 2 2 2，人和车就贴在一起，发生交通事故。
上面避让树是静态避障的案例，下面避让人是动态避障的案例。
自动驾驶规划中静态和动态避障的约束空间都不是凸空间，所以规划算法相对比较复杂、比较难做。因为解空间或约束空间是非凸的，当然难点远不止于空间是非凸的，还有其他难点，后面再说。
显然，虽然不知道代价函数是不是凸函数，但是至少解空间和约束空间肯定不是凸空间，所以肯定是非凸问题。
5、凸优化与非凸优化的对比
对于凸问题，只要给初值，然后迭代就可求解，迭代的最后迭代收敛的值一定有全局最小值。这是凸问题的性质，是好性质。
对于非凸问题，可能存在多个极值点，或者空间破碎，结论非常依赖于初值，初值选的不好就会收敛到局部极小值。
如何求解非凸问题的最小值？
可以很遗憾地说，目前为止没有完美解决所有的非凸问题的算法，只是针对不同情况有不同的考虑。
九、非凸优化问题的求解思路与策略
1、非凸问题求解思路
求解非凸问题的主要思路就是找非凸问题中的凸结构。
比如这样的问题：
函数是凸的，但约束空间不是凸空间，要怎样求解这样的问题？
主流方法是启发式算法，首先随机在约束空间里采样一些离散函数值，比大小，然后取值最小的作为迭代初值。
2、函数与约束空间非凸的情况
对于非凸函数以及非凸空间的最小值问题，方法也差不多，比如这样的函数：
先离散地在约束空间中采样取点，比较大小，从中选最小的作为初值迭代。用梯度下降法、高斯牛顿法这类方法迭代，先在约束空间里采样，然后再找到采样点的最小值。本质上是连续空间离散后，在离散约束空间的最优解。
3、非凸问题的求解流程
举个例子，在连续的约束空间中采样撒点，想要在哪个点选最小值，本质上是把连续的约束空间转换为一个个离散点：
即最优解只能在这些点中寻找，采样比大小，然后求出最小值，最小值本质上是连续约束空间离散化后，在离散约束空间的最优解，是粗略的解。
对于非凸空间，求解过程如下：
非凸空间 离散化 粗解 迭代 最终解
首先离散化，然后再采样得到粗解，再对粗解进行迭代，得到最终解，粗解起到基点的作用，以粗解为基础，然后得到最终解。
求解思想：先离散地采样撒点，看一下最小值最有可能的位置，然后再根据最有可能位置作为初值迭代，从而找到最优解。
4、非凸问题求解的局限性
当然方法也不是尽善尽美，比如这样的非凸问题：
如果采样点较少，像这样采样点并没有落在最优解所在的约束空间，就容易陷入局部极小值。所以采样点越少就越容易收敛到局部最优，而不是整体得最值，但采样多容易发生维度灾难。
十、维度灾难
-
一维空间中，在前后两个方向上采样：
-
二维空间中，在前后左右方向上采样：
-
三维空间中，在前后左右上下方向上采样。
如果一维空间采样 100 100 100 个点，那么二维空间可能要采样 1 1 1 万个点，三维的话就要还要乘 100 100 100，就是采 100 100 100 万个点，所以非凸优化没有尽善尽美的解决方案，只能根据不同问题调整。
十一、总结
本篇博客主要从宏观层面介绍了凸优化在自动驾驶规划中的应用，讲解了代价函数构建、约束条件处理、迭代法求解以及非凸优化问题的处理策略。
下一节将介绍自然坐标系和直角坐标系之间的转化关系及相关推导。
参考资料
后记：
🌟 感谢您耐心阅读这篇关于 凸优化与非凸优化 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀