當(dāng)前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

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

發(fā)布時間：2024/3/24 编程问答 44 豆豆

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

文章目錄

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

參考資料

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

本篇博客主要摘抄自書上速度規(guī)劃這一部分，僅用于學(xué)習(xí)，方便查閱。

1. 介紹

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

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

1.1 常用方法

速度規(guī)劃有以下常見的方法:

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

2. QP算法

下面詳解QP算法進(jìn)行速度規(guī)劃。

2.1 障礙物預(yù)測

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

根據(jù)移動障礙當(dāng)前所處位置 $(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)$ , 可以預(yù) 測經(jīng)過時間 $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}$ 后, 根據(jù)移動障礙輪廓計算其覆蓋區(qū)域 $Q$ :
$\Rightarrow Q$
圖 $4.23$ 所示為障礙物覆蓋區(qū)域與規(guī)劃軌跡的交集示意圖。

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

2.2 S-T圖生成

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

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

2.3 S-T 圖柵格化

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

從當(dāng)前時刻開始根據(jù)采樣周期針對末來某一時刻 $t_{k}$ 計算移動障礙所處位置 $Yk\boldsymbol{Y}_{k}$ , 其中 $k$ 為 $\sim c$ 的自然數(shù), $c$ 的值根據(jù)移動障礙離開規(guī)劃路徑的時刻確定。 $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}$ 相對應(yīng)的標(biāo)記在網(wǎng)格化后的 $S?T\mathrm{S}-\mathrm{T}$ 圖上, 此即為 $S?T\mathrm{S}-\mathrm{T}$ 搜索圖。

2.4 創(chuàng)建目標(biāo)時刻點

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

2.5 搜索產(chǎn)生路徑

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

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

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

2.6 速度平滑

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

多項式回歸在速度平滑中的應(yīng)用。

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

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

當(dāng)通過路徑搜索算法在 S-T 圖中搜索產(chǎn)生路徑后, 可以獲取一組路徑點 $(ti,si)(i=0\left(t_{i}, s_{i}\right)(i=0$ , $\cdots, m)$ 。用于速度規(guī)劃路徑點的 5 次擬合函數(shù)為:
$s(t)=k_{0}+k_{1} t+k_{2} t^{2}+k_{3} t^{3}+k_{4} t^{4}+k_{5} t^{5}$
由此可以構(gòu)建損失函數(shù):
$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}$
在速度規(guī)劃中, 目標(biāo)汽車在軌跡上所處的位置用 $s$ 表示, 速度用 $s′s^{\prime}$ 表示, 加速度用 $s′′s^{\prime \prime}$ 表示, 則在時刻 $t$ , 目標(biāo)汽車的車速為:
$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}$

目標(biāo)汽車加速度為:
$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$ 時初始狀態(tài)下目標(biāo)汽車在規(guī)劃軌跡上所處位置、汽車速度、汽車加速度為已知量, 即
$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}$
并且可以知道目標(biāo)時刻點信息, 即存在
$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}$
那么聯(lián)立可得 :
${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.$
損失函數(shù)可以表示為僅關(guān)于 $k_{4}, k_{5}$ 的表達(dá)式, 即
$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]$
梯度下降尋優(yōu)的過程就是不斷更新 $k_{4}, k_{5}$ 。

求最小損失函數(shù)值的過程:
$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}$
當(dāng)損失函數(shù)值收斂于允許誤差之內(nèi)時, 認(rèn)為找到最優(yōu)解, 此時得到的 $k_{4}, k_{5}$ 可以最終確定對給定的路徑點擬合程度最好的擬合函數(shù)方程。

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

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