本發(fā)明設(shè)計(jì)自動(dòng)駕駛路徑規(guī)劃領(lǐng)域，設(shè)計(jì)一種基于數(shù)值優(yōu)化的自動(dòng)駕駛路徑規(guī)劃方法，在該規(guī)劃方法下，駕駛車可以生成一條平滑的躲避障礙物的路線，進(jìn)而實(shí)現(xiàn)自動(dòng)駕駛。

背景技術(shù)：

1、自動(dòng)駕駛技術(shù)的發(fā)展始于20世紀(jì)中期，但真正的突破出現(xiàn)在21世紀(jì)初。最早的自動(dòng)駕駛汽車原型多由研究機(jī)構(gòu)和大學(xué)開發(fā)，主要依賴于規(guī)則驅(qū)動(dòng)的系統(tǒng)和簡(jiǎn)單的傳感器技術(shù)。隨著谷歌、特斯拉、uber等科技巨頭的加入，自動(dòng)駕駛技術(shù)得到了迅速發(fā)展。自動(dòng)駕駛被認(rèn)為能夠提高交通安全性、減少交通事故、優(yōu)化交通流量、降低能源消耗和環(huán)境污染，同時(shí)也可能改變未來(lái)的城市規(guī)劃和生活方式。

2、自動(dòng)駕駛技術(shù)相對(duì)于人類駕駛員來(lái)書有諸多優(yōu)勢(shì)。首先，自動(dòng)駕駛汽車能夠高度高效地感知周圍環(huán)境，迅速獲取行駛路線所需的信息，這種感知效率優(yōu)于人類。其次，自動(dòng)駕駛汽車能夠根據(jù)預(yù)測(cè)的交通狀況合理規(guī)劃行駛路線，并在行駛中選擇最佳路徑，從而有效避開擁堵和繁忙路段。最后，自動(dòng)駕駛汽車還能夠預(yù)測(cè)周圍車輛的行駛方向，并通過調(diào)整自身的行駛軌跡，提前避免潛在的風(fēng)險(xiǎn)和危險(xiǎn)。

3、近年來(lái)，對(duì)于車輛路徑規(guī)劃的研究主要有基于搜索的方法、基于采樣的方法、基于數(shù)值優(yōu)化的路徑方法。其中，基于搜索的局部路徑規(guī)劃方法是把規(guī)劃問題向圖搜索問題進(jìn)行轉(zhuǎn)化，對(duì)求解空間進(jìn)行網(wǎng)格劃分，建立成圖，然后進(jìn)行起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑的搜索求解?；谒阉鞯囊?guī)劃方法通過不斷迭代搜索，可以得到全局層面的最優(yōu)解，但其搜索效率與規(guī)劃問題狀態(tài)維數(shù)相關(guān)，當(dāng)維數(shù)較高時(shí)，搜索過程比較耗時(shí)，因此只適用于較低維數(shù)的規(guī)劃問題的求解?；诓蓸拥穆窂揭?guī)劃方法更適用于高維空間中的路徑搜索問題。該方法通過在搜索空間中隨機(jī)或規(guī)則地采樣生成候選路徑，然后在這些候選路徑中選擇一條最優(yōu)路徑來(lái)引導(dǎo)車輛從起點(diǎn)到達(dá)終點(diǎn)。但是，隨著采樣速度分辨率的提高，其求解時(shí)間將會(huì)大幅度增加，從而需要更強(qiáng)大的算力。在變換場(chǎng)景時(shí)，代價(jià)函數(shù)選擇不合理也將會(huì)導(dǎo)致軌跡規(guī)劃的穩(wěn)定性較差。對(duì)于自動(dòng)駕駛汽車來(lái)說(shuō)，規(guī)劃路徑足夠平滑對(duì)于跟蹤控制至關(guān)重要，而通過搜索或者采樣方法得到的結(jié)果很難滿足車輛運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)要求。基于數(shù)值優(yōu)化的規(guī)劃方法能通過多項(xiàng)式曲線來(lái)表示規(guī)劃結(jié)果，即將路徑規(guī)劃問題轉(zhuǎn)化成曲線參數(shù)的求解問題，在這個(gè)過程中，可以添加車輛行駛規(guī)劃中所需要的約束條件，并最終將路徑規(guī)劃問題抽象成一個(gè)有約束優(yōu)化問題?；谠摲椒ǖ乃惴壳暗玫搅藢W(xué)術(shù)界和工業(yè)界的認(rèn)可和深度的研究。

