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

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

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

📚 系列专栏:【决策规划】系列,带您深入浅出,领略自动驾驶决策规划的魅力。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!

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



引言

  各位小伙伴们大家好,欢迎收看自动驾驶决策规划算法第二节,内容整理自 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 P0P1P2P3 ∣ 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} P1rP2rP3r,优化后的点记为 P 1 、 P 2 、 P 3 P_1、P_2、P_3 P1P2P3。衡量与几何相似度的标准就是,如果这三条绿线的长度 ∣ 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=aP2P3=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+bP2P3=ab  认为比较分散

  不妨把两边的长度平方和算一下,前面紧凑的等于 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+(ab)2=2a2+2b2,这样就能看出来,用 ∣ P 1 P 2 ∣ 2 + ∣ P 2 P 3 ∣ 2 |P_1P_2|^2+|P_2P_3|^2 P1P22+P2P32 来衡量,即 ∣ P 1 P 2 ∣ 2 + ∣ P 2 P 3 ∣ 2 |P_1P_2|^2+|P_2P_3|^2 P1P22+P2P32 越小,意味着越均匀、越紧凑。

  综上就可以写出平滑算法的代价函数,比如下图:
在这里插入图片描述

  原来的点为 P 1 r , P 2 r , P 3 r P_{1r},P_{2r},P_{3r} P1rP2rP3r,优化后的点为 P 1 、 P 2 、 P 3 P_1、P_2、P_3 P1P2P3

  原来路径点坐标记为 ( 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=13(xixir)2+(yiyir)2}+w2[(x1+x32x2)2+(y1+y32y2)2]+w3{i=12(xi+1xi)2+(yi+1yi)2}  包含三个代价,第一项为与原路径点相似代价,第二项为平滑代价,第三项为紧凑代价。 w 1 、 w 2 、 w 3 w_1、w_2、 w_3 w1w2w3 是相应的权重。

  希望代价函数越小越好,因为越小就与原路径点越相似、越平滑、越紧凑。

  我们的任务就是选取合适的 ( 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.tAxblbxub   接下来看一下上面的代价函数如何写成二次规划的形式。

(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+x32x2)2+(y1+y32y2)2(x1+x32x2,y1+y32y2)(x1+x32x2,y1+y32y2)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+x32x2,y1+y32y2)=(x1,y1,x2,y2,x3,y3) 102010010201
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=(100120021001)  则平滑代价函数为
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+x32x2)2+(y1+y32y2)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=1n2(xi+xi+22xi+1)2+(yi+yi+22yi+1)2  一共是 ( 2 n − 4 ) (2n-4) (2n4) 项。

  展开写成向量的形式:
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+x32x2,y1+y32y2,x2+x42x3,y2+y42y3,)(x1+x32x2,y1+y32y2,x2+x42x3,y2+y42y3,)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,) 1020100102011020100102011020100201   大括号内的矩阵是将 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×(2n4) 的矩阵。

  将大括号矩阵记为 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_costxTA1TA1x  因为 A 1 T A^T_1 A1T ( 2 n , 2 n − 4 ) (2n,2n-4) (2n,2n4) 的矩阵,则 A 1 A_1 A1 就是 ( 2 n − 4 , 2 n ) (2n-4,2n) (2n4,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=1n1(xixi+1)2+(yiyi+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=(x1x2,y1y2,x2x3,y2y3)(x1x2,y1y2,x2x3,y2y3)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,) 1010010110100101101001   其中, ( 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×(2n2) 的矩阵。

  将大括号矩阵记为 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_costxTA2TA2x  因为 A 2 T A^T_2 A2T ( 2 n , 2 n − 2 ) (2n,2n-2) (2n,2n2) 的矩阵,则 A 2 A_2 A2 就是 ( 2 n − 2 , 2 n ) (2n-2,2n) (2n2,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=1n(xixir)2+(yiyir)2=i=1n(xi2+yi2)+i=1n(2xirxi2yiryi)+i=1n(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=1n(xi2+yi2)+i=1n(2xirxi2yiryi)写成二次规划的形式:
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,......) x1y1xnyn   其中,大括号矩阵为单位矩阵,记为 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= 2x1r2y1r2xnr2ynr

  综上所述,把这三个代价写成二次规划的形式,统一起来:
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(wsommthA1TA1+wlengthA2TA2+wrefA3TA3)x+wrefhTx二次规划的标准形式:
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(wsommthA1TA1+wlengtnA2TA2+wrefA3TA3)=wrefhT  至此,如何把优化问题转化为二次规划问题,以及怎么转化就讲解完毕。

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} xxrefbuff展开
x r e f − buff ≤ x ≤ x r e f + buff x_{ref}-\text{buff}\leq x\leq x_{ref}+\text{buff} xrefbuffxxref+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 执行一次,也是问题。虽然理论不难,但从工程实践上来说,要处理很多的逻辑和算法。


六、总结

  在自动驾驶决策规划算法中,参考线是解决导航路径过长且不平滑问题的关键。通过截取全局路径中的一段较短路径并进行平滑处理,简化了障碍物投影和匹配点的确定,使得规划算法能够在较小的范围内搜索最优路径。参考线的优点在于,较短的参考线投影更容易确定,且经过平滑处理后,路径更加平滑。

  参考线平滑算法通过代价函数来优化,代价函数包含了与原路径点相似代价、平滑代价和紧凑代价。通过将代价函数写成二次规划的形式,可以求解出最优的参考线点。在实际应用中,参考线平滑算法面临着许多挑战,特别是在算法的执行速度上。为了提高算法效率,可以采取降低执行频率和轨迹拼接的方法。

  本篇博客就讲解参考线平滑的理论部分,下一篇博客会讲解如何让算法跑得更快,编写具体的实践代码,欢迎关注后续内容!


参考资料

  自动驾驶决策规划算法第二章第二节(上) 参考线模块


后记:

🌟 感谢您耐心阅读这篇关于 参考线平滑算法与二次规划 的技术博客。 📚

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

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

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

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

更多推荐