[發明專利]一種自動駕駛交叉口車輛進入時刻與行駛軌跡控制方法

有效

• 申請號: 202110049609.8
• 申請日: 2021-01-14
• 公開（公告）號: CN112373472B
• 公開（公告）日: 2021-04-20
• 發明（設計）人: 劉洋;吳偉;龍科軍
• 申請（專利權）人: 長沙理工大學
• 主分類號: B60W30/14
• 分類號: B60W30/14;B60W40/00;B60W40/105;B60W60/00;G08G1/01;G08G1/052;G08G1/056;G08G1/0967
• 代理公司: 長沙中海宏圖專利代理事務所(普通合伙) 43224
• 代理人: 羅霞

1.一種自動駕駛交叉口車輛進入時刻與行駛軌跡控制方法，其特征在于，該方法包括如下步驟：

步驟1：采集交叉口信息作為控制模型的輸入條件，包括交叉口的類型、大小、各方向進口道和出口道的車道數、車道寬度；采集車輛信息，包括當前時刻車輛所在位置到交叉口停車線的距離s_c，其中c表示車輛，c∈C，C表示車輛的集合，當前時刻車輛的速度，根據s_c對車輛進入交叉口的次序進行排序，車輛按次序以先到先服務的規則依次進入交叉口；

步驟2：在交叉口建立直角坐標系，確定車輛通過進口道和出口道的位置點坐標，根據進口道位置點和出口道位置點，建立方程刻畫車輛在交叉口內部的行車路徑，路徑間的交點為沖突點，通過路徑方程確定各沖突點的坐標，根據距離公式計算每條軌跡上進口道位置點到沖突點的距離；

步驟3：設置速度控制區域，針對每輛車計算速度控制區域的長度，車輛進入速度控制區域后不再變換車道，在速度控制區域內調整車速；

步驟4：將車輛分為有前車和無前車兩類，車輛為每條進口車道第一輛車時，建立控制模型，在滿足沖突點時間制約地條件下，車輛以盡可能大的速度到達沖突點，確定車輛進入交叉口的時間及行駛軌跡，當車輛通過沖突點后更新沖突點的時間；

步驟5：車輛在進口車道上有前車時，前車和沖突點對車輛均有制約，為了確保行車安全，兼顧效率，后車盡可能以前車進入交叉口的時間到達交叉口，且車速不能大于前車車速，建立控制模型，確定車輛進入交叉口的時間及行駛軌跡，當車輛通過沖突點后更新沖突點的時間。

2.根據權利要求1所述的一種自動駕駛交叉口車輛進入時刻與行駛軌跡控制方法，其特征在于，所述步驟3包括如下步驟：

步驟31：車輛在各自的速度控制區域內調整速度，不換道行駛；速度控制線外車輛勻速行駛速度為V_M，車輛行駛到速度控制線內減速，速度控制區域為以a_L的最大舒適減速度將速度從V_M減到0，到達停車線或隊尾的距離，用L_c表示，由公式(1)計算：

式中：V_M表示速度控制線外車輛勻速行駛的速度，a_L表示最大舒適減速度，L_c表示速度控制線的長度，c表示車輛，c∈C，C表示車輛集合，當車輛c同一車道上無前車時，L_c從停車線算起，當車輛c同一車道上有前車時，L_c從前車車尾算起；

所述步驟4，包括如下步驟：

步驟41：根據同一車道上車輛是否有前車，將車輛分為兩類，第一類，同一車道上沒有前車，即車輛為當前車道第一輛車時，只有沖突點的時間制約車輛，當車輛到達沖突點的時刻大于等于沖突點的時刻時，車輛能順利通過沖突點，否則，車輛不能順利通過沖突點，需要根據沖突點的時間調整車速或者停車等待；第二類，同一車道上有前車時，在控制車輛行駛軌跡時需要同時考慮前車和沖突點的約束；

步驟42：對于無前車的情況，即車輛為當前車道的第一輛車，只有沖突點制約車輛進入交叉口，根據沖突點的時間調整車速，再計算車輛通過交叉口停車線的時間和速度，為后車控制提供依據；表示第n輛車以V_M的速度勻速行駛到沖突點p_i所花費的時間，由公式(2)計算：

式中：C₁表示各方向進口道第一輛車的集合，L_n，pi表示車輛n從速度控制線到沖突點p_i的距離；

當車輛通過每個沖突點后，更新沖突點的時間，車輛n只能在T_pi^(m)之后通過該沖突點才能確保安全，車輛通過該沖突點的時間要大于等于更新后的沖突點時間，T_pi^(m)表示第m輛車通過沖突點p_i更新后的時間，T_pi^(m)由公式(3)計算：

T_pi^(m)＝T_m，pi+τ (3)

式中：T_m，pi表示第m輛車到達沖突點p_i時刻，τ表示安全時間間隔；

