本發(fā)明涉及智能交通管控，具體而言，涉及一種基于可插入間隙的高速公路車輛匝道匯入決策方法及系統(tǒng)。

背景技術(shù)：

1、高速公路匝道匯入場(chǎng)景為內(nèi)側(cè)設(shè)置有自動(dòng)駕駛專用道和匝道的高速公路駕駛場(chǎng)景，其特征為：一、由于存在自動(dòng)駕駛專用道，默認(rèn)自動(dòng)駕駛車全部行駛于高速公路主路的內(nèi)側(cè)車道上，而高速公路主路的外側(cè)車道上只駕駛有人工駕駛車輛；二、匯入車道上會(huì)同時(shí)存在人工駕駛車輛和自動(dòng)駕駛車輛，即對(duì)于自動(dòng)駕駛車輛，匝道上行駛的前車和后車不一定能夠配合操作；三、受到人工駕駛車不具備通訊能力的限制，使得自動(dòng)駕駛車輛的規(guī)劃方案不能與環(huán)境車輛進(jìn)行通訊，僅可以通過與路測(cè)單元通訊獲得環(huán)境車輛的歷史狀態(tài)信息和當(dāng)前狀態(tài)信息。

2、由于高速公路車輛匝道匯入場(chǎng)景具有上述特征，至少存在以下問題：

3、(1)高速公路主路外側(cè)車道上人工駕駛車輛形成的間隙選擇問題。

4、(2)車輛從高速公路匝道匯入切入過程中的軌跡規(guī)劃問題。

5、對(duì)于第一個(gè)問題，間隙的選擇顯著影響車輛換道的效率與成功率，進(jìn)而影響整個(gè)交通流的效率。目前，針對(duì)間隙選擇的研究多集中于全部智能網(wǎng)聯(lián)環(huán)境下的間隙選擇，該情況下環(huán)境車輛可以主動(dòng)配合換道，不存在主路和匝道車輛相互博弈的問題；鮮少研究討論了自動(dòng)駕駛車輛面對(duì)人工駕駛環(huán)境時(shí)間隙的選擇，且忽略了人工駕駛車在面對(duì)主車切入時(shí)的駕駛行為，更多是基于絕對(duì)安全考慮間隙能不能被選擇而并非間隙選擇是否最優(yōu)的問題，反而對(duì)交通流效率帶來負(fù)面影響。

6、對(duì)于第二個(gè)問題，當(dāng)完成間隙選擇后主車進(jìn)行軌跡規(guī)劃完成切入。目前，在求解軌跡規(guī)劃問題時(shí)，大部分是基于先采樣后優(yōu)化再評(píng)估的方式將問題簡(jiǎn)化后進(jìn)行軌跡求解。該方法存在顯著問題：若采樣數(shù)量少，則特征數(shù)據(jù)間隔大，求解精度低；反之若進(jìn)行精細(xì)采樣，那么將存在多個(gè)并列的待求解子問題，求解效率低下，盡管并行計(jì)算能夠一定程度上解決這個(gè)問題，但對(duì)于單車智能來說將會(huì)極大提升成本。

7、因此，亟需提出一種基于可插入間隙的高速公路車輛匝道匯入決策方法及系統(tǒng)，平衡軌跡規(guī)劃計(jì)算精度與效率，實(shí)現(xiàn)車輛從匝道匯入高速公路外側(cè)車道時(shí)的可插入間隙識(shí)別、選取和軌跡規(guī)劃的精確控制。

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

1、本發(fā)明的目的在于提供一種基于可插入間隙的高速公路車輛匝道匯入決策方法，旨在至少解決上述現(xiàn)有技術(shù)存在的技術(shù)問題之一，為實(shí)現(xiàn)上述目的，其采用的技術(shù)解決方案如下：

2、基于可插入間隙的高速公路車輛匝道匯入決策方法，包括以下步驟：

3、獲取高速公路車輛匝道匯入場(chǎng)景中主路外側(cè)車道上的車輛，以及待匯入車道的主車的車輛信息，其中，所述主車為待規(guī)劃自動(dòng)駕駛車輛，所述車輛信息包括當(dāng)前車輛的位移，速度和加速度；

4、基于所述主車的車輛信息判斷所述主車是否到達(dá)決策規(guī)劃觸發(fā)點(diǎn)；

5、當(dāng)所述主車到達(dá)所述決策規(guī)劃觸發(fā)點(diǎn)時(shí)，迭代執(zhí)行第一階段軌跡規(guī)劃，直至所述主車滿足切入條件，控制主車完成從匯入車道切入匝道口前的縱向行駛，其中，所述切入條件基于所述主車的可插入間隙設(shè)置；