4、因此，本發(fā)明設(shè)計(jì)了一種基于數(shù)值優(yōu)化的路徑規(guī)劃方法，通過構(gòu)建一系列路徑點(diǎn)，將環(huán)境中的所有避障約束轉(zhuǎn)化為求代價(jià)函數(shù)最小值的約束，消除最優(yōu)控制問題規(guī)模與環(huán)境復(fù)雜程度之間的相關(guān)性，通過凸多邊形覆蓋采樣點(diǎn)可行域，減少冗余區(qū)域，提高空間利用率，有效簡(jiǎn)化自動(dòng)駕駛汽車的避障約束，簡(jiǎn)化最優(yōu)控制問題數(shù)學(xué)模型，提高規(guī)劃路線的求解速度。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明主要目的在于設(shè)計(jì)了一種基于數(shù)值優(yōu)化的自動(dòng)駕駛路徑規(guī)劃方法，以克服現(xiàn)有技術(shù)中的不足，為自動(dòng)駕駛車輛提供更安全、更舒適、更平滑的駕駛軌跡，并解決了其他自動(dòng)駕駛方法存在的不滿足車輛動(dòng)力學(xué)模型約束和車輛運(yùn)動(dòng)學(xué)模型約束的問題。具體實(shí)現(xiàn)步驟如下：

2、步驟一，計(jì)算車輛在高精度地圖中以全局路徑規(guī)劃生成的路線作為待平滑的參考線，離散參考線上的每一個(gè)點(diǎn)得到一系列離散點(diǎn)，然后對(duì)相鄰的離散點(diǎn)進(jìn)行平滑處理得到平滑后的參考線；

3、步驟二，根據(jù)當(dāng)前車輛的位置在cartesian(笛卡爾)坐標(biāo)系下的坐標(biāo)信息和所述步驟一中參考線上的離散點(diǎn)在cartesian坐標(biāo)系下的信息，找到與車輛位置相鄰最近的離散點(diǎn)記為匹配點(diǎn)，根據(jù)匹配點(diǎn)在cartesian坐標(biāo)系下信息，找到自車位置在frenet坐標(biāo)系中的理想投影點(diǎn)；

4、步驟三，根據(jù)所述步驟二中理想投影點(diǎn)的信息，計(jì)算車輛在frenet坐標(biāo)系下的坐標(biāo)信息。同理，獲取障礙物在cartesian坐標(biāo)系的坐標(biāo)信息，并計(jì)算出障礙物在frenet坐標(biāo)下的坐標(biāo)信息；

5、步驟四，離散的在將要行駛的路徑上采樣一些點(diǎn)，進(jìn)行動(dòng)態(tài)規(guī)劃，正向?qū)?yōu)，逆向求解；正向計(jì)算從規(guī)劃起點(diǎn)到每一縱向采樣間隔的最小代價(jià)，同時(shí)記錄該結(jié)點(diǎn)的前一個(gè)結(jié)點(diǎn)的橫向位置信息，假如從起點(diǎn)到第j個(gè)縱向采樣間隔的最小代價(jià)為a，從第j個(gè)到j(luò)+1個(gè)的最小代價(jià)為b，則從起點(diǎn)到j(luò)+1個(gè)的最小代價(jià)為a+b，以此類推，直到終點(diǎn)；從終點(diǎn)逆向?qū)ふ宜拿恳粋€(gè)父節(jié)點(diǎn)從而得到最短路徑，形成一個(gè)可行解區(qū)域記為凸空間；

