【自动驾驶】决策规划算法(二)参考线模块Ⅰ| 平滑算法与二次规划

news2024/9/23 11:12:38

写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_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 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 执行一次,也是问题。虽然理论不难,但从工程实践上来说,要处理很多的逻辑和算法。


六、总结

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

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

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


参考资料

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


后记:

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

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

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

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

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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/2157392.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

(学习记录)使用 STM32CubeMX——GPIO引脚输入配置

STM32F103C8T6的GPIO引脚输入配置 时钟配置 (学习记录)使用 STM32CubeMX——配置时钟(入门)https://blog.csdn.net/Wang2869902214/article/details/142423522 GPIO 引脚输出配置 (学习记录)使用 STM32…

Springcloud框架-能源管理系统-能源管理系统源码-能源在线监测平台-双碳平台

一、介绍 基于SpringCloud的能管管理系统-能源管理平台源码-能源在线监测平台-双碳平台源码-SpringCloud全家桶-能管管理系统源码 有需者咨询,非诚勿扰; 二、软件架构 二、功能介绍 三、数字大屏展示 四、数据采集原理 五、软件截图

macos pyenv 安装python tk 、tkinter图形库方法步骤和使用总结

在macos中, pyenv 是一款用来管理多版本python 的工具, 我们常用的tk图形库是一个独立的工具库,我们在python里面使用的tkinter模块仅是调用这个独立的tk图形库, 所以如果我们希望在python里面使用它, 就必须要先安装t…

委托的注册及注销+观察者模式

事件 委托变量如果公开出去,很不安全,外部可以随意调用 所以取消public,封闭它,我们可以自己书写两个方法,供外部注册与注销,委托调用在子方法里调用,这样封装委托变量可以使它更安全,这个就叫…

金融加密机的定义与功能

金融加密机是一种用于保护金融交易数据和信息安全的重要安全设备。它通过硬件和软件的多重保障,确保金融交易中的敏感数据不被泄露或篡改。以下是关于金融加密机的详细介绍: 一、定义与功能 金融加密机是一种硬件安全设备,通过实现各种密码算…

深度deepin初体验(一)系统详细安装过程 | 国产系统

这里写自定义目录标题 深度deepin初体验(一)系统详细安装过程1.介绍2.安装要求3.环境4.创建虚拟机/系统升级系统选择语言硬盘分区备份文件拷贝系统重启常规设置 深度deepin初体验(一)系统详细安装过程 1.介绍 深度deepin是在debi…

Python开发深度学习常见安装包 error 解决

Python Python 是一种广泛使用的高级编程语言,它以其清晰的语法和代码可读性而闻名。Python 支持多种编程范式,包括面向对象、命令式、函数式和过程式编程。由于其简洁性和强大的标准库,Python 成为了数据科学、机器学习、网络开发、自动化脚…

气膜馆:新型场馆的盈利之道—轻空间

气膜馆作为一种创新的场馆形式,凭借其先进的技术和灵活的应用,正在快速崛起,展现出广阔的市场前景与丰富的盈利潜力。通过多元化的经营模式,气膜馆为创业者提供了前所未有的商机。本文将深入分析气膜馆的盈利模式及其在市场中的竞…

气膜储煤棚:未来能源管理的新选择—轻空间

在全球对可持续发展与环保的日益重视下,传统的煤炭储存方式面临着诸多挑战。气膜储煤棚应运而生,成为现代煤炭储存的理想解决方案。本文将深入探讨气膜储煤棚的优势与应用,为企业提供新的思路。 先进的技术设计 气膜储煤棚采用创新的气膜技术…

AcWing算法基础课-790数的三次方根-Java题解

大家好,我是何未来,本篇文章给大家讲解《AcWing算法基础课》790 题——数的三次方根。本题考查算法为浮点数二分查找。本文详细介绍了一个使用二分法计算浮点数三次方根的算法。通过逐步逼近目标值,程序能够在给定的区间内精确计算出结果&…

关闭小广告【JavaScript】