6、迭代執(zhí)行第二階段軌跡規(guī)劃，基于所述第二階段軌跡規(guī)劃控制所述主車完成匝道匯入，其中，所述第二階段軌跡規(guī)劃為確定所述主車切入匝道口時(shí)的二維軌跡規(guī)劃方案；

7、其中，所述第一階段軌跡規(guī)劃和所述第二階段軌跡規(guī)劃分別基于mpc框架執(zhí)行。

8、優(yōu)選地，所述當(dāng)所述主車到達(dá)所述決策規(guī)劃觸發(fā)點(diǎn)時(shí)，迭代執(zhí)行第一階段軌跡規(guī)劃包括：

9、在mpc框架下，迭代規(guī)劃預(yù)設(shè)時(shí)間周期內(nèi)的主車行駛軌跡，包括：

10、基于所述主路外側(cè)車道上的車輛，以及待匯入車道的主車的車輛信息確定預(yù)設(shè)時(shí)間周期內(nèi)主車的可達(dá)范圍，識(shí)別可插入間隙；

11、基于預(yù)構(gòu)建的第一milp問題計(jì)算主車最佳插入間隙、插入時(shí)狀態(tài)以及插入前行車軌跡；

12、確定所述第一milp問題是否有可行解，若存在可行解，則將計(jì)算得到的插入前行車軌跡設(shè)置為第一階段軌跡規(guī)劃方案，執(zhí)行第一階段軌跡規(guī)劃方案并更新時(shí)間步，以規(guī)劃下一輪預(yù)設(shè)時(shí)間周期的主車行駛軌跡；若不存在可行解，則控制所述主車減速并更新時(shí)間步，以規(guī)劃下一輪預(yù)設(shè)時(shí)間周期的主車行駛軌跡；

13、其中，預(yù)構(gòu)建第一milp問題包括基于最小化交通影響和最少換道時(shí)間設(shè)置第一階段軌跡規(guī)劃方案的目標(biāo)函數(shù)，結(jié)合車輛動(dòng)力學(xué)方程、運(yùn)動(dòng)學(xué)方程、切入時(shí)安全指標(biāo)閾值范圍、車輛跟馳行為、匝道處加速長(zhǎng)度設(shè)置約束條件，建立第一milp問題。

14、優(yōu)選地，所述迭代執(zhí)行第二階段軌跡規(guī)劃包括：

15、基于預(yù)構(gòu)建的第二階段軌跡規(guī)劃模型，確定主車的縱向位移終止?fàn)顟B(tài)方程式和橫向位移終止?fàn)顟B(tài)方程式；

16、建立第二階段優(yōu)化問題，在mpc框架下，基于所述第二階段優(yōu)化問題對(duì)所述主車的縱向位移終止?fàn)顟B(tài)方程式和橫向位移終止?fàn)顟B(tài)方程式進(jìn)行迭代求解，獲取主車在每個(gè)規(guī)劃周期內(nèi)的縱向位移終止?fàn)顟B(tài)參數(shù)和橫向位移終止?fàn)顟B(tài)參數(shù)，直至所述主車切入完成，包括：

17、對(duì)tend在可行的設(shè)定范圍內(nèi)進(jìn)行均勻采樣生成若干參考；

18、以當(dāng)前時(shí)間步的環(huán)境車輛信息與每一個(gè)參考tend為輸入，基于所述mpc框架對(duì)預(yù)構(gòu)建的第二階段軌跡規(guī)劃模型進(jìn)行迭代求解，求得每個(gè)參考tend對(duì)應(yīng)的最佳軌跡，基于所述每個(gè)參考tend對(duì)應(yīng)的最佳軌跡形成軌跡簇；

19、根據(jù)所述第二階段優(yōu)化問題中的目標(biāo)函數(shù)，評(píng)估所述軌跡簇中每個(gè)軌跡的目標(biāo)函數(shù)值，基于所述目標(biāo)函數(shù)值對(duì)所述軌跡簇進(jìn)行篩選，基于篩選結(jié)果確定第二階段的執(zhí)行軌跡，所述執(zhí)行軌跡包括主車在每個(gè)規(guī)劃周期內(nèi)的縱向位移終止?fàn)顟B(tài)參數(shù)和橫向位移終止?fàn)顟B(tài)參數(shù)，

20、其中，所述第二階段優(yōu)化問題基于時(shí)變風(fēng)險(xiǎn)場(chǎng)模型建立，建立方法包括：

