當前位置：首頁 >

【自动驾驶】汽车速度规划介绍

發布時間：2024/3/24 72 豆豆

生活随笔收集整理的這篇文章主要介紹了【自动驾驶】汽车速度规划介绍小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

文章目錄

參考資料
1. 介紹
- 1.1 常用方法
2. QP算法
- 2.1 障礙物預測
- 2.2 S-T圖生成
- 2.3 S-T 圖柵格化
- 2.4 創建目標時刻點
- 2.5 搜索產生路徑
- 2.6 速度平滑
- - 多項式回歸在速度平滑中的應用。

參考資料

《自動駕駛汽車決策與控制》書籍

本篇博客主要摘抄自書上速度規劃這一部分，僅用于學習，方便查閱。

1. 介紹

當局部路徑規劃給定了一條或者若干條選出的路徑曲線之后, 運動規劃模塊需要解決的后續問題是在此局部路徑的基礎上加人與速度相關的信息, 這一問題被稱為速度規劃。

速度規劃的目標是在給定的局部路徑曲線上, 在滿足反饋控制的操作限制及符合行為決策的輸出結果這兩個前提下, 將路徑點賦予速度及加速度信息。速度規劃主要考慮的是對動態障礙物的規避。

1.1 常用方法

速度規劃有以下常見的方法:

對路徑指定線加速度來生成速度: 線加速度可以是某一常數, 或是由比例-微分控制器來生成,
樣條插值: 在給定時間內通過一段給定路徑的速度生成問題。其解決方案是將時間域劃分為若干區間, 使用速度關于時間的三次樣條函數來插值。這一方法容易產生加速度變化率較大的問題。
函數擬合: 直接用速度關于路徑長度的二次多項式來生成速度。這種方法較為簡單。
目標時刻點法: 速度規劃部分首先根據對障礙物未來運動狀態的預測, 在規劃路徑與時間這兩個維度構成的二維圖中標記障礙物在未來一段時間內所占據的區域。以目標汽車當前車速勻速通過規劃路徑所需的時間為基準, 根據一定原則創建一組目標時刻點, 在此二維圖中以目標時刻點為目標點搜索產生一組速度規劃方案。
QP算法: 這一方法引人了S-T圖的概念,并把自動駕駛汽車速度規劃歸納為S-T圖上的搜索問題進行求解。S-T圖是一個關于給定局部路徑縱向位移和時間的二維關系圖。任何一個S-T圖都基于一條已經給定的軌跡曲線。根據自動駕駛汽車預測模塊對動態障礙物的軌跡預測,每個動態障礙物都會在這條給定的路徑上有投影, 從而產生對于一定S-T區域的覆蓋。

2. QP算法

下面詳解QP算法進行速度規劃。

2.1 障礙物預測

當環境中出現移動障礙的時候, 移動障礙會在末來某些時刻 $(t1、t2、t3)\left(t_{1} 、 t_{2} 、 t_{3}\right)$ 占據已經規劃好的路徑。如果目標汽車仍然以當前車速勻速行駛, 有可能會在末來的某一時刻與移動障礙相碰撞。