當時，說明車輛n以V_M的速度勻速行駛到沖突點的時刻晚于沖突點的時刻，此時車輛以V_M速度勻速行駛能安全通過沖突點p_i，行駛到達停車線的時間T_n，sl由公式(4)計算，車尾通過停車線的時間用T′_n，sl表示，由公式(5)計算；

式中：d_c表示車輛長度和最小車頭間距之和，L_n，sl表示車輛n速度控制線到停車線的距離；

步驟43：表示第n輛車進入速度控制區域后以a_L的最大舒適減速度從V_M立即減速到v_min后勻速行駛到沖突點p_i花費的時間，由公式(6)計算：

式中：v_min為車輛行駛的最小速度，a_L最大舒適減速度；

當時，表示車輛以最小速度行駛消磨最多的時間，到達沖突點的時刻仍然早于沖突點的時刻，此時如果不停車，則不能安全通過沖突點，車輛n必須在停車線停車等待；車輛以最快速度到達停車線停車，減少對路段上后車的影響，到達停車線的時刻T_n，sl由公式(7)計算：

車輛從停車線以a_U的最大舒適加速度啟動進入交叉口，行駛到沖突點的距離用L_{n，sl，pi}表示，車輛在交叉口等待的時間t_n，w，由公式(8)計算：

式中：t_n，w表示車輛在停車線等待的時間，a_U表示最大舒適加速度，L_n，sl，pi表示車輛n從停車線sl位置點到沖突點p_i的距離，車尾通過停車線的時間T′_n，sl由公式(9)計算：

步驟44：當區間時，車輛n可以通過調整車速，使得車輛盡可能在沖突點時刻T_pi^(m)后通過沖突點，且車輛通過沖突點的速度盡可能接近V_M，速度調整如情況1和情況2所示：

情況1：調整速度后，到達沖突點的時刻等于沖突點時刻T_n，pi＝T_pi^(m)，到達沖突點的車速v_n，pi＝V_M，則可能存在勻速行駛階段，通過公式(10)-(15)計算v′、v_n，pi、各階段行駛結束的時刻T₁、T₂、T₃、T₄、T_n，pi：

-a_L≤a_-，a₊≤a_U (13)

v_min≤v′≤v_n，pi＝V_M (14)

0＜T₁＜T₂≤T₃＜T₄≤T_n，pi＝T_pi^(m) (15)

式中：T₁、T₂、T₃、T₄、T_n，pi表示每個行駛階段結束的時刻，v′為中間變量，表示減速的最終速度，a_-表示減速度大小、a₊表示加速度大小，v_min表示規定車輛在速度調整階段的最小速度，v_n，pi表示到達沖突點的車速；

以速度波動最小為最佳方案，如公式(16)約束：

min(V_M-v′) (16)

最終得到距離與時間的函數和速度與時間的函數如公式(17)-(18)所示：

式中：s_n(t)表示車輛n在t時刻與出發點的距離，v_n(t)表示車輛n在t時刻的速度；

情況2：當車輛進入速度控制區域，通過速度調整，到達沖突點的車速可能達不到V_M，此時車輛在行駛過程中不存在以V_M勻速行駛階段，通過公式(13)、(19)-(23)計算v_n，pi、中間變量v″和每個階段的行駛時間T₆、T_n，pi：

v″＝V_M-a_-·T₆ (20)

v_n，pi＝v″+a₊·(T_n，pi-T₆) (21)

v_min≤v″≤v_n，pi＜V_M (22)

0＜T₆≤T_n，pi＝T_pi^(m) (23)

式中：T₆、T_n，pi表示每個行駛階段結束的時刻，v″為中間變量，表示減速的最終速度，v_n，pi表示到達沖突點的車速；情況2中當T₆＝T_n，pi時，示車輛n沒有勻加速行駛階段；

以車輛到達沖突點的速度最大為目標，滿足公式(24)約束：

min(V_M-v_n，pi) (24)

最終得到距離與時間的函數和速度與時間的函數如公式(25)-(26)所示：

步驟45：根據車輛到達沖突點的速度v_n，pi和時間T_n，pi，以及停車線位置點到沖突點的距離L_n，sl，pi，計算到達停車線的時間T_n，sl、車尾通過停車線的時間T′_n，sl和速度v_n，sl，為后車速度控制提供依據。

3.根據權利要求2所述的一種自動駕駛交叉口車輛進入時刻與行駛軌跡控制方法，其特征在于，所述步驟5包括如下步驟：

步驟51：對于有前車的情況，前車對后車制約，由于車輛在速度控制區域不換道，后車不可能越過前車提前進入交叉口，因此首先考慮前車對后車的制約；t_{n，sl，pi}表示第n輛車從停車線到沖突點p_i的時間，由公式(27)計算：

當T′_n-1，sl+t_{n-1，sl，pi}≥T_pi^(m)時，說明第n-1輛車到達沖突點的時刻大于等于沖突點的時刻T_pi^(m)，第n-1輛車能順利通過沖突點，此時，為了提高通行效率，降低控制延誤，同車道的第n輛車應盡可能追趕第n-1輛車，以第n-1輛車車尾進入行車線的時刻到達停車線，且速度與第n-1輛車保持一致，T′_n，sl表示第n輛車車尾通過停車線的時刻，T_pi^(m)表示沖突點p_i的時間；