21、以最小化換道時(shí)間、舒適度和風(fēng)險(xiǎn)場(chǎng)強(qiáng)度為目標(biāo)確定目標(biāo)函數(shù)，結(jié)合車輛動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)、道路限速約束設(shè)置約束條件，構(gòu)建所述第二階段優(yōu)化問題。

22、優(yōu)選地，所述高速公路車輛匝道匯入場(chǎng)景用于模擬主車經(jīng)匝道匯入高速公路外側(cè)車道的過程，所述高速公路車輛匝道匯入場(chǎng)景包括主路和匯入車道，其中，所述主路中設(shè)置有多條車道，主路外側(cè)車道上行駛車輛均為人工駕駛車輛，所述匯入車道位于主路外側(cè)車道一側(cè)，且匯入車道與主路外側(cè)車道的匯入?yún)^(qū)域?yàn)樵训揽冢鰠R入車道上行駛有自動(dòng)駕駛車輛和人工駕駛車輛。

23、優(yōu)選地，所述主路外側(cè)車道上的車輛信息為其中，si＝[xi，vi，ai]，i∈{1，2，..，nm}，i為人工駕駛車輛的序號(hào)，nm為外側(cè)車道上車輛總數(shù)，si為第i輛車的車輛信息，xi為第i輛車的位移，vi為第i輛車的速度，ai為第i輛車的加速度；

24、所述主車的車輛信息為sego＝[xego，vego，aego]，其中，xego為主車的位移，vego為主車的速度，aego為主車的加速度；

25、將可插入間隙定義為前方車輛與后方車輛之間的間隙，根據(jù)可插入間隙的長(zhǎng)度確定切入條件，所述可插入間隙的長(zhǎng)度根據(jù)主車可達(dá)范圍確定，其中，所述主車的最大可達(dá)位移為：

26、

27、式中，vub為最大限速；aub為最大加速度限制；t為預(yù)設(shè)的時(shí)間周期；

28、所述主車的最小可達(dá)位移為：

29、

30、式中，vlb為最小限速；alb為最小加速度限制；

31、基于所述主車可達(dá)范圍確定可插入間隙為：

32、

33、式中，為預(yù)設(shè)的時(shí)間周期末外側(cè)車道上第i輛人工駕駛車輛可達(dá)位置，l為車輛長(zhǎng)度；d為相鄰車輛之間的安全距離。

34、優(yōu)選地，所述預(yù)設(shè)的時(shí)間周期被等間隔離散為多個(gè)時(shí)間步t，每個(gè)時(shí)間步內(nèi)獲取主車和高速公路上其他行駛車輛的車輛信息，結(jié)合車輛動(dòng)力學(xué)方程、運(yùn)動(dòng)學(xué)方程、切入時(shí)安全指標(biāo)閾值范圍、車輛跟馳行為、匝道處加速長(zhǎng)度設(shè)置約束條件，建立所述第一milp問題；

35、所述第一milp問題表示為：

36、

37、式中，minj為所述第一milp問題的目標(biāo)函數(shù)；ω1、ω2、ω3均為權(quán)重系數(shù)；t為時(shí)間步序號(hào)；at為t時(shí)間步時(shí)的加速度，vt為t時(shí)間步時(shí)的速度，st為t時(shí)間步時(shí)的車輛信息，是包括位置、速度、加速度的向量；at+1為t+1時(shí)間步時(shí)的加速度，st+1為t+1時(shí)間步時(shí)的車輛信息；為時(shí)間步為t時(shí)第i輛人工駕駛車輛的速度，為時(shí)間步為t時(shí)第i-1輛人工駕駛車輛的速度；為時(shí)間步為t時(shí)第i輛人工駕駛車輛的加速度，為時(shí)間步為t+1時(shí)第i輛人工駕駛車輛的加速度；均為用于線性化的中間變量；為時(shí)間步t時(shí)的零一變量，為時(shí)間步t+1時(shí)的零一變量，所述零一變量用于表示車輛是否已執(zhí)行換道，其中，車輛換道前車輛換道后m為一個(gè)預(yù)設(shè)參數(shù)；為時(shí)間步為t時(shí)第i輛人工駕駛車輛的中間變量，為時(shí)間步為t+1時(shí)第i輛人工駕駛車輛的中間變量；s0為在0時(shí)刻的主車車輛信息；sinit為初始的主車車輛信息；為時(shí)間步t時(shí)主車位移的縱坐標(biāo)，為時(shí)間步t時(shí)主車的匯入指標(biāo)變量，為時(shí)間步t時(shí)主車的速度；lacc為匝道口內(nèi)加速區(qū)長(zhǎng)度；nac為milp問題所轉(zhuǎn)化線性規(guī)劃問題的個(gè)數(shù)；γi,i+1為車輛i與車輛i+1之間的可插入間隙，γi-1，i為車輛i-1與車輛i之間的可插入間隙，為時(shí)間步t時(shí)車輛i-1與車輛i之間的可插入間隙；為時(shí)間步為t時(shí)第i輛人工駕駛車輛的位移，為時(shí)間步為t時(shí)第i-1輛人工駕駛車輛的位移；∈ttc為ttc風(fēng)險(xiǎn)指標(biāo)的閾值；δt為時(shí)間間隔；為時(shí)間步為t+1時(shí)第i輛車對(duì)前車跟馳行為計(jì)算出的加速度；為時(shí)間步為t+1時(shí)第i輛車對(duì)切入車輛跟馳行為計(jì)算出的加速度。