6、步驟五，根據(jù)動(dòng)態(tài)規(guī)劃開辟的凸空間，二次規(guī)劃在凸空間中找到最優(yōu)解，以生成車輛在frenet坐標(biāo)下的最優(yōu)路徑；

7、步驟六，對(duì)路徑規(guī)劃得到得路徑點(diǎn)作進(jìn)一步的速度方面的動(dòng)態(tài)規(guī)劃和二次規(guī)劃。最終生成一條安全、平滑、舒適的避障路線；

8、步驟七、將避障路線上的坐標(biāo)點(diǎn)從frenet坐標(biāo)系轉(zhuǎn)換為cartesian坐標(biāo)系，發(fā)送到控制模塊進(jìn)行執(zhí)行。

9、進(jìn)一步，所述步驟一中，還包括：

10、其一，不妨將參考線上相鄰的三個(gè)離散點(diǎn)按縱向順序分別記為p1r、p2r和p3r，對(duì)應(yīng)的平滑后的三個(gè)點(diǎn)分別記為p1、p2和p3，若p1r、p2r和p3r三個(gè)點(diǎn)不共線，則必然可以支成一個(gè)平行四邊形，記平行四邊形的對(duì)角線的模長(zhǎng)為|p2rp0|，該模長(zhǎng)越小，則參考線越平滑；其二，點(diǎn)p1r到點(diǎn)p1的模長(zhǎng)|p1rp1|加上點(diǎn)p2r到點(diǎn)p2的模長(zhǎng)|p2rp2|再加上點(diǎn)p3r到點(diǎn)p3的模長(zhǎng)|p3rp3|的總和越小，則平滑后的路徑越接近原幾何路徑；其三，對(duì)于平滑后的點(diǎn)，p1點(diǎn)到p2點(diǎn)的模長(zhǎng)的平方|p1p2|2加上p2點(diǎn)到p3點(diǎn)的模長(zhǎng)的平方|p2p3|2越小，則平滑后的點(diǎn)越緊湊越均勻?？偟钠交拇鷥r(jià)函數(shù)公式為：

11、

12、

13、其中，xi為對(duì)應(yīng)的未平滑的點(diǎn)xir平滑后的橫坐標(biāo)，yi為對(duì)應(yīng)的未平滑的點(diǎn)yir平滑后的縱坐標(biāo)，此坐標(biāo)是在cartesian坐標(biāo)系下的坐標(biāo)，w1為平滑后的參考線上的點(diǎn)與原路徑點(diǎn)的相似代價(jià)，w2為平滑代價(jià)，w3為緊湊代價(jià)。

14、上述代價(jià)函數(shù)最終可以化簡(jiǎn)成二次規(guī)劃的函數(shù)形式，最終的化簡(jiǎn)公式如下：

15、

16、其中，a1為相似代價(jià)的系數(shù)矩陣，a2為平滑代價(jià)的系數(shù)矩陣，a3為緊湊代價(jià)的系數(shù)矩陣，該系統(tǒng)矩陣都是己知的；最終代價(jià)函數(shù)可以簡(jiǎn)化成二次規(guī)劃的標(biāo)準(zhǔn)形式，

17、

18、lb≤x≤ub

19、利用二次規(guī)劃求解器找到使代價(jià)函數(shù)最小值的最優(yōu)解x。

20、進(jìn)一步，所述步驟二中，還包括：

21、從所述參考線上的起始點(diǎn)開始遍歷參考線上的每一個(gè)離散點(diǎn)，根據(jù)所述參考線上的每一個(gè)離散點(diǎn)到車輛當(dāng)前位置的距離，從所述參考線上的離散點(diǎn)中找到距離車輛當(dāng)前位置距離最小的點(diǎn)記為最小匹配點(diǎn)pm，從cartesian坐標(biāo)系原點(diǎn)到pm的位矢記為從點(diǎn)pm到車輛當(dāng)前位置的位矢記為點(diǎn)pm處的方向向量記為則當(dāng)前車輛在cartesian坐標(biāo)系下的理想投影點(diǎn)的位矢為：

