一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法
1.本發明屬于自動駕駛車輛軌跡規劃領域，具體涉及一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法。
背景技術：
2.軌跡規劃是自動駕駛技術中的重點與難點，換道是基本駕駛行為之一。在交叉口進口道的車輛換道具有強制性，而且換道頻率高，需要考慮的因素比常規路段更為復雜。換道軌跡規劃不合理會影響通行效率或者導致換道失敗，甚至會出現車輛壓線換道等違反交通規則的情況，同時也將威脅行車安全。為自動駕駛車輛提供極限換道軌跡的預警后，就能夠避免發生上述情況。因此，確定交叉口進口道自動駕駛車輛的極限換道軌跡對于提高駕駛安全性、換道成功率和通行效率具有重要意義。
3.現有對軌跡規劃的研究比較廣泛，但大多數集中在避障上，并且較少涉及交叉口進口道場景。發明專利202111661902.6根據道路環境、障礙物信息以及當前車速，通過求解關于舒適性、避障效率、側滑的多目標優化問題，確定最優的避障換道回正點和基于多項式曲線的避障換道軌跡；發明專利202111406002.7首先根據道路形式、車輛的位姿和速度信息生成候選軌跡，然后構建并計算候選軌跡的成本函數，選擇成本最小的候選軌跡作為最優軌跡。然而，以往未見自動駕駛車輛在交叉口進口道極限換道軌跡的研究。交叉口進口道的換道行為與常規路段不同，為了保證行車安全、不違反交通規則和成功換至目標車道以滿足轉向需求，自動駕駛車輛需要極限換道軌跡的預警以指導其進行換道操作，因此，在軌跡規劃領域，交叉口進口道自動駕駛車輛極限換道軌跡的確定方法是非常有必要的。
4.基于上述背景，亟需提出一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法，在考慮車輛自身條件約束和道路條件約束的情況下，基于五次多項式確定極限換道軌跡，以提高駕駛安全性、換道成功率和通行效率。經查，暫未有交叉口進口道自動駕駛車輛極限換道軌跡的確定方法的相關報道。
技術實現要素：
5.1.一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法，包括如下步驟：
6.步驟一、生成基于五次多項式的換道軌跡集：
7.步驟1.1建立frenet坐標系：
8.定義換道前自動駕駛車輛所在車道為原車道，以下將自動駕駛車輛簡稱為車輛，以原車道的中心線為參考線，以車輛自身為原點，分別以參考線的切向量s和參考線的法向量d所在直線為坐標軸建立frenet坐標系，定義參考線的切向量s方向為縱向，參考線的法向量d方向為橫向；
9.步驟1.2建立并求解基于五次多項式的換道軌跡模型：
10.在frenet坐標系下分別建立縱向位移和橫向位移關于換道過程中某一時刻t的五次多項式函數s(t)和d(t)：
[0011][0012]
其中，pk、qk是時刻t的系數，k＝0，1，2，3，4，5；
[0013]
求解縱向位移關于時刻t的函數s(t)，對s(t)進行兩次求導分別得到縱向速度和縱向加速度關于時刻t的函數和
[0014][0015]
將換道開始時刻t＝0和此時車輛的縱向位置s(0)、縱向速度和縱向加速度分別帶入上式求得p0＝s(0)、和
[0016]
將五次多項式函數s(t)寫成如下矩陣形式：
[0017][0018]
以交叉口進口道導向車道線遠離交叉口方向的端點位置為參考確定換道結束時刻t＝t車輛的縱向位置s(t)，定義交叉口進口道限速為v
limit
，車輛最大加速度為a
max
，將交叉口進口道限速的縱向分量v
limit,s
和車輛最大加速度的縱向分量a
max,s
分別作為換道結束時刻車輛的縱向速度和縱向加速度，再將換道結束時刻車輛的縱向位置s(t)、縱向速度v
limit,s
和縱向加速度a
max,s
代入上面的矩陣中求得p3、p4和p5；
[0019]
求解橫向位移關于時刻t的函數d(t)，對d(t)進行兩次求導分別得到橫向速度和橫向加速度關于時刻t的函數和
[0020][0021]
將換道開始時刻t＝0和此時車輛的橫向位置d(0)、橫向速度和橫向加速度分別帶入上式求得q0＝d(0)、和
[0022]
將五次多項式函數d(t)寫成如下矩陣形式：
[0023][0024]
以交叉口進口道導向車道線遠離交叉口方向的端點位置為參考確定換道結束時刻車輛的橫向位置d(t)，將交叉口進口道限速的橫向分量v
limit,d
和車輛最大加速度的橫向分量a
max,d
分別作為換道結束時刻車輛的橫向速度和橫向加速度，再將換道結束時刻車輛的橫向位置d(t)、橫向速度v
limit,d
和橫向加速度a
max,d
代入上面的矩陣中求得q3、q4和q5；
[0025]
最后，以0.2秒為時間步長將換道結束時刻t取為[3秒，10秒]，生成換道軌跡集；
[0026]
步驟二、考慮車輛動力學約束對換道軌跡集進行優化：
[0027]
加入車輛動力學約束，優化步驟一所生成的換道軌跡集：
[0028]
速度約束：換道過程中車輛的速度v(t)不大于交叉口進口道限速，即0≤v(t)≤v
limit
；加速度約束：換道過程中車輛的加速度a(t)不大于車輛最大加速度a
max
，不小于車輛最大減速度a-max
，即a-max
≤a(t)≤a
max
；橫向加速度約束：換道過程中車輛的橫向加速度應保證車輛不發生側滑和側傾，即其中b為車輛輪距，單位：cm，hg為質心高度，單位：cm，hr為側傾中心高度，單位：cm，g為重力加速度，為車輛側傾率，單位：rad/g，為路面附著系數；
[0029]
步驟三、遍歷換道起點位置，確定極限換道起點及其對應的極限換道軌跡：
[0030]
將交叉口進口道可跨越同向車道線分界線的區域稱為可換道區域，以0.1秒為時間步長，在可換道區域內，從步驟一中所代入的換道開始時刻的車輛位置(s(0)，d(0))向交叉口方向遍歷換道起點位置，直到步驟二所得到的換道軌跡集中只剩下一條軌跡時停止遍歷，遍歷停止時的換道起點即為極限換道起點，其對應的軌跡為極限換道軌跡；
[0031]
跨越兩條車道以上的換道操作需要分多次進行計算，每次僅可換道至相鄰車道，每次確定極限換道起點及極限換道軌跡時均重復步驟一至三。
[0032]
與現有技術相比，本發明的有益效果是：
[0033]
本發明所述的一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法，考慮車輛自身條件約束和道路條件約束，為自動駕駛車輛提供了極限換道軌跡的預警，避免出現車輛壓線換道等違反交通規則的情況，同時也提高了駕駛安全性、換道成功率和通行效率，為自動駕駛車輛合理規劃換道軌跡提供了技術支撐。
附圖說明
[0034]
圖1是本發明的總體流程圖；
[0035]
圖2是本發明的frenet坐標系示意圖；
[0036]
圖3是本發明步驟一至三仿真計算結果圖；
[0037]
圖4是本發明一次換道的極限換道軌跡示例圖；
[0038]
圖5是本發明多次換道的極限換道軌跡示例圖。
具體實施方式
[0039]
本發明專利所述的一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法，適用于確定車輛在進入交叉口前的換道過程中周圍無其他車輛干擾的情況下保證車輛行駛安全和不違反交通規則的極限換道軌跡。該極限換道軌跡不會與導向車道線相交，滿足車輛動力學約束，車輛駛過極限換道起點后便不可進行換道操作，并且，剛好到達極限換道起點的車輛必須按照規劃的極限換道軌跡行駛。
[0040]
參閱圖1，本發明所述的一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法有以下幾個步驟：
[0041]
步驟一、在frenet坐標系下，如圖2所示，建立并求解基于五次多項式的換道軌跡模型，生成換道軌跡集；
[0042]
步驟二、加入車輛動力學約束，優化步驟一中所生成的換道軌跡集；
[0043]
步驟三、由步驟一中所代入的換道開始時刻的車輛位置向交叉口方向遍歷換道起點，直到步驟二的換道軌跡集中只剩下一條軌跡時，即確定極限換道起點及其對應的極限換道軌跡。
[0044]
步驟一至三的仿真計算結果如圖3所示，本發明確定的一次換道的極限換道軌跡如圖4所示，極限換道終點即為以交叉口進口道導向車道線遠離交叉口方向的端點位置為參考確定的換道結束時刻車輛的位置(s(t)，d(t))。
[0045]
《中華人民共和國道路交通安全法》規定車輛不得一次連續變更兩條以上機動車道。因此，跨越兩條車道以上的換道操作需要分多次進行計算，每次僅可換道至相鄰車道，每次確定極限換道起點及極限換道軌跡均重復步驟一至三，如圖5所示。
[0046]
實施例
[0047]
本發明所述的一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法的實施例，給出實施過程和檢驗結果，但本發明的保護范圍不限于下述的實施例。
[0048]
通過仿真實驗進行實例論述。選取石家莊市裕華路與友誼大街交叉口西進口道。該進口道每車道寬為3.25米，導向車道線長為70米，共7條車道，分別為左轉專用道兩條，直行專用道四條，右轉專用道一條。根據本發明所述的一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法對自動駕駛車輛在各車道間的極限換道軌跡進行計算，主要的換道操作如下表，仿真計算結果如圖3所示。
[0049]
表1各車道間的主要換道操作
[0050][0051]
對同時段該交叉口進口道的航拍視頻數據進行觀察統計，發現在禁止換道區域，即導向車道線區域，壓線換道的車輛有6輛。采用本發明所述的一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法后，仿真實驗中未出現壓線換道的車輛，且仿真實驗中臨近導向車道線進行換道操作的車輛，比實際場景中的平均行駛時間減少了9.3％。由此可見，采用該方法為自動駕駛車輛提供極限換道軌跡的預警，確保了車輛行駛安全，提高了換道成功率和通行效率。
技術特征：
1.一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法，包括如下步驟：步驟一、生成基于五次多項式的換道軌跡集：步驟1.1建立frenet坐標系：定義換道前自動駕駛車輛所在車道為原車道，以下將自動駕駛車輛簡稱為車輛，以原車道的中心線為參考線，以車輛自身為原點，分別以參考線的切向量s和參考線的法向量d所在直線為坐標軸建立frenet坐標系，定義參考線的切向量s方向為縱向，參考線的法向量d方向為橫向；步驟1.2建立并求解基于五次多項式的換道軌跡模型：在frenet坐標系下分別建立縱向位移和橫向位移關于換道過程中某一時刻t的五次多項式函數s(t)和d(t)：其中，p
k
、q
k
是時刻t的系數，k＝0，1，2，3，4，5；求解縱向位移關于時刻t的函數s(t)，對s(t)進行兩次求導分別得到縱向速度和縱向加速度關于時刻t的函數和和將換道開始時刻t＝0和此時車輛的縱向位置s(0)、縱向速度和縱向加速度分別帶入上式求得p0＝s(0)、和將五次多項式函數s(t)寫成如下矩陣形式：以交叉口進口道導向車道線遠離交叉口方向的端點位置為參考確定換道結束時刻t＝t車輛的縱向位置s(t)，定義交叉口進口道限速為v
limit
，車輛最大加速度為a
max
，將交叉口進口道限速的縱向分量v
limit,s
和車輛最大加速度的縱向分量a
max,s
分別作為換道結束時刻車輛的縱向速度和縱向加速度，再將換道結束時刻車輛的縱向位置s(t)、縱向速度v
limit,s
和縱向加速度a
max,s
代入上面的矩陣中求得p3、p4和p5；求解橫向位移關于時刻t的函數d(t)，對d(t)進行兩次求導分別得到橫向速度和橫向加速度關于時刻t的函數和
將換道開始時刻t＝0和此時車輛的橫向位置d(0)、橫向速度和橫向加速度分別帶入上式求得q0＝d(0)、和將五次多項式函數d(t)寫成如下矩陣形式：以交叉口進口道導向車道線遠離交叉口方向的端點位置為參考確定換道結束時刻車輛的橫向位置d(t)，將交叉口進口道限速的橫向分量v
limit,d
和車輛最大加速度的橫向分量a
max,d
分別作為換道結束時刻車輛的橫向速度和橫向加速度，再將換道結束時刻車輛的橫向位置d(t)、橫向速度v
limit,d
和橫向加速度a
max,d
代入上面的矩陣中求得q3、q4和q5；最后，以0.2秒為時間步長將換道結束時刻t取為[3秒，10秒]，生成換道軌跡集；步驟二、考慮車輛動力學約束對換道軌跡集進行優化：加入車輛動力學約束，優化步驟一所生成的換道軌跡集：速度約束：換道過程中車輛的速度v(t)不大于交叉口進口道限速，即0≤v(t)≤v
limit
；加速度約束：換道過程中車輛的加速度a(t)不大于車輛最大加速度a
max
，不小于車輛最大減速度a-max
，即a-max
≤a(t)≤a
max
；橫向加速度約束：換道過程中車輛的橫向加速度應保證車輛不發生側滑和側傾，即其中b為車輛輪距，單位：cm，h
g
為質心高度，單位：cm，h
r
為側傾中心高度，單位：cm，g為重力加速度，為車輛側傾率，單位：rad/g，為路面附著系數；步驟三、遍歷換道起點位置，確定極限換道起點及其對應的極限換道軌跡：將交叉口進口道可跨越同向車道線分界線的區域稱為可換道區域，以0.1秒為時間步長，在可換道區域內，從步驟一中所代入的換道開始時刻的車輛位置(s(0),d(0))向交叉口方向遍歷換道起點位置，直到步驟二所得到的換道軌跡集中只剩下一條軌跡時停止遍歷，遍歷停止時的換道起點即為極限換道起點，其對應的軌跡為極限換道軌跡；跨越兩條車道以上的換道操作需要分多次進行計算，每次僅可換道至相鄰車道，每次確定極限換道起點及極限換道軌跡時均重復步驟一至三。
技術總結
本發明專利公開了一種交叉口進口道自動駕駛車輛極限換道軌跡的確定方法。具體方法主要包括三個步驟：一、生成基于五次多項式的換道軌跡集；二、考慮車輛動力學約束對換道軌跡集進行優化；三、遍歷換道起點位置，確定極限換道起點及其對應的極限換道軌跡。本發明專利考慮車輛自身條件約束和道路條件約束，提出交叉口進口道自動駕駛車輛極限換道軌跡的確定方法，為自動駕駛車輛提供了極限換道軌跡的預警，從而提高駕駛安全性、換道成功率和通行效率。率。率。