38、優(yōu)選地，預(yù)構(gòu)建的第二階段軌跡規(guī)劃模型為二維軌跡，表示為：

39、

40、式中，tm為在切入過程中的時(shí)刻；x(·)為二維軌跡的縱向位置；y(·)為二維軌跡的橫向位置；p＝[p0，p1，p2，p3，p4，p5]，p0、p1、p2、p3、p4、p5均為縱向位移控制變量；q＝[q0，q1，q2，q3，q4，q5]，q0、q1、q2、q3、q4、q5均為橫向位移控制變量；t＝[1，tm，tm2，tm3，tm4，tm5]；

41、基于主車在x軸方向和y軸方向上的運(yùn)動(dòng)，主車的初始狀態(tài)和終止?fàn)顟B(tài)滿足條件：

42、

43、式中，t0為第二階段軌跡規(guī)劃開始時(shí)間；主車的初始狀態(tài)為其中，x0為初始縱坐標(biāo)，為初始縱向速度，為初始縱向加速度；y0為初始橫坐標(biāo)，為初始橫向速度，為初始橫向加速度；主車的終止?fàn)顟B(tài)為其中，xend為終止縱坐標(biāo)，為終止速度，為終止加速度，tend為終止時(shí)間；ytar為終止橫坐標(biāo)；

44、確定主車的縱向位移終止?fàn)顟B(tài)方程式為：

45、

46、確定主車的橫向位移終止?fàn)顟B(tài)方程式為：

47、

48、優(yōu)選地，所述第二milp問題中的目標(biāo)函數(shù)設(shè)置為：

49、minj＝ω1×jacc+ω2×jjerk+ω3×tend+ω4×jrisk

50、其中，

51、

52、式中，ω1、ω2、ω3、ω4均為權(quán)重系數(shù)；jacc為加速度對(duì)舒適度的影響參數(shù)；jjerk為加速度變化率對(duì)舒適度的影響參數(shù)；jrisk為環(huán)境風(fēng)險(xiǎn)場(chǎng)；

53、所述環(huán)境風(fēng)險(xiǎn)場(chǎng)jrisk為：

54、jrisk＝ufront+ubehind

55、其中，

56、

57、ubehind＝k×usta+(1-k)×umov；

58、

59、式中，t″為t的二階導(dǎo)數(shù)；t″′為t的三階導(dǎo)數(shù)；ufront為前車風(fēng)險(xiǎn)場(chǎng)；xfront為與前車之間的距離；σx為縱軸方向控制系數(shù)；σy為橫軸方向控制系數(shù)；a為場(chǎng)強(qiáng)振幅；β為風(fēng)險(xiǎn)場(chǎng)形態(tài)控制參數(shù)；ubehind為后車風(fēng)險(xiǎn)場(chǎng)強(qiáng)度；k為權(quán)重系數(shù)；usta為靜態(tài)風(fēng)險(xiǎn)場(chǎng)強(qiáng)度；umov為動(dòng)態(tài)風(fēng)險(xiǎn)場(chǎng)強(qiáng)度；exp(·)為指數(shù)函數(shù)；

60、其中，沿x軸方向的速度、加速度以及橫向加速度基于車輛動(dòng)力學(xué)設(shè)置約束條件如下：

61、vlb≤p·t′≤vub

62、alb≤p·t″≤aub

63、-λ≤q·t″≤λ

64、式中，f′為t的一階導(dǎo)數(shù)；λ為向心加速度的限制閾值。