22、

23、根據(jù)最小匹配點(diǎn)pm處的曲率km和傾斜角θm，θm為該點(diǎn)的切線方向與cartesian坐標(biāo)系下x軸的夾角，可以計(jì)算出理想投影點(diǎn)處的曲率kr和傾斜角θr，當(dāng)離散點(diǎn)之間的距離不太大時(shí)，可近似認(rèn)為km等于kr，理想投影點(diǎn)的傾斜角θr的計(jì)算公式為：

24、

25、至此，cartesian坐標(biāo)系下代表車輛位置的點(diǎn)在參考線上的的投影點(diǎn)的信息：橫坐標(biāo)、縱坐標(biāo)、傾斜角和曲率均己求得，障礙物在參考線上的理想投影點(diǎn)的計(jì)算公式同理。

26、進(jìn)一步，所述步驟三中，還包括：

27、根據(jù)所述步驟二中理想投影點(diǎn)的信息，計(jì)算車輛在frenet坐標(biāo)系下的坐標(biāo)信息。記車輛在frenet坐標(biāo)系下的縱坐標(biāo)為s，車輛在frenet坐標(biāo)系下的縱坐標(biāo)對(duì)時(shí)間的一階導(dǎo)數(shù)為車輛在frenet坐標(biāo)系下的縱坐標(biāo)對(duì)時(shí)間的二階導(dǎo)數(shù)為車輛在frenet坐標(biāo)系下的橫坐標(biāo)為l，車輛在frenet坐標(biāo)下的橫坐標(biāo)對(duì)弧長(zhǎng)ds的導(dǎo)數(shù)為l′，車輛在frenet坐標(biāo)下的橫坐標(biāo)對(duì)弧長(zhǎng)ds的二階導(dǎo)數(shù)為l″，車輛在frenet坐標(biāo)下的橫坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)為車輛在frenet坐標(biāo)下的橫坐標(biāo)對(duì)時(shí)間的二階導(dǎo)數(shù)為其計(jì)算公式分別如下：

28、s＝sr

29、

30、其中，sr為車輛在參考線上的理想投影點(diǎn)到規(guī)劃起點(diǎn)的距離；為車輛在cartesian坐標(biāo)系下的位置相對(duì)于坐標(biāo)原點(diǎn)的位矢；為車輛在cartesian坐標(biāo)系下的速度向量；為車輛在cartesian坐標(biāo)系下的加速度向量；為投影位矢；kr為投影位矢相對(duì)于cartesian坐標(biāo)系下在道路幾何上的曲率；為投影位矢相對(duì)于cartesian坐標(biāo)系下在道路幾何上的切線向量；為投影位矢相對(duì)于cartesian坐標(biāo)系下在道路幾何上的法線向量；障礙物在cartesian坐標(biāo)系下的坐標(biāo)與frenet坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換與之同理。

31、根據(jù)靜態(tài)障礙物在frenet坐標(biāo)下的縱坐標(biāo)s和橫坐標(biāo)l，建立靜態(tài)障礙物的s-l圖；根據(jù)動(dòng)態(tài)障礙物在frenet坐標(biāo)系下的縱坐標(biāo)s和橫坐標(biāo)l以及縱坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)和橫坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)建立動(dòng)態(tài)障礙物的s-t圖。

32、進(jìn)一步，所述步驟四中，還包括：

33、對(duì)于相鄰的采樣點(diǎn)之間的路徑使用五次多項(xiàng)式生成的軌跡連接，在frenet坐標(biāo)系下的五次多項(xiàng)式公式如下：

34、l＝a0+a1s+a2s2+a3s3+a4s4+a5s5

35、l′＝a1+2a2s+3a3s2+4a4s3+5a5s4

36、l″＝2a2+6a3s+12a4s2+20a5s3