根據移動障礙當前所處位置 $(x0obj,y0obj)\left(x_{0_{o b j}}, y_{0_{o b j}}\right)$ 、速度 $(v0obj,v0obj)\left(v_{0_{o b j}}, v_{0_{o b j}}\right)$ 、加速度 $(a0obj,a0obj)\left(a_{0_{o b j}}, a_{0_{o b j}}\right)$ , 可以預測經過時間 $t$ 后移動障礙所處位置 $(xtobj,ytobj)\left(x_{t_{\mathrm{obj}}}, y_{t_{\mathrm{obj}}}\right)$ :
${xtobj=x0obj+vxobj×t+12×axobj×t2ytobj=y0obj+vyobj×t+12×ayobj×t2\left\{\begin{array}{l} x_{t_{\mathrm{obj}}}=x_{0_{\mathrm{obj}}}+v x_{\mathrm{obj}} \times t+\frac{1}{2} \times a x_{\mathrm{obj}} \times t^{2} \\ y_{t_{\mathrm{obj}}}=y_{0_{\mathrm{obj}}}+v y_{\mathrm{obj}} \times t+\frac{1}{2} \times a y_{\mathrm{obj}} \times t^{2} \end{array}\right.$

用矩陣表示為:
$Y=[xtobjytobj],X=[1tt2],A=[x0objvxobj12axobjy0objvyobj12ayobj]Y=AX\boldsymbol{Y}=\left[\begin{array}{l} x_{t_{o b j}} \\ y_{t_{o b j}} \end{array}\right], \quad \boldsymbol{X}=\left[\begin{array}{c} 1 \\ t \\ t^{2} \end{array}\right], \quad \boldsymbol{A}=\left[\begin{array}{ccc} x_{0_{o b j}} & v x_{\mathrm{obj}} & \frac{1}{2} a x_{\mathrm{obj}} \\ y_{0_{\mathrm{obj}}} & v y_{\mathrm{obj}} & \frac{1}{2} a y_{\mathrm{obj}} \end{array}\right] \\ \\ Y=AX$

得到 $t$ 時刻移動障礙所處位置 $Y\boldsymbol{Y}$ 后, 根據移動障礙輪廓計算其覆蓋區域 $Q$ :
$\Rightarrow Q$
圖 $4.23$ 所示為障礙物覆蓋區域與規劃軌跡的交集示意圖。

計算 $Q$ 與規劃路徑 $S$ 的交集 $ΔS\Delta S$ :
$ΔS=Q∩S\Delta S=Q \cap S$
那么, $ΔS\Delta S$ 與 $t$ 一一對應。

2.2 S-T圖生成

S-T圖是已經規劃完成的路徑縱向位移與時間之間的二維關系圖(見圖4.24)。前面已經獲得 $ΔS\Delta S$ 關于 $t$ 的信息, 那么可以在S-T圖中標記某一時刻t在路徑S上占據的某一段路徑 $ΔS\Delta S$ 。如圖4.24所示,假如移動障礙物經過了規劃路徑, 那么因為移動障礙物在一段時間內占據了規劃路徑, 在S-T圖中將會有一塊覆蓋區域。

圖 $4.24$ 中 $Sgoal?S_{\text {goal }}$ 為規劃路徑的長度。虛線代表的是目標汽車以當前車速勻速沿規劃路徑行駛, 用時記為 $t_{a}$ 。可以發現在某些時刻目標汽車和移動障礙將會同時出現在期望路徑的某一位置, 這意味著目標汽車將與移動障礙碰撞。實線代表的是目標汽車從當前時刻開始以最大加速度加速通過期望路徑, 用時記為 $tmin?t_{\text {min }}$ 。

2.3 S-T 圖柵格化

對 S-T 圖柵格化。為了有利于在 S-T 圖中進行路徑搜索, 需要對 S-T 圖進行離散化、網格化。根據需要確定采樣周期和規劃路徑的路徑點間隔, 如圖 $4.25$ 所示。

從當前時刻開始根據采樣周期針對末來某一時刻 $t_{k}$ 計算移動障礙所處位置 $Yk\boldsymbol{Y}_{k}$ , 其中 $k$ 為 $\sim c$ 的自然數, $c$ 的值根據移動障礙離開規劃路徑的時刻確定。 $t_{k}$ 與 $t_{k+1}$ 之間的時間差為采樣周期。

$Xk=[1tktk2],Yk=[xtkytk],k∈(1,c)且?k∈NYk=AXkYk?QkΔSk=Qk∩Sxy\begin{aligned} &\boldsymbol{X}_{k}=\left[\begin{array}{c} 1 \\ t_{k} \\ t_{k}^{2} \end{array}\right], \quad \boldsymbol{Y}_{k}=\left[\begin{array}{l} x_{t_{k}} \\ y_{t_{k}} \end{array}\right], \quad k \in(1, c) \text { 且 } k \in \mathbf{N} \\ &\boldsymbol{Y}_{k}=\boldsymbol{A} \boldsymbol{X}_{k} \\ &\boldsymbol{Y}_{k} \Rightarrow Q_{k} \\ &\Delta S_{k}=Q_{k} \cap S_{x y} \end{aligned}$
最終得到 $ΔSk,ΔSk\Delta S_{k}, \Delta S_{k}$ 中包含在時刻 $t k$ 移動障礙物在采樣后的期望路徑點上的投影。將 $t k$ 與 $ΔSk\Delta S_{k}$ 相對應的標記在網格化后的 $S?T\mathrm{S}-\mathrm{T}$ 圖上, 此即為 $S?T\mathrm{S}-\mathrm{T}$ 搜索圖。

2.4 創建目標時刻點

創建 $S?T\mathrm{S}-\mathrm{T}$ 搜索圖的目的在于為搜索算法提供搜索空間。從 $t_{a}$ 與 $t_{\min }$ 中選擇較小的值記為 $Ttmin?T_{t_{\text {min }}}$ , 從 $Ttmin?T_{t_{\text {min }}}$ 開始向后等間隔產生一組時刻點, 這些時刻點(包括 $Ttmin?T_{t_{\text {min }}}$ ) 用 $Ttr(r∈NT_{t_{r}}(r \in \mathbf{N}$ 且 $\neq 0)$ 表示, 數目由移動障礙在 $S?T\mathrm{S}-\mathrm{T}$ 圖中占據的區域而定。 $T_{t_{r}}$ 被稱為目標時刻, $T_{t_{r}}$ 的集合記為 $T_{T}, T_{T}$ 被稱為目標時刻集。同時產生與之數量相匹配的一組 $T_{s_{r}}, T_{s_{r}}$ 的大小與 $Sgoal?S_{\text {goal }}$ 相等, 被稱為目標路徑點, 這一組 $T_{s_{r}}$ 的集合記為 $T_{S}$ , 稱為目標路徑點集。 $T_{T}$ 與 $T_{S}$ 中的元素一一對應, 并且對應了 $S?T\mathrm{S}-\mathrm{T}$ 圖上 $T_{S}$ 線上的一組點, 這組點稱為目標時刻點。

2.5 搜索產生路徑

結合上游行為決策輸出的信息, 速度規劃可以靈活設置障礙物體周邊的代價 (Cost), 達到調整速度方案的目的。當上游決定針對物體 a 進行搶先決策時, 在 $S?T\mathrm{S}-\mathrm{T}$ 圖上物體 $a\mathrm{a}$ 運動路徑上方的網格的 Cost 就可以調成偏小。同時, 為了避免任何潛在的碰撞, 所有動態障礙物的路徑經過的網格的 Cost 都需要調大。

除此之外, 還需要考慮一條給定速度方案在加速度等方面的 Cost。例如, S-T 圖上過于 “陡峭”的曲線代表加速度大甚至不連續, 這樣很有可能導致反饋控制模塊 (Feedback Control)無法實際執行。所以, 每條曲線所對應的速度方案均有一個整體的 Cost。實際上, 根據上游 (決策) 輸出和下游 (控制) 限制來調整 Cost, 是速度規劃中的 S-T 圖算法的關鍵設置。在設置好 Cost 的基礎上, 最小 Cost 局部路徑的產生可以用類似 $A?\mathrm{A}^{*}$ 或者 Dijkstra 等簡單搜索算法實現。在得到了最小 Cost 的 $S?T\mathrm{S}-\mathrm{T}$ 路徑后,可以簡單算出局部路徑上任何一個位置對應的速度 (對應 S-T 圖任意點斜率) 和加速度 (斜率的導數), 從而完成速度規劃的計算。

基于路徑搜索算法針對每一個 S-T 候選目標點,在 S-T 禁忌搜索空間中搜索產生從起點到目標點的路徑, 如圖 4.26 所示。

2.6 速度平滑

通過路徑搜索算法在S-T禁忌搜索圖中找到一條從起點到目標點的路徑的時候, 由于對原有 S-T圖的離散采樣, 所以得到的路徑也是曲折且離散的。為了獲得平滑的路徑曲線, 可以利用多項式回歸分析對離散路徑點進行擬合, 此外, QP 算法同樣可應用于速度平滑過程中。

多項式回歸在速度平滑中的應用。

速度規劃中通過路徑搜索產生的路徑點數量巨大, 而且對計算機計算時間也有要求, 所以對速度規劃路徑點進行回歸分析通過迭代尋優的方式完成。以基于 5 次曲線的多項式為例, 對其對應 5 次多項式的系數進行迭代尋優, 從而獲取平滑后的速度曲線。

在對 S-T 圖中的路徑點進行 5 次曲線擬合得到速度規劃方案時, 需要考慮目標汽車在 $t$ 為 0 時當前時刻目標汽車的速度值和加速度值以及 S-T 圖中創建的目標時刻點。也就是說利用 5 次曲線對 S-T 圖中速度規劃路徑點的擬合, 是一個包含等式約束的優化問題。

當通過路徑搜索算法在 S-T 圖中搜索產生路徑后, 可以獲取一組路徑點 $(ti,si)(i=0\left(t_{i}, s_{i}\right)(i=0$ , $\cdots, m)$ 。用于速度規劃路徑點的 5 次擬合函數為:
$s(t)=k_{0}+k_{1} t+k_{2} t^{2}+k_{3} t^{3}+k_{4} t^{4}+k_{5} t^{5}$
由此可以構建損失函數:
$J(k0,k1,k2,k3,k4,k5)=12m∑i=1m(hk(ti)?si)2J\left(k_{0}, k_{1}, k_{2}, k_{3}, k_{4}, k_{5}\right)=\frac{1}{2 m} \sum_{i=1}^{m}\left(h_{k}\left(t_{i}\right)-s_{i}\right)^{2}$
在速度規劃中, 目標汽車在軌跡上所處的位置用 $s$ 表示, 速度用 $s′s^{\prime}$ 表示, 加速度用 $s′′s^{\prime \prime}$ 表示, 則在時刻 $t$ , 目標汽車的車速為:
$s′(t)=k1+2k2t+3k3t2+4k4t3+5k5t4s^{\prime}(t)=k_{1}+2 k_{2} t+3 k_{3} t^{2}+4 k_{4} t^{3}+5 k_{5} t^{4}$

目標汽車加速度為:
$s′′(t)=2k2+6k3t+12k4t2+20k5t3s^{\prime \prime}(t)=2 k_{2}+6 k_{3} t+12 k_{4} t^{2}+20 k_{5} t^{3}$
已知 $t = 0$ 時初始狀態下目標汽車在規劃軌跡上所處位置、汽車速度、汽車加速度為已知量, 即
$s(0)=k0=st=0s′(0)=k1=st=0′s′′(0)=2k2=st=0′′\begin{aligned} &s(0)=k_{0}=s_{t=0} \\ &s^{\prime}(0)=k_{1}=s_{t=0}^{\prime} \\ &s^{\prime \prime}(0)=2 k_{2}=s_{t=0}^{\prime \prime} \end{aligned}$
并且可以知道目標時刻點信息, 即存在
$s(tr)=k0+k1tr+k2tr2+k3tr3+k4tr4+k5tr5=sts\left(t_{r}\right)=k_{0}+k_{1} t_{r}+k_{2} t_{r}^{2}+k_{3} t_{r}^{3}+k_{4} t_{r}^{4}+k_{5} t_{r}^{5}=s_{t}$
那么聯立可得 :
${k0=st=0k1=st=0′k2=12st=0′′k3=sttr3?st=0tr3?st=0′tr2?st=0′′2tr?k4tr?k5tr2\left\{\begin{array}{l} k_{0}=s_{t=0} \\ k_{1}=s_{t=0}^{\prime} \\ k_{2}=\frac{1}{2} s_{t=0}^{\prime \prime} \\ k_{3}=\frac{s_{t}}{t_{r}^{3}}-\frac{s_{t=0}}{t_{r}^{3}}-\frac{s_{t=0}^{\prime}}{t_{r}^{2}}-\frac{s_{t=0}^{\prime \prime}}{2 t_{r}}-k_{4} t_{r}-k_{5} t_{r}^{2} \end{array}\right.$
損失函數可以表示為僅關于 $k_{4}, k_{5}$ 的表達式, 即
$J(k4,k5)=12m∑i=1m(hk(ti)?si)2J\left(k_{4}, k_{5}\right)=\frac{1}{2 m} \sum_{i=1}^{m}\left(h_{k}\left(t_{i}\right)-s_{i}\right)^{2}$
則 $k_{4}, k_{5}$ 的梯度分別為:
$??k4J(k4,k5)=??k412m∑i=1m(hk(ti)?si)2=1m12m∑i=1m[(hk(ti)?si)(ti4?trti3)]\frac{\partial}{\partial k_{4}} J\left(k_{4}, k_{5}\right)=\frac{\partial}{\partial k_{4}} \frac{1}{2 m} \sum_{i=1}^{m}\left(h_{k}\left(t_{i}\right)-s_{i}\right)^{2}=\frac{1}{m} \frac{1}{2 m} \sum_{i=1}^{m}\left[\left(h_{k}\left(t_{i}\right)-s_{i}\right)\left(t_{i}^{4}-t_{r} t_{i}^{3}\right)\right]$

$??k5J(k4,k5)=??k512m∑i=1m(hk(ti)?si)2=1m12m∑i=1m[(hk(ti)?si)(ti5?tr2ti3)]\frac{\partial}{\partial k_{5}} J\left(k_{4}, k_{5}\right)=\frac{\partial}{\partial k_{5}} \frac{1}{2 m} \sum_{i=1}^{m}\left(h_{k}\left(t_{i}\right)-s_{i}\right)^{2}=\frac{1}{m} \frac{1}{2 m} \sum_{i=1}^{m}\left[\left(h_{k}\left(t_{i}\right)-s_{i}\right)\left(t_{i}^{5}-t_{r}^{2} t_{i}^{3}\right)\right]$
梯度下降尋優的過程就是不斷更新 $k_{4}, k_{5}$ 。

求最小損失函數值的過程:
$k4=k4+α1m∑i=1m[(hk(ti)?si)(ti4?trti3)]k5=k5+α1m∑i=1m[(hk(ti)?si)(ti5?trti3)]\begin{aligned} k_{4} &=k_{4}+\alpha \frac{1}{m} \sum_{i=1}^{m}\left[\left(h_{k}\left(t_{i}\right)-s_{i}\right)\left(t_{i}^{4}-t_{r} t_{i}^{3}\right)\right] \\ k_{5} &=k_{5}+\alpha \frac{1}{m} \sum_{i=1}^{m}\left[\left(h_{k}\left(t_{i}\right)-s_{i}\right)\left(t_{i}^{5}-t_{r} t_{i}^{3}\right)\right] \end{aligned}$
當損失函數值收斂于允許誤差之內時, 認為找到最優解, 此時得到的 $k_{4}, k_{5}$ 可以最終確定對給定的路徑點擬合程度最好的擬合函數方程。

總結

以上是生活随笔為你收集整理的【自动驾驶】汽车速度规划介绍的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇：虚拟机Linux6下安装Oracle 1
下一篇：拼多多买手机靠谱不？