65、本發(fā)明還提出了基于可插入間隙的高速公路車輛匝道匯入決策系統(tǒng)，應(yīng)用于如權(quán)利要求1-6中任一項(xiàng)所述的基于可插入間隙的高速公路車輛匝道匯入決策方法，所述決策系統(tǒng)包括：車輛信息獲取設(shè)備和服務(wù)器；

66、其中，所述車輛信息獲取設(shè)備用于獲取高速公路車輛匝道匯入場(chǎng)景中主路外側(cè)車道上的車輛，以及待匯入車道的主車的車輛信息，其中，所述主車為待規(guī)劃自動(dòng)駕駛車輛，所述車輛信息包括當(dāng)前車輛的位移，速度和加速度；

67、所述服務(wù)器用于基于所述主車的車輛信息判斷所述主車是否到達(dá)決策規(guī)劃觸發(fā)點(diǎn)；當(dāng)所述主車到達(dá)所述決策規(guī)劃觸發(fā)點(diǎn)時(shí)，迭代執(zhí)行第一階段軌跡規(guī)劃，直至所述主車滿足切入條件時(shí)，基于所述第一階段軌跡規(guī)劃控制所述主車執(zhí)行第一階段軌跡，以控制主車完成從匯入車道切入匝道口前的縱向行駛，其中，所述切入條件包括識(shí)別所述主車具有可插入間隙；以及迭代執(zhí)行第二階段軌跡規(guī)劃，基于所述第二階段軌跡規(guī)劃控制所述主車完成匝道匯入，其中，所述第二階段軌跡規(guī)劃為確定所述主車切入匝道口時(shí)的二維軌跡規(guī)劃方案；其中，所述第一階段軌跡規(guī)劃和所述第二階段軌跡規(guī)劃分別基于mpc框架執(zhí)行；

68、其中，所述車輛信息獲取設(shè)備與所述服務(wù)器通訊連接。

69、優(yōu)選地，所述服務(wù)器包括：

70、判斷模塊，配置為基于所述主車的車輛信息判斷所述主車是否到達(dá)決策規(guī)劃觸發(fā)點(diǎn)；

71、間隙識(shí)別模塊，配置為當(dāng)所述主車到達(dá)所述決策規(guī)劃觸發(fā)點(diǎn)時(shí)，迭代執(zhí)行第一階段軌跡規(guī)劃，直至所述主車滿足切入條件時(shí)，基于所述第一階段軌跡規(guī)劃控制所述主車執(zhí)行第一階段軌跡，以控制主車完成從匯入車道切入匝道口前的縱向行駛，其中，所述切入條件包括識(shí)別所述主車具有可插入間隙；

72、匯入決策模塊，配置為迭代執(zhí)行第二階段軌跡規(guī)劃，基于所述第二階段軌跡規(guī)劃控制所述主車完成匝道匯入，其中，所述第二階段軌跡規(guī)劃為確定所述主車切入匝道口時(shí)的二維軌跡規(guī)劃方案；其中，所述第一階段軌跡規(guī)劃和所述第二階段軌跡規(guī)劃分別基于mpc框架執(zhí)行。

73、與現(xiàn)有技術(shù)相比本發(fā)明具如下有益效果：

74、本發(fā)明一種基于可插入間隙的高速公路車輛匝道匯入決策方法及系統(tǒng)將高速公路車輛切入匝道過程細(xì)分為第一階段和第二階段，通過識(shí)別及選取可插入間隙，分別針對(duì)主車從匯入車道切入匝道口前制定縱向軌跡規(guī)劃方案以及針對(duì)主車切入匝道口時(shí)制定二維軌跡規(guī)劃方案，綜合考慮前后車輛對(duì)自動(dòng)駕駛車輛匝道切入過程的影響，有效解決了高速公路匝道處匯車時(shí)自動(dòng)駕駛車輛換道空間狹小、安全性低、對(duì)交通流影響大的問題，為指導(dǎo)高速公路上自動(dòng)駕駛車輛的匝道切入提供了依據(jù)。

75、同時(shí)，本公開的實(shí)施例的基于可插入間隙的高速公路車輛匝道匯入決策方法，通過優(yōu)化軌跡規(guī)劃方案的計(jì)算過程，在保證軌跡規(guī)劃計(jì)算精度的同時(shí)提高了計(jì)算效率，有效實(shí)現(xiàn)了對(duì)自動(dòng)駕駛車輛軌跡的精確控制，降低了自動(dòng)駕駛車輛的軌跡規(guī)劃成本。