37、以frenet坐標(biāo)軸的縱向距離s作為五次多項(xiàng)式的自變量，當(dāng)相鄰兩結(jié)點(diǎn)的橫向狀態(tài)，如l、l′、l″均己知時(shí)，可以求得五次多項(xiàng)式的系數(shù)矩陣，當(dāng)系數(shù)矩陣求得時(shí)，則相鄰兩結(jié)點(diǎn)隨著縱向s連續(xù)變化的橫向狀態(tài)也均可求得，即兩個(gè)相鄰結(jié)點(diǎn)的連接軌跡，假設(shè)位于start_s處的橫向狀態(tài)分別為start_l、start_dl、startddl，位于end_s處的橫向狀態(tài)分別為end_l、end_dl、end_ddl，則五次多項(xiàng)式的自變量矩陣a和己知的狀態(tài)矩陣b分別為：

38、

39、b＝[start_l，start_dl，start_ddl，end_l，end_dl，end_ddl]t

40、令待求解的系數(shù)矩陣為coff，則coff的形式如下：

41、coff＝[a0，a1，a2，a3，a4，a5]t

42、則五次多項(xiàng)式的矩陣表達(dá)形式為：

43、b＝a×coff

44、根據(jù)車輛在frenet坐標(biāo)系中的運(yùn)動(dòng)初始狀態(tài)和運(yùn)動(dòng)結(jié)束狀態(tài)，即b矩陣和a矩陣是己知的，可求得五次多項(xiàng)式的系數(shù)矩陣，根據(jù)完備的五次多項(xiàng)式，可以求得隨著縱向采樣頻率的變化，對(duì)應(yīng)的橫向狀態(tài)end_l、end_dl、end_ddl。

45、當(dāng)相鄰結(jié)點(diǎn)的軌跡己知時(shí)，需要評(píng)價(jià)該軌跡的代價(jià)，響應(yīng)的代價(jià)函數(shù)分別有軌跡平滑代價(jià)、障礙物碰撞代價(jià)和參考線的距離代價(jià)，分別記為costsmooth、costcollision和costref，其計(jì)算公式分別如下：

46、costsmooth＝wdl·(l′t·l′)+wddl·(l″t·l″)+wdddl·(l″′t·l″′)

47、

48、costref＝w_ref·(lt·l)

49、其中，l′為frenet坐標(biāo)系下的橫坐標(biāo)對(duì)縱坐標(biāo)的一階導(dǎo)數(shù)，l″為frenet坐標(biāo)系下的橫坐標(biāo)對(duì)縱坐標(biāo)的二階導(dǎo)數(shù)，l″′為frenet坐標(biāo)系下的橫坐標(biāo)對(duì)縱坐標(biāo)的三階導(dǎo)數(shù)，wdl、wddl和wdddl分別為其對(duì)應(yīng)的權(quán)重系數(shù)；minl為車輛到障礙物的距離，wcollision為碰撞代價(jià)權(quán)重系數(shù)；l為frenet坐標(biāo)系下的橫坐標(biāo)，w_ref為與參考線距離的代價(jià)函數(shù)權(quán)重系數(shù)。總的代價(jià)函數(shù)為三者之和，即：

50、cost＝costsmooth+costcollison+costref

51、經(jīng)過相鄰結(jié)點(diǎn)之間的代價(jià)函數(shù)的計(jì)算，得到相鄰結(jié)點(diǎn)之間的代價(jià)，隨后從采樣起點(diǎn)到采樣終點(diǎn)進(jìn)行動(dòng)態(tài)規(guī)劃。

52、己知各結(jié)點(diǎn)之間的代價(jià)，找到一條從采樣起點(diǎn)到采樣終點(diǎn)代價(jià)之和的最小的路徑，作為動(dòng)態(tài)規(guī)劃的結(jié)果。動(dòng)態(tài)規(guī)劃的步驟如下：

