写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。
👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!
📚 系列专栏:【决策规划】系列,带您深入浅出,领略自动驾驶决策规划的魅力。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、自动驾驶决策规划算法概述
- 二、参考线
- 2.1 参考线的作用
- 2.2 过长路径的缺点
- (1) 匹配点难以确定
- (2) 障碍物投影不唯一
- (3) 导航路径不平滑
- 2.3 生成参考线的方法
- 2.4 参考线的优点
- 三、参考线平滑算法
- 3.1 平滑算法的代价函数
- 3.2 转化为二次规划问题
- (1) 平滑代价
- (2) 紧凑代价
- (3) 几何相似代价
- 3.3 约束问题
- (1) 距离约束
- (2) 曲率约束
- 四、算法加速方法
- 4.1 降低执行频率
- 4.2 轨迹拼接
- 五、参考线平滑算法难点
- 5.1 快速找到车在全局路径下的投影点
- 5.2 执行频率的调度问题
- 六、总结
- 参考资料
引言
各位小伙伴们大家好,欢迎收看自动驾驶决策规划算法第二节,内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。
一、自动驾驶决策规划算法概述
本篇博客所讲的内容是 参考线(Reference Line),在讲参考线之前,先讲一下决策规划的总体流程。假设已经有了导航路径,决策规划流程如下:
- S t e p 1 Step1 Step1 定位 + 导航:生成参考线
- S t e p 2 Step2 Step2 障碍物投影:静态障物投影到以参考线为坐标轴的 Frenet 坐标系上
- S t e p 3 Step3 Step3 开辟凸空间:决策算法对障碍物做决策(往左绕、往右绕、忽略)开辟凸空间
- S t e p 4 Step4 Step4 搜索最优路径:规划算法在决策算法所开辟的凸空间内搜索出一条最优路径
- S t e p 5 Step5 Step5 后处理:在规划中轨迹中选点坐标转化成笛卡尔坐标系,再输出给控制去跟踪。
比如在自然坐标系下有如下障碍物:
在第三步中,如果决策算法选择了往左绕,就意味着决策算法开辟了上图紫色的凸空间;如果决策算法决定往右绕,开辟的是蓝色的凸空间,也就是最终规划轨迹要么在紫色的凸空间中搜索,要么在蓝色凸空间中搜索,到底在哪里搜索是决策算法所干的事情。
决策算法开辟最优凸空间,在凸空间内搜索轨迹,把轨迹交给控制执行。
有人可能会觉得“开辟”动词不是特别准确,这更像是一种选择,比如有很多凸空间决策算法是选择最优的凸空间,那为什么要叫开辟?
其实当学完之后才发现开辟动词还是相对来说最恰当。
在第四步中,比如决策算法已经决策出来了,在蓝色的凸空间上搜索,规划就是在蓝色的凸空间上搜索出一条最优路径。
这就是整个决策规划的具体步骤,相对来说比较粗略,而且没有考虑动态障碍物。因为暂时先不管动态障碍,先把静态障碍物做出来,凡事都由简到难、由简单到复杂。
二、参考线
本篇博客就是讲怎么通过定位加导航的路径生成参考线。
2.1 参考线的作用
首先要讲一下参考线是干什么的,参考线是一种解决方案:解决导航路径过长且不平滑的问题。
比如如果导航计算出来的路径是非常长的路径,过长的路径不利于坐标转换,比如下图:
在本系列博客中,数学基础部分第三节的坐标转换,核心步骤就是找匹配点,具体参见:
【自动驾驶】决策规划算法 | 数学基础(三)直角坐标与自然坐标转换Ⅰ
【自动驾驶】决策规划算法 | 数学基础(三)直角坐标与自然坐标转换Ⅱ
2.2 过长路径的缺点
(1) 匹配点难以确定
路径越长,找匹配点就越麻烦,因为找匹配点需要遍历,路径越长点就越多,越多找匹配点就越慢。
- 如果是只是自车的坐标转换,那还好办,因为只需要做一次。
- 如果是障碍物也要做坐标转换,每个障碍物都要做投影,计算量就非常大。
(2) 障碍物投影不唯一
比如在上图路径中的绿色障碍物,投影可能有两个,俩距离都相等,到底哪个才是它的投影?这就是个非常麻烦的问题。
(3) 导航路径不平滑
导航路径一般为平滑曲线,上图中的蓝色点导数都不连续、不平滑。
所以解决方案就是参考线,就是在全局路径中截取一小段较短的路径,进行平滑,平滑后的曲线即为参考线,将参考线作为障碍物投影的坐标轴线。
2.3 生成参考线的方法
比如下图是不平滑的导航路径:
先找到匹配点以及投影点。每个规划周期内,首先找投影点,以投影点为坐标原点,往后取 30 m 30m 30m,往前取 150 m 150m 150m,取这些范围内的点。
图中橙色线就是 150 m 150m 150m ,弧长是 150 m 150m 150m ,后面取紫红色线 30 m 30m 30m ,就把范围内的点全部拿出来做平滑,上图中蓝色点为未平滑点,平滑后变成黑色点,平滑后的点的集合称为参考线。
2.4 参考线的优点
参考线能解决上面所说的导航路径过长的问题,因为较短的参考线投影比较好找,而且短的话一般曲率也不会太大,投影就是唯一的,而且做过平滑,比全局路径更平滑,这就是参考线作用。
三、参考线平滑算法
如何平滑参考线呢?
参考线平滑算法不唯一,这里只讲其中一种平滑算法,认为点与点之间越接近直线就越平滑,越不接近直线就越不平滑。
以三个点做向量,做如图所示的向量, P 0 , P 1 , P 2 , P 3 P_0,P_1,P_2,P_3 P0,P1,P2,P3。 ∣ P 1 P 3 → ∣ |\overrightarrow{P_1P_3}| ∣P1P3∣ 向量的长度就是衡量平滑与不平滑的标准。如果 ∣ P 1 P 3 → ∣ |\overrightarrow{P_1P_3}| ∣P1P3∣ 越小,就证明越平滑,也就越接近直线。
但参考线不是越平滑越好,比如下面黑色的三个点是原来全局路径的点,如果越平滑越好,可能会优化成像紫色的这样一条线。
虽然平滑了,但几何形状和原来的路径点差距实在是太大,这样也不好。
3.1 平滑算法的代价函数
如何衡量和几何形状相关的标准?
用图中三段绿色线衡量。记原来的路径点为 P 1 r , P 2 r , P 3 r P_{1r},P_{2r},P_{3r} P1r,P2r,P3r,优化后的点记为 P 1 、 P 2 、 P 3 P_1、P_2、P_3 P1、P2、P3。衡量与几何相似度的标准就是,如果这三条绿线的长度 ∣ P 1 P 1 r ∣ + ∣ P 2 P 2 r ∣ + ∣ P 3 P 3 r ∣ |P_1P_{1r}|+|P_2P_{2r}|+|P_3P_{3r}| ∣P1P1r∣+∣P2P2r∣+∣P3P3r∣ 加起来越少,就意味着越接近原路径几何。
除了平滑性因素,还有道路几何因素,以及长度要尽可能均匀和紧凑的因素。
比如这是三个原来的黑色点:
希望优化点变成紫色线。但如果像绿色线过于分散也不好,所以长度要尽可能均匀、紧凑。
那怎么衡量呢?
如果
∣
P
1
P
2
∣
=
a
∣
P
2
P
3
∣
=
a
|P_1P_2|=a\quad |P_2P_3|=a
∣P1P2∣=a∣P2P3∣=a 认为比较紧凑
反之,如果
∣
P
1
P
2
∣
=
a
+
b
∣
P
2
P
3
∣
=
a
−
b
|P_1P_2|=a+b\quad |P_2P_3|=a-b
∣P1P2∣=a+b∣P2P3∣=a−b 认为比较分散
不妨把两边的长度平方和算一下,前面紧凑的等于 2 a 2 2a^2 2a2,后面比较分散的是 ( a + b ) 2 + ( a − b ) 2 = 2 a 2 + 2 b 2 (a+b)^2+(a-b)^2=2a^2+2b^2 (a+b)2+(a−b)2=2a2+2b2,这样就能看出来,用 ∣ P 1 P 2 ∣ 2 + ∣ P 2 P 3 ∣ 2 |P_1P_2|^2+|P_2P_3|^2 ∣P1P2∣2+∣P2P3∣2 来衡量,即 ∣ P 1 P 2 ∣ 2 + ∣ P 2 P 3 ∣ 2 |P_1P_2|^2+|P_2P_3|^2 ∣P1P2∣2+∣P2P3∣2 越小,意味着越均匀、越紧凑。
综上就可以写出平滑算法的代价函数,比如下图:
原来的点为 P 1 r , P 2 r , P 3 r P_{1r},P_{2r},P_{3r} P1r,P2r,P3r,优化后的点为 P 1 、 P 2 、 P 3 P_1、P_2、P_3 P1、P2、P3。
原来路径点坐标记为 ( x 1 r , y 1 r ) 、 ( x 2 r , y 2 r ) 、 ( x 3 r , y 3 r ) \left( x_{1r},y_{1r} \right) \text{、}\left( x_{2r},y_{2r} \right) \text{、}\left( x_{3r},y_{3r} \right) (x1r,y1r)、(x2r,y2r)、(x3r,y3r)
优化后路径坐标记为 ( x 1 , y 1 ) 、 ( x 2 , y 2 ) 、 ( x 3 , y 3 ) \left( x_1,y_1 \right) \text{、}\left( x_2,y_2 \right) \text{、}\left( x_3,y_3 \right) (x1,y1)、(x2,y2)、(x3,y3)
其中, x 1 r , x 2 r , x 3 r , y 1 r , y 2 r , y 3 r x_{1r},x_{2r},x_{3r},y_{1r},y_{2r},y_{3r} x1r,x2r,x3r,y1r,y2r,y3r 已知, x 1 , x 2 , x 3 , y 1 , y 2 , y 3 x_1,x_2,x_3,y_1,y_2,y_3 x1,x2,x3,y1,y2,y3 未知。
代价函数为
J
=
w
1
{
∑
i
=
1
3
(
x
i
−
x
i
r
)
2
+
(
y
i
−
y
i
r
)
2
}
+
w
2
[
(
x
1
+
x
3
−
2
x
2
)
2
+
(
y
1
+
y
3
−
2
y
2
)
2
]
+
w
3
{
∑
i
=
1
2
(
x
i
+
1
−
x
i
)
2
+
(
y
i
+
1
−
y
i
)
2
}
\begin{aligned} J&=w_1\{\sum_{i=1}^3{\left( x_i-x_{ir} \right) ^2}+\left( y_i-y_{ir} \right) ^2\}\\ &+w_2\left[ \left( x_1+x_3-2x_2 \right) ^2+\left( y_1+y_3-2y_2 \right) ^2 \right]\\ &+w_3\{\sum_{i=1}^2{\left( x_{i+1}-x_i \right) ^2}+\left( y_{i+1}-y_i \right) ^2\} \end{aligned}
J=w1{i=1∑3(xi−xir)2+(yi−yir)2}+w2[(x1+x3−2x2)2+(y1+y3−2y2)2]+w3{i=1∑2(xi+1−xi)2+(yi+1−yi)2} 包含三个代价,第一项为与原路径点相似代价,第二项为平滑代价,第三项为紧凑代价。
w
1
、
w
2
、
w
3
w_1、w_2、 w_3
w1、w2、w3 是相应的权重。
希望代价函数越小越好,因为越小就与原路径点越相似、越平滑、越紧凑。
我们的任务就是选取合适的 ( x 1 , y 1 ) 、 ( x 2 , y 2 ) 、 ( x 3 , y 3 ) \left( x_1,y_1 \right) \text{、}\left( x_2,y_2 \right) \text{、}\left( x_3,y_3 \right) (x1,y1)、(x2,y2)、(x3,y3),使得代价函数最小。
3.2 转化为二次规划问题
这是典型的二次规划问题,二次规划问题的典型描述为:
1
2
x
T
H
x
+
f
T
x
=
min
s.t
A
x
≤
b
l
b
≤
x
≤
u
b
\begin{array}{c} \frac{1}{2}x^THx+f^Tx=\min\\ \text{s.t\quad\quad }Ax\leq b\\ \quad \quad lb\leq x\leq ub\\ \end{array}
21xTHx+fTx=mins.tAx≤blb≤x≤ub 接下来看一下上面的代价函数如何写成二次规划的形式。
(1) 平滑代价
首先写一下平滑代价的表达式,平方和可以写成向量乘以向量的转置。
J
s
m
o
o
t
h
=
(
x
1
+
x
3
−
2
x
2
)
2
+
(
y
1
+
y
3
−
2
y
2
)
2
=
(
x
1
+
x
3
−
2
x
2
,
y
1
+
y
3
−
2
y
2
)
(
x
1
+
x
3
−
2
x
2
,
y
1
+
y
3
−
2
y
2
)
T
\begin{aligned} J_{smooth}=&\left( x_1+x_3-2x_2 \right) ^2+\left( y_1+y_3-2y_2 \right) ^2\\ =&\left( x_1+x_3-2x_2,y_1+y_3-2y_2 \right) \left( x_1+x_3-2x_2,y_1+y_3-2y_2 \right) ^T\\ \end{aligned}
Jsmooth==(x1+x3−2x2)2+(y1+y3−2y2)2(x1+x3−2x2,y1+y3−2y2)(x1+x3−2x2,y1+y3−2y2)T 前面向量可以写成
J
s
m
o
o
t
h
=
(
x
1
+
x
3
−
2
x
2
,
y
1
+
y
3
−
2
y
2
)
=
(
x
1
,
y
1
,
x
2
,
y
2
,
x
3
,
y
3
)
(
1
0
0
1
−
2
0
0
−
2
1
0
0
1
)
\begin{aligned} J_{smooth}&=\left( x_1+x_3-2x_2,y_1+y_3-2y_2 \right) \\ &=\left( x_1,y_1,x_2,y_2,x_3,y_3 \right) \left( \begin{matrix} 1& 0\\ 0& 1\\ -2& 0\\ 0& -2\\ 1& 0\\ 0& 1\\ \end{matrix} \right) \end{aligned}
Jsmooth=(x1+x3−2x2,y1+y3−2y2)=(x1,y1,x2,y2,x3,y3)
10−2010010−201
记
x
=
(
x
1
y
1
y
2
⋮
)
A
1
=
(
1
0
−
2
0
1
0
0
1
0
−
2
0
1
)
x=\begin{pmatrix}x_1\\y_1\\y_2\\\varvdots\end{pmatrix}\quad A_1=\begin{pmatrix}1&0&-2&0&1&0\\0&1&0&-2&0&1\end{pmatrix}
x=
x1y1y2⋮
A1=(1001−200−21001) 则平滑代价函数为
J
s
m
o
o
t
h
=
(
x
1
+
x
3
−
2
x
2
)
2
+
(
y
1
+
y
3
−
2
y
2
)
2
=
x
T
A
1
T
A
1
x
J_{smooth}=(x_1+x_3-2x_2)^2+(y_1+y_3-2y_2)^2=x^TA_1^TA_1x
Jsmooth=(x1+x3−2x2)2+(y1+y3−2y2)2=xTA1TA1x 这只是
3
3
3 个点的情况。
如果是
n
n
n 个点的情况,先把平滑代价函数写出来
J
s
m
o
o
t
h
=
∑
i
=
1
n
−
2
(
x
i
+
x
i
+
2
−
2
x
i
+
1
)
2
+
(
y
i
+
y
i
+
2
−
2
y
i
+
1
)
2
J_{smooth}=\sum_{i=1}^{n-2}{\left( x_i+x_{i+2}-2x_{i+1} \right) ^2+\left( y_i+y_{i+2}-2y_{i+1} \right) ^2}
Jsmooth=i=1∑n−2(xi+xi+2−2xi+1)2+(yi+yi+2−2yi+1)2 一共是
(
2
n
−
4
)
(2n-4)
(2n−4) 项。
展开写成向量的形式:
J
s
m
o
o
t
h
=
(
x
1
+
x
3
−
2
x
2
,
y
1
+
y
3
−
2
y
2
,
x
2
+
x
4
−
2
x
3
,
y
2
+
y
4
−
2
y
3
,
…
)
(
x
1
+
x
3
−
2
x
2
,
y
1
+
y
3
−
2
y
2
,
x
2
+
x
4
−
2
x
3
,
y
2
+
y
4
−
2
y
3
,
…
)
T
J_{smooth}=(x_{1}+x_{3}-2x_{2},y_{1}+y_{3}-2y_{2},x_{2}+x_{4}-2x_{3},y_{2}+y_{4}-2y_{3},\ldots)\\(x_{1}+x_{3}-2x_{2},y_{1}+y_{3}-2y_{2},x_{2}+x_{4}-2x_{3},y_{2}+y_{4}-2y_{3},\ldots)^{T}
Jsmooth=(x1+x3−2x2,y1+y3−2y2,x2+x4−2x3,y2+y4−2y3,…)(x1+x3−2x2,y1+y3−2y2,x2+x4−2x3,y2+y4−2y3,…)T 写成矩阵的形式:
J
s
m
o
o
t
h
=
(
x
1
,
y
1
,
⋯
)
(
1
0
0
1
−
2
0
1
0
0
−
2
0
1
1
0
−
2
0
1
0
1
0
−
2
0
⋮
1
0
−
2
0
0
1
0
−
2
1
0
⋱
0
1
⋱
)
J_{smooth}= \left( x_1,y_1,\cdots \right) \left. \left( \begin{matrix} 1& 0& & & & & & \\ 0& 1& & & & & & \\ -2& 0& 1& 0& & & & \\ 0& -2& 0& 1& & & & \\ 1& 0& -2& 0& 1& & & \\ 0& 1& 0& -2& 0& \vdots& & \\ & & 1& 0& -2& 0& & \\ & & 0& 1& 0& -2& & \\ & & & & 1& 0& \ddots& \\ & & & & 0& 1& & \ddots \\ & & & & & & & & & \\ \end{matrix} \right. \right)
Jsmooth=(x1,y1,⋯)
10−2010010−20110−2010010−20110−2010⋮0−201⋱⋱
大括号内的矩阵是将
A
1
T
A_1^T
A1T按照对角线不断复制,每隔两行、每隔两列不断复制。其中,
(
x
1
,
y
1
,
⋯
)
\left( x_1,y_1,\cdots \right)
(x1,y1,⋯) 为
1
×
2
n
1\times 2n
1×2n 的矩阵,大括号矩阵为
2
n
×
(
2
n
−
4
)
2n\times (2n-4)
2n×(2n−4) 的矩阵。
将大括号矩阵记为
A
1
T
A^T_1
A1T,则平滑代价可以写成:
J
s
m
o
o
t
h
=
w
smooth_cost
⋅
x
T
A
1
T
A
1
x
J_{smooth}= w_{\text{smooth\_cos}\text{t}}\cdot x^{\text{T}}A_{1}^{\text{T}}A_1x
Jsmooth=wsmooth_cost⋅xTA1TA1x 因为
A
1
T
A^T_1
A1T 是
(
2
n
,
2
n
−
4
)
(2n,2n-4)
(2n,2n−4) 的矩阵,则
A
1
A_1
A1 就是
(
2
n
−
4
,
2
n
)
(2n-4,2n)
(2n−4,2n) 的矩阵。
(2) 紧凑代价
紧凑代价直接写
n
n
n 个点的情况,代价函数为:
J
l
e
n
g
t
h
=
∑
i
=
1
n
−
1
(
x
i
−
x
i
+
1
)
2
+
(
y
i
−
y
i
+
1
)
2
J_{length}=\sum_{i=1}^{n-1}{\left( x_i-x_{i+1} \right) ^2+\left( y_i-y_{i+1} \right) ^2}
Jlength=i=1∑n−1(xi−xi+1)2+(yi−yi+1)2改写成向量形式:
J
l
e
n
g
t
h
=
(
x
1
−
x
2
,
y
1
−
y
2
,
x
2
−
x
3
,
y
2
−
y
3
…
)
(
x
1
−
x
2
,
y
1
−
y
2
,
x
2
−
x
3
,
y
2
−
y
3
…
)
T
J_{length}=(x_1-x_2,y_1-y_2,x_2-x_3,y_2-y_3\ldots)\\(x_1-x_2,y_1-y_2,x_2-x_3,y_2-y_3\ldots)^T
Jlength=(x1−x2,y1−y2,x2−x3,y2−y3…)(x1−x2,y1−y2,x2−x3,y2−y3…)T写成矩阵的形式:
J
l
e
n
g
t
h
=
(
x
1
,
y
1
,
⋯
)
(
1
0
0
1
−
1
0
1
0
0
−
1
0
1
−
1
0
1
0
−
1
0
⋮
−
1
0
0
−
1
⋱
⋱
)
J_{length}= \left( x_1,y_1,\cdots \right) \left. \left( \begin{matrix} 1& 0& & & & & & \\ 0& 1& & & & & & \\ -1& 0& 1& 0& & & & \\ 0& -1& 0& 1& & & & \\ & & -1& 0& 1& & & \\ & & 0& -1& 0& \vdots& & \\ & & & & -1& 0& & \\ & & & & 0& -1& & \\ & & & & & & \ddots& \\ & & & & & & & \ddots \\ & & & & & & & & & \\ \end{matrix} \right. \right)
Jlength=(x1,y1,⋯)
10−10010−110−10010−110−10⋮0−1⋱⋱
其中,
(
x
1
,
y
1
,
⋯
)
\left( x_1,y_1,\cdots \right)
(x1,y1,⋯) 为
1
×
2
n
1\times 2n
1×2n 的矩阵,大括号矩阵为
2
n
×
(
2
n
−
2
)
2n\times (2n-2)
2n×(2n−2) 的矩阵。
将大括号矩阵记为
A
2
T
A^T_2
A2T,则紧凑代价可以写成:
J
l
e
n
g
t
h
=
w
length_cost
⋅
x
T
A
2
T
A
2
x
J_{length}=w_{\text{length\_cos}\text{t}}\cdot x^{\text{T}}A_{2}^{\text{T}}A_2x
Jlength=wlength_cost⋅xTA2TA2x 因为
A
2
T
A^T_2
A2T 是
(
2
n
,
2
n
−
2
)
(2n,2n-2)
(2n,2n−2) 的矩阵,则
A
2
A_2
A2 就是
(
2
n
−
2
,
2
n
)
(2n-2,2n)
(2n−2,2n) 的矩阵。
(3) 几何相似代价
代价函数为:
J
r
e
f
=
∑
i
=
1
n
(
x
i
−
x
i
r
)
2
+
(
y
i
−
y
i
r
)
2
=
∑
i
=
1
n
(
x
i
2
+
y
i
2
)
+
∑
i
=
1
n
(
−
2
x
i
r
x
i
−
2
y
i
r
y
i
)
+
∑
i
=
1
n
(
x
i
r
2
+
y
i
r
2
)
\begin{aligned} J_{ref}&=\sum_{i=1}^n{\left( x_i-x_{ir} \right) ^2}+\left( y_i-y_{ir} \right) ^2\\ &=\sum_{i=1}^n{\left( x_{i}^{2}+y_{i}^{2} \right)}+\sum_{i=1}^n{\left( -2x_{ir}x_i-2y_{ir}y_i \right)}+\sum_{i=1}^n{\left( x_{ir}^{2}+y_{ir}^{2} \right)}\\ \end{aligned}
Jref=i=1∑n(xi−xir)2+(yi−yir)2=i=1∑n(xi2+yi2)+i=1∑n(−2xirxi−2yiryi)+i=1∑n(xir2+yir2) 因为
x
i
r
,
y
i
r
x_{ir},y_{ir}
xir,yir 是常数或已知量,所以
∑
i
=
1
n
(
x
i
r
2
+
y
i
r
2
)
\sum_{i=1}^n{\left( x_{ir}^{2}+y_{ir}^{2} \right)}
∑i=1n(xir2+yir2) 是常数,不受
x
i
,
y
i
x_{i},y_{i}
xi,yi 的影响,所以最后一项可以去掉,即
J
r
e
f
=
∑
i
=
1
n
(
x
i
2
+
y
i
2
)
+
∑
i
=
1
n
(
−
2
x
i
r
x
i
−
2
y
i
r
y
i
)
J_{ref}=\sum_{i=1}^n{\left( x_{i}^{2}+y_{i}^{2} \right)}+\sum_{i=1}^n{\left( -2x_{ir}x_i-2y_{ir}y_i \right)}
Jref=i=1∑n(xi2+yi2)+i=1∑n(−2xirxi−2yiryi)写成二次规划的形式:
J
r
e
f
=
(
x
1
,
y
1
,
.
.
.
)
(
1
1
1
⋱
)
(
x
1
y
1
⋮
⋮
)
+
(
−
2
)
(
x
1
,
y
1
,
.
.
.
.
.
.
)
(
x
1
y
1
⋮
x
n
y
n
)
J_{ref}=\left( x_1,y_1,... \right) \left( \begin{matrix} 1& & & & \\ & 1& & & \\ & & 1& & \\ & & & \ddots& \\ \end{matrix} \right) \left( \begin{array}{c} x_1\\ y_1\\ \vdots\\ \vdots\\ \end{array} \right) +\left( -2 \right) \left( x_1,y_1,...... \right) \left( \begin{array}{c} x_1\\ y_1\\ \vdots\\ x_n\\ y_n\\ \end{array} \right)
Jref=(x1,y1,...)
111⋱
x1y1⋮⋮
+(−2)(x1,y1,......)
x1y1⋮xnyn
其中,大括号矩阵为单位矩阵,记为
A
3
T
A^T_3
A3T,为
(
2
n
×
2
n
)
(2n\times2n)
(2n×2n)的矩阵。
写成二次规划形式:
J
r
e
f
=
w
r
e
f
_
c
o
s
t
⋅
(
x
T
A
3
T
A
3
x
+
h
T
x
)
J_{ref}=w_{ref\_cost}\cdot \left( x^{\text{T}}A_{3}^{\text{T}}A_3x+h^{\text{T}}x \right)
Jref=wref_cost⋅(xTA3TA3x+hTx)其中,
h
=
(
−
2
x
1
r
−
2
y
1
r
⋮
−
2
x
n
r
−
2
y
n
r
)
h=\left( \begin{array}{c} -2x_{1r}\\ -2y_{1r}\\ \vdots\\ -2x_{nr}\\ -2y_{nr}\\ \end{array} \right)
h=
−2x1r−2y1r⋮−2xnr−2ynr
综上所述,把这三个代价写成二次规划的形式,统一起来:
J
=
x
T
(
w
s
o
m
m
t
h
⋅
A
1
T
A
1
+
w
l
e
n
g
t
h
⋅
A
2
T
A
2
+
w
r
e
f
⋅
A
3
T
A
3
)
x
+
w
r
e
f
⋅
h
T
x
J=x^T\left( w_{sommth}\cdot A_{1}^{T}A_1+w_{length}\cdot A_{2}^{T}A_2+w_{ref}\cdot A_{3}^{T}A_3 \right) x+w_{ref}\cdot h^Tx
J=xT(wsommth⋅A1TA1+wlength⋅A2TA2+wref⋅A3TA3)x+wref⋅hTx二次规划的标准形式:
1
2
x
T
H
x
+
f
T
x
=
x
T
(
H
2
)
x
+
f
T
x
\frac{1}{2}x^THx+f^Tx=x^T\left( \frac{H}{2} \right) x+f^Tx
21xTHx+fTx=xT(2H)x+fTx则
H
=
2
(
w
s
o
m
m
t
h
⋅
A
1
T
A
1
+
w
l
e
n
g
t
n
A
2
T
A
2
+
w
r
e
f
A
3
T
A
3
)
f
T
=
w
r
e
f
⋅
h
T
\begin{aligned} H&=2\left( w_{sommth}\cdot A_{1}^{T}A_1+w_{lengtn}A_{2}^{T}A_2+w_{ref}A_{3}^{T}A_3 \right)\\ f^T&=w_{ref}\cdot h^T \end{aligned}
HfT=2(wsommth⋅A1TA1+wlengtnA2TA2+wrefA3TA3)=wref⋅hT 至此,如何把优化问题转化为二次规划问题,以及怎么转化就讲解完毕。
3.3 约束问题
接下来讲约束问题,就是怎么求二次规划的约束。
(1) 距离约束
记待优化的路径点 x = ( x 1 , y 1 , ⋯ , x n , y n ) T x=\left( x_1,y_1,\cdots ,x_n,y_n \right) ^T x=(x1,y1,⋯,xn,yn)T
原始路径点 x r e f = ( x 1 r , y 1 r , ⋯ , x n r , y n r ) x_{ref}=\left( x_{1r},y_{1r},\cdots ,x_{nr},y_{nr} \right) xref=(x1r,y1r,⋯,xnr,ynr)
约束就是不希望 x x x 与 x r e f x_{ref} xref 相距太远
有人可能觉得有代价函数保证了,就是不有几何形状代价,但可能参数调的不对,更倾向于平滑和紧凑,而几何形状的权重不高,就有可能比较散,所以说有必要再加约束。
所以约束就是
x
x
x 和
x
r
e
f
x_{ref}
xref 之间的距离要小于值 buff
,值可以自己确定:
∣
x
−
x
r
e
f
∣
≤
buff
|x-x_{ref}|\leq \text{buff}
∣x−xref∣≤buff展开
x
r
e
f
−
buff
≤
x
≤
x
r
e
f
+
buff
x_{ref}-\text{buff}\leq x\leq x_{ref}+\text{buff}
xref−buff≤x≤xref+buff 可取值 buff = 0.1
,也可以自己标定,觉得
0.1
0.1
0.1 不合适,可以放大或缩小一点。
(2) 曲率约束
约束一般只需要 x x x 和 x r e f x_{ref} xref 之间不要差太远即可。但也有教程会讲曲率约束。在参考线平滑算法里,曲率约束一般不需要,因为曲率约束本身是非线性约束,要加上去比较麻烦,处理起来也很麻烦。
而且曲率约束的与车的最大侧向加速度有关,可以放到后面再考虑。
为什么要对曲率做约束?
因为有些弯可能太急了过不去,车有最大侧向加速度的限制,如果侧向加速度特别大,车可能会翻。但侧向加速度本身既和曲率有关,也和速度有关,所以最大曲率限制没有必要在一开始时就在参考线平滑中考虑,可以在后面速度规划时再考虑相关的曲率。
所以在这里就先介绍一下曲率到底该怎么计算,看一下为什么曲率约束是非线性,至于曲率约束,在参考线平滑算法暂时不考虑。如果不放心想约束的话,更推荐把 平滑算法目标函数中,关于平滑代价函数权重增大一点,曲率自然会变小。
曲率的计算
下面看一下曲率该怎么算。
首先声明一下曲率的计算,是近似的算法,不是精确算法,不过近似程度也够了。
比如这里有三个点,三点确定圆:
近似认为上图中两段 d s ds ds 相等,即 ∣ P 1 P 2 → ∣ = ∣ P 2 P 3 → ∣ |\overrightarrow{P_1P_2}|=|\overrightarrow{P_2P_3}| ∣P1P2∣=∣P2P3∣,向量求和 P 2 P 1 → + P 2 P 3 → \overrightarrow{P_2P_1}+\overrightarrow{P_2P_3} P2P1+P2P3 遵循平行四边形定则,所以蓝色图形肯定是平行四边形,但又因为 d s ds ds 相等,所以两个边相等的平行四边形是菱形。既然是菱形,绿色三角形自然就是等腰三角形。
同理浅粉红色三角形,因为两边的都是
R
R
R,也是等腰三角形。橙黄色角就是这两个等腰三角形之间的公共角。有两个等腰三角形其中有角是公共角,意味着两个三角形相似,则
l
d
s
=
d
s
R
\frac l{ds}=\frac{ds}R
dsl=Rds则曲率为
κ
=
1
R
=
l
d
s
2
\kappa =\frac{1}{R}=\frac{l}{ds^2}
κ=R1=ds2l
l
=
∣
P
2
P
1
→
+
P
2
P
3
→
∣
l=|\overrightarrow{P_2P_1}+\overrightarrow{P_2P_3}|
l=∣P2P1+P2P3∣ 可见
l
l
l 是非线性,因为算向量的模的话,向量得先点乘自己,再开根号,有根号就是非线性,所以曲率约束是非线性约束。
所以曲率约束在参考线平滑这里一般不加,因为处理起来比较麻烦。
这样整个参考线以及参考线平滑内容讲解完毕。至于具体实践部分,放到下一篇博客再介绍。
参考线平滑理论上比较简单,内容不多,目标函数就是三个代价相加,写上二次规划,加约束就可以求解了。
四、算法加速方法
前面的理论看起来好像不是特别难,但实际上这一节很难。因为难度不难在理论上,而是难在实践上。实践有最大的难点就是快。因为算法不能太慢,因为参考线算法是一切的基础,看开头所说的决策规划流程,第一步就是要计算参考线,剩下的步骤都得以此为基础。所以的参考线平滑算法不能太慢,必须要快。写出参考线平滑算法不难,但让程序运行得非常快,是非常费时间和费功夫。
从 GitHub 上的模型就能看出来,真正的二次规划算平滑参考线只占非常一小块的地方,有很大部分都是解决怎样让代码运行得更快的。方法不是唯一的,在 GitHub 上的模型写的只是一种加速方法。
4.1 降低执行频率
首先,规划执行频率不要高,大概 100 m s 100ms 100ms 执行一次即可,不要每次算得很频繁,要算得频繁自然就慢,因此二次规划算法不要执行得太过于频繁。
4.2 轨迹拼接
每次参考线的选取都是以车的匹配点或投影点为原点往前取 150 m 150m 150m,往后取 30 m 30m 30m 。这一点作为参考线的输入,因为车的运动速度有限,在每个规划周期之间不可能运动得非常快。所以两个规划周期之间必然很多参考线的选取是重复的,在上一次规划平滑时,就已经算过了,优化结果已知,所以就没有必要再算一遍,直接用上一次规划周期的结果。
当然不可能和旧的结果完全一样,因为肯定会有新点加入进来,只需要处理新点即可,把新加进来的点做二次规划,这样二次规划的规模和计算量就会小很多,因为处理的点比较少。
五、参考线平滑算法难点
5.1 快速找到车在全局路径下的投影点
这是一切的基础,因为如果找不到投影点,没法往前取 150 m 150m 150m、往后取 30 m 30m 30m,如何去快速找到车在全局路径下的投影点,这也是一个问题。由此可见,解决这些问题有多难,要写轨迹拼接算法。
5.2 执行频率的调度问题
如果规划是 100 m s 100ms 100ms 执行一次的话,得写调度,如何让 Simulink 每 100 m s 100ms 100ms 执行一次,也是问题。虽然理论不难,但从工程实践上来说,要处理很多的逻辑和算法。
六、总结
在自动驾驶决策规划算法中,参考线是解决导航路径过长且不平滑问题的关键。通过截取全局路径中的一段较短路径并进行平滑处理,简化了障碍物投影和匹配点的确定,使得规划算法能够在较小的范围内搜索最优路径。参考线的优点在于,较短的参考线投影更容易确定,且经过平滑处理后,路径更加平滑。
参考线平滑算法通过代价函数来优化,代价函数包含了与原路径点相似代价、平滑代价和紧凑代价。通过将代价函数写成二次规划的形式,可以求解出最优的参考线点。在实际应用中,参考线平滑算法面临着许多挑战,特别是在算法的执行速度上。为了提高算法效率,可以采取降低执行频率和轨迹拼接的方法。
本篇博客就讲解参考线平滑的理论部分,下一篇博客会讲解如何让算法跑得更快,编写具体的实践代码,欢迎关注后续内容!
参考资料
自动驾驶决策规划算法第二章第二节(上) 参考线模块
后记:
🌟 感谢您耐心阅读这篇关于 参考线平滑算法与二次规划 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