δ表示車身通過停車線的時間，車輛長度用d_c表示，車輛以v_n，sl的速度通過停車線進入交叉口，當v_n，sl＝V_M時，勻速通過，當v_n，sl＜V_M，由于d_c為車輛長度，距離很短，認為不存在加速到V_M后勻速的階段，一直勻加速行駛，δ由公式(28)計算：

表示第n輛車以V_M的速度勻速行駛到停車線所花費的時間，由公式(29)計算；

表示第n輛車進入速度控制區域后以最大舒適減速度a_L從V_M立即減速到v_min后勻速行駛到停車線花費的時間，由公式(30)計算：

當時，表示第n輛車以V_M勻速行駛，也追不到第n-1輛車，則車輛n以V_M行駛，其中μ表示前后車安全時間間隔；

當時，表示第n輛車以a_L的減速度從V_M立即減速到v_min的時間早于第n-1輛車進入交叉口的時間，這種情況下車輛n必須停車；

當時，車輛n可以通過調整車速，使得行駛到停車線的時刻盡可能接近前車車尾通過停車線的時刻，且車速等于前車車速；

車輛從當前位置開始以V_M勻速行駛，以舒適減速度a_-減速行駛，速度減到與前車速度v″′后勻速行駛，再加速行駛到停車線，速度增加到v_n，sl，再以v_n，sl勻速行駛，由公式(13)、(31)-(36)計算T₈、T₉、T₁₀、T₁₁、T_n，sl、加速度a_-、a₊、中間變量v″′；

v″′＝V_M-a_-·(T₉-T₈) (32)

v_n，sl＝v″′+a₊·(T₁₁-T₁₀) (33)

v_min≤v″′≤v_n，sl≤v_n-1，sl (34)

0≤T₁₂＜T₁₃≤T₁₄＜T₁₅≤T_n，sl (35)

T_n，sl≥T′_n-1，sl+μ (36)

式中：T₈、T₉、T₁₀、T₁₁、T_n，sl表示每個行駛階段結束的時刻，v″′為中間變量，表示減速后的速度，規定車輛在速度調整階段的最小速度為v_min，則減速后的速度v″′大于等于最小速度v_min；μ表示前后車安全時間間隔，T₈＝0表示車輛n沒有以VM勻速行駛階段；T₉＝T₁₀表示車輛n沒有以v″′勻速行駛階段；T₁₁＝T_n，sl表示車輛n沒有以v_n，sl勻速行駛階段；

在速度v_n，sl盡可能接近v_n-1，sl的前提下，速度波動最小為目標，滿足公式(37)約束：

α·min(v_n-1，sl-v_n，sl)+β·min(V_M-v″′) (37)

式中，α、β表示權重系數；

得到距離與時間的函數和速度與時間的函數如公式(38)-(39)所示：

當T′_n-1，sl+t_{n-1，sl，pi}＜T_pi^(m)時，說明第n-1輛車不能通過沖突點，則需要在交叉口停車等待，則后車減速行駛到前車車尾時速度為0；

步驟52：當T′_n-1，sl+t′_{n-1，sl，pi}≥T_pi^(m)且時，說明第n-1輛車不受沖突點的約束，能在T′_n-1，sl時刻進入交叉口，且車輛n能夠通過調整速度在T′_n-1，sl+μ的時刻以與n-1輛車相同的速度v_n，sl≤v_n-1，sl到達停車線，繼續判斷車輛n是否能通過制約車輛n的沖突點，同一車道上的車輛轉向不一定相同，根據車輛n-1與車輛n的轉向是否相同，可以分為情況3和情況4兩種情況：

情況3：車輛n-1與車輛n的轉向相同時，在交叉口內部的行駛軌跡相同，制約車輛n和車輛n-1的沖突點相同，車輛n-1不受沖突點時刻的制約，則第n輛車同樣不受沖突點時刻制約，能緊跟前車通過交叉口；

情況4：車輛n-1與車輛n的轉向不相同時，在交叉口內部的行駛軌跡不相同，制約車輛n和車輛n-1的沖突點不相同，需進一步判斷車輛n能否不停車進入交叉口；

表示第n輛車通過停車線后以a_L的減速度從v_n，sl立即減速到v_min，勻速行駛到沖突點花費的時間，由公式(40)計算：

L_{n，，sl，pi}表示車輛n從停車線位置點到沖突點p_i的距離，

T_pi^(m)表示沖突點p_i的時間，當時，表示第n輛車以最大消耗時間行駛到沖突點，仍然早于沖突點的時間，則必須在停車線停車等待；

表示車輛n在速度v_n，sl以最大舒適加速度a_U立即增加到V_M后勻速花費的時間，當時，說明車輛可以調整車速在T_pi^(m)時刻通過沖突點，轉至步驟4繼續計算車輛通過沖突點的車速。