53、首先，遍歷第一列結(jié)點(diǎn)，找到一條從起點(diǎn)到第一列結(jié)點(diǎn)的最短代價(jià)路徑，并記錄最短路徑上當(dāng)前結(jié)點(diǎn)的父結(jié)點(diǎn)；其次，外層循環(huán)遍歷第二列結(jié)點(diǎn)，內(nèi)層循環(huán)遍歷前一列結(jié)點(diǎn)，找到從起點(diǎn)到第二列的最短代價(jià)路徑，并記錄當(dāng)前結(jié)點(diǎn)的父結(jié)點(diǎn)；依次類推，直到最后遍歷到最后一列結(jié)點(diǎn)，找到從起點(diǎn)到最后一列結(jié)點(diǎn)的最短代價(jià)路徑，并依次索引父結(jié)點(diǎn)，找到中間路徑結(jié)點(diǎn)。

54、相鄰結(jié)點(diǎn)之間的軌跡用五次多項(xiàng)式連接，動(dòng)態(tài)規(guī)劃的結(jié)果對(duì)于相鄰結(jié)點(diǎn)而言有一個(gè)直線連接，但是對(duì)于整體的軌跡而言并不平滑。因此，動(dòng)態(tài)規(guī)劃的結(jié)果僅能作為最優(yōu)軌跡的一個(gè)粗解，最優(yōu)軌跡需要在動(dòng)態(tài)規(guī)劃的基礎(chǔ)上進(jìn)行二次規(guī)劃得到。

55、進(jìn)一步，所述步驟五中，還包括：

56、二次規(guī)劃對(duì)動(dòng)態(tài)規(guī)劃得到的l、l′、l″進(jìn)行進(jìn)一步優(yōu)化，其主要的代價(jià)函數(shù)如下：

57、

58、其中，wref為車輛所在的frenet坐標(biāo)系下橫坐標(biāo)位置到參考線距離的代價(jià)權(quán)重，wdl為車輛所在的frenet坐標(biāo)系下橫坐標(biāo)的一階導(dǎo)數(shù)的代價(jià)權(quán)重，wddl為車輛所在的frenet坐標(biāo)系下橫坐標(biāo)的二階導(dǎo)數(shù)的代價(jià)權(quán)重，wdddl為車輛所在的frenet坐標(biāo)系下橫坐標(biāo)的三階導(dǎo)數(shù)的代價(jià)權(quán)重，wmid為車輛所在凸空間中間的代價(jià)權(quán)重。代價(jià)函數(shù)的值越小，說(shuō)明車輛的軌跡越平滑，且車輛位置越靠近凸空間中央。

59、二次規(guī)劃的代價(jià)函數(shù)最終可轉(zhuǎn)化為二次規(guī)劃的標(biāo)準(zhǔn)形式，其二次規(guī)劃的標(biāo)準(zhǔn)形式如下：

60、xthx+ftx

61、其中，需要滿足三種約束，分別是二階連續(xù)性約束、碰撞約束和規(guī)劃起點(diǎn)約束，函數(shù)形式分別如下：

62、aeqx＝beq

63、ax≤b

64、lb≤x≤ub

65、aeq為二階連續(xù)性約束矩陣，a為碰撞約束矩陣，lb和ub為凸空間的邊界范圍，最后求解的x矩陣為：

66、x＝[l1，l′1，l″1，...，ln，l′n，l″n]t

67、x矩陣即為二次規(guī)劃優(yōu)化后的車輛在frenet坐標(biāo)系下的橫坐標(biāo)，橫坐標(biāo)對(duì)縱坐標(biāo)的一階導(dǎo)數(shù)以及橫坐標(biāo)對(duì)縱坐標(biāo)的二階導(dǎo)數(shù)。

68、進(jìn)一步，所述步驟六中，還包括：

69、獲取從路徑規(guī)劃模塊己經(jīng)計(jì)算好的參考路徑，這條路徑是車輛在當(dāng)前環(huán)境下應(yīng)當(dāng)行駛的理想路線。將參考路徑按照一定的間隔進(jìn)行采樣，以生成多個(gè)離散的路徑點(diǎn)，這些路徑點(diǎn)將用于速度規(guī)劃。