在 JavaScript 中实现关闭小广告的功能&#xff0c;可以通过监听点击事件来隐藏广告元素。 实现效果&#xff1a; 代码&#xff1a; <!DOCTYPE html> <html lang"zh"><head><meta charset"UTF-8"><meta name"viewport&q…

SpringBoot框架之KOB项目 - 配置Mysql与注册登录模块(中)

修改Spring Security 登录验证模式 传统的验证登录模式 公开页面&#xff1a;输入url就可以直接访问授权页面&#xff1a;登录之后才可以访问 Jwt验证模式 容易实现跨域不需要在服务器端存储 对比于传统模式将所有的sessionId换成jwt token access token refresh token 过…

如何在Chrome最新浏览器中调用ActiveX控件?

小编最近登陆工商银行网上银行&#xff0c;发现工商银行的个人网银网页&#xff0c;由于使用了ActiveX安全控件&#xff0c;导致不能用高版本Chrome浏览器打开&#xff0c;目前只有使用IE或基于IE内核的浏览器才能正常登录网上银行&#xff0c;而IE已经彻底停止更新了&#xff…

Tile View Kanban Board平铺视图和看板

Goto 数据网格和视图入门 平铺视图&#xff08;TileView 类&#xff09;将数据记录显示为平铺。此视图类型可以以任何自定义方式排列多个元素&#xff08;bound 和 unbound&#xff09;。用户可以按如下方式编辑瓦片&#xff1a; 使用模态 Edit Form。利用 HTML-CSS 平铺模板…

VScode配置连接远程服务器configure ssh Hosts

VScode配置连接远程服务器&#xff0c;具体步骤 一、点击VScode左下脚这两个∟的按钮 二、点击完上面的按钮后&#xff0c;出现如下的下拉选项&#xff0c;选择“Connect to Host” 三、选择“Connect to Host”后&#xff0c;下拉选项会更新&#xff0c;选择“Configure SSH …

openFrameworks_如何使用ofxXmlSettings和ofxGui来创建识别界面

效果图&#xff1a; 代码及详解 1.添加两个插件的头文件: #include "ofxGui.h" #include "ofxXmlSettings/src/ofxXmlSettings.h" 2.添加GUI部分&#xff0c;然后在.h声明右边的openframeworks的UI部分&#xff0c;包括面板ofxPanel&#xff0c;按钮ofx…

【JUC并发编程系列】深入理解Java并发机制:线程局部变量的奥秘与最佳实践(五、ThreadLocal原理、对象之间的引用)

文章目录 【JUC并发编程系列】深入理解Java并发机制&#xff1a;线程局部变量的奥秘与最佳实践(五、ThreadLocal原理、对象之间的引用)1. 基本 API 介绍2. 简单用法3. 应用场景4. Threadlocal与Synchronized区别5. 内存溢出和内存泄漏5.2 内存溢出 (Memory Overflow)5.2 内存泄…

如何融合文本信息提高时序预训练模型?

今天小编给大家介绍两篇联合文本和时序数据进行预训练的文章。 UniTime: A Language-Empowered Unified Model for Cross-Domain Time Series Forecasting 文献地址&#xff1a;https://arxiv.org/pdf/2310.09751.pdf 代码地址&#xff1a;https://github.com/liuxu77/UniTim…

【永磁同步电机(PMSM)】 4. 同步旋转坐标系仿真模型

【永磁同步电机&#xff08;PMSM&#xff09;】 4. 同步旋转坐标系仿真模型 1. Clarke 变换的模型与仿真1.1 Clarke 变换1.2 Clarke 变换的仿真模型 2. Park 变换的模型与仿真2.1 Park 变换2.2 Park 变换的仿真模型 3. Simscape标准库变换模块3.1 abc to Alpha-Beta-Zero 模块3…

【COMSOL】1-1 COMSOL6.2软件安装

1.解压COMSOL软件安装包&#xff0c;以管理员身份运行Setup.exe 2.选择简体中文&#xff0c;点击下一步&#xff0c;点击新安装。 3.选择许可证文件 4.自定义安装的位置 5.取消勾选更新&#xff0c;点击下一步 6.若已安装MATLAB则自动识别文件夹&#xff0c;若未安装空着即可&a…