写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_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 万个点,所以非凸优化没有尽善尽美的解决方案,只能根据不同问题调整。
十一、总结
本篇博客主要从宏观层面介绍了凸优化在自动驾驶规划中的应用,讲解了代价函数构建、约束条件处理、迭代法求解以及非凸优化问题的处理策略。
下一节将介绍自然坐标系和直角坐标系之间的转化关系及相关推导。
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 凸优化与非凸优化 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