70、計(jì)算障礙物的s-t圖(時(shí)空?qǐng)D)，時(shí)空?qǐng)D是一種以時(shí)間(t)為橫軸，距離(s)為縱軸的二維圖表，圖中的每個(gè)點(diǎn)表示在某一時(shí)間點(diǎn)上，車輛可以處于某一距離位置。將環(huán)境中的靜態(tài)和動(dòng)態(tài)障礙物投影到時(shí)空?qǐng)D上，靜態(tài)障礙物通常在某一個(gè)位置上形成一條豎直線，而動(dòng)態(tài)障礙物則根據(jù)其具有的速度在時(shí)空?qǐng)D上形成一個(gè)平行四邊形區(qū)域。

71、設(shè)定目標(biāo)路徑點(diǎn)速度，根據(jù)參考路徑以及交通規(guī)則(例如限速等)和障礙物的位置和速度，動(dòng)態(tài)調(diào)整目標(biāo)速度曲線。例如，如果前方有車輛減速，則需要調(diào)整本車的速度曲線以避免相撞。

72、設(shè)計(jì)代價(jià)函數(shù)，用于評(píng)估每一條可能的速度曲線，其代價(jià)函數(shù)主要包含車輛與障礙物的距離代價(jià)、加速度代價(jià)、推薦速度代價(jià)和舒適度代價(jià)。通過動(dòng)態(tài)規(guī)劃從所有可能的速度曲線中選擇一條代價(jià)成本最低的一條作為最優(yōu)速度曲線。其動(dòng)態(tài)規(guī)劃流程與步驟四相似。

73、通過二次規(guī)劃對(duì)初步生成的速度曲線進(jìn)行平滑處理，以減少突變或不連續(xù)的速度變化。在平滑過程中，確保曲線滿足一定的約束條件，如加速度、加加速度不能超出車輛的物理極限。

74、進(jìn)一步，所述步驟七中，還包括：

75、經(jīng)過速度規(guī)劃找到平滑軌跡的最優(yōu)解，此時(shí)己經(jīng)得到frenet坐標(biāo)系下的行駛軌跡點(diǎn)的橫坐標(biāo)l和縱坐標(biāo)s的相關(guān)信息，要想發(fā)送到控制模塊控制執(zhí)行規(guī)劃的軌跡，還需將frenet坐標(biāo)系下的軌跡點(diǎn)轉(zhuǎn)化到cartesian坐標(biāo)系，其轉(zhuǎn)換公式如下：

76、xx＝xr-l?sinθr

77、yx＝y(tǒng)r-l?cosθr

78、

79、其中，xx和yx為從frenet坐標(biāo)系轉(zhuǎn)換到cartesian坐標(biāo)系下的橫坐標(biāo)和縱坐標(biāo)；θx為車輛在cartesian坐標(biāo)系下的速度方向與x軸的夾角；vx為車輛在cartesian坐標(biāo)系下的線速度；ax為車輛在cartesian坐標(biāo)系下的加速度；kx為車輛在cartesian坐標(biāo)系下的曲率；θr為frenet坐標(biāo)系下理想投影點(diǎn)的切線向量與cartesian坐標(biāo)軸的x軸的夾角。路徑規(guī)劃的結(jié)果是一系列離散點(diǎn)，每個(gè)點(diǎn)具有以上所述的信息，這一系列離散點(diǎn)組成的路徑即為路徑規(guī)劃得到的理想路線。

80、最終，將速度曲線轉(zhuǎn)化為控制命令，傳遞給車輛的執(zhí)行模塊，控制車輛的油門、剎車等。隨著車輛的移動(dòng)和環(huán)境的變化，速度規(guī)劃也需要不斷更新，以應(yīng)對(duì)新出現(xiàn)的障礙物或變化的道路狀況。

81、通過這些步驟，可以為自動(dòng)駕駛車輛生成一條安全、平穩(wěn)且高效的規(guī)劃曲線，確保車輛在各種環(huán)境下能夠正常行駛。