摘 要：為提高智能汽車在動(dòng)態(tài)行車環(huán)境下的行駛安全和通行效率，研究了基于周邊車輛軌跡預(yù)測(cè)的路徑規(guī)劃方法，并進(jìn)行了仿真。提出了一種基于時(shí)空?qǐng)D卷積網(wǎng)絡(luò)（STGCN）的周邊車輛軌跡預(yù)測(cè)方法，通過(guò)STGCN對(duì)車輛歷史軌跡進(jìn)行編碼，提取交通圖的時(shí)空特征，并結(jié)合長(zhǎng)短時(shí)記憶網(wǎng)絡(luò)實(shí)現(xiàn)周邊車輛的軌跡預(yù)測(cè)。在此基礎(chǔ)上，提出了一種基于改進(jìn)人工勢(shì)場(chǎng)（APF）的路徑規(guī)劃方法；建立了基于APF的行車危險(xiǎn)評(píng)價(jià)模塊；利用Frenet坐標(biāo)描述駕駛危險(xiǎn)度，通過(guò)目標(biāo)障礙物和道路邊界的勢(shì)能分布及梯度下降法完成路徑規(guī)劃。結(jié)果表明：本算法的短時(shí)預(yù)測(cè)精度提高了3%，長(zhǎng)時(shí)預(yù)測(cè)精度提高了1%；所得路徑曲線的前輪轉(zhuǎn)角不超過(guò)0.12 rad，曲率不超過(guò)0.1；因此，在確保有效避撞的前提下，保證了車輛行駛的舒適性和高效性。

關(guān)鍵詞：智能汽車；路徑規(guī)劃；軌跡預(yù)測(cè)；時(shí)空?qǐng)D卷積網(wǎng)絡(luò)（STGCN）；人工勢(shì)場(chǎng)（APF）

Intelligent vehicle path planning method based on peripheral vehicle trajectory prediction

Abstract： A path planning method was investigated based on the peripheral vehicle trajectory prediction with doing digital simulations to improve the driving safety and access efficiency of intelligent vehicles in dynamic driving environments. The peripheral vehicle trajectory prediction method was proposed based on the Spatio-Temporal Graph Convolutional Network （STGCN）， which encoded the historical vehicle trajectories through STGCN， extracted the spatio-temporal features of traffic maps and combined with long and short-term memory networks to achieve the trajectory prediction of peripheral vehicles. On this basis， a path planning method was proposed based on an Improved Artificial Potential Field （APF）， and an APF-based driving hazard evaluation module was established， which described the driving hazard by using the Frenet coordinates， and completed the path planning through the potential distribution of target obstacles and road boundaries and the gradient descent method. The results show that the proposed algorithm improves prediction accuracy by about 3% in the short-time prediction and by 1% in the long-time prediction with a path curve of the front wheel angle not exceeding 0.12 rad， and a curvature not exceeding 0.1， ensuring comfort and high efficiency during vehicle travel while effectively avoiding collisions.

Key words： intelligent vehicles; path planning; trajectory prediction; spatiotemporal graph convolutional network （STGCN）; artificial potential field （APF）

自動(dòng)駕駛汽車卓越的安全性和道路利用效率使其成為改善道路交通系統(tǒng)的前景之一。然而，在復(fù)雜多變的交通環(huán)境中，自動(dòng)駕駛汽車需要進(jìn)行精確的軌跡預(yù)測(cè)和規(guī)劃，才能確保行駛的安全性和效率。

車輛軌跡預(yù)測(cè)的方法主要包括基于物理的方法、基于機(jī)器學(xué)習(xí)的方法、基于深度學(xué)習(xí)的方法^[1]和基于強(qiáng)化學(xué)習(xí)的方法。基于物理的方法計(jì)算成本低，時(shí)間短，但無(wú)法描述駕駛環(huán)境和其他參與者的影響，僅限于短期預(yù)測(cè)和靜態(tài)場(chǎng)景。ZHANG Lei等^[2]將基于恒速和加速度的運(yùn)動(dòng)預(yù)測(cè)模型與基于機(jī)動(dòng)的運(yùn)動(dòng)預(yù)測(cè)模型融合，采用多模型交互的方法對(duì)周邊車輛的運(yùn)動(dòng)軌跡進(jìn)行預(yù)測(cè)。宗長(zhǎng)富等^[3]提出一種雙層隱Markov模型（hidden Markov model，HMM）來(lái)識(shí)別駕駛意圖和預(yù)測(cè)駕駛行為：下層的多維Gauss HMM用于短期駕駛行為預(yù)測(cè)，上層的多維離散HMM用于復(fù)合情況下的長(zhǎng)期駕駛意圖預(yù)測(cè)。基于傳統(tǒng)機(jī)器學(xué)習(xí)的方法考慮的因素比物理模型多，準(zhǔn)確度有所提升，但大多需要針對(duì)特定場(chǎng)景提供特定策略，難以有效預(yù)測(cè)復(fù)雜交通場(chǎng)景中的軌跡。GUO Hongyan等^[4]提出了一種基于長(zhǎng)短期記憶的雙注意力機(jī)制軌跡預(yù)測(cè)方法，該方法結(jié)合自我車輛的運(yùn)動(dòng)趨勢(shì)，利用時(shí)間注意機(jī)制最大限度地利用歷史軌跡信息，并通過(guò)空間注意機(jī)制分析相鄰車輛對(duì)目標(biāo)車輛軌跡的影響，從而減少潛在軌跡的不確定性。近年來(lái)，基于深度學(xué)習(xí)的軌跡預(yù)測(cè)方法被廣泛應(yīng)用。李文禮等^[5]采用社會(huì)生成對(duì)抗網(wǎng)絡(luò)（social generative adversarial network，Social-GAN）和注意力機(jī)制，對(duì)自車與周圍車輛進(jìn)行交互軌跡預(yù)測(cè)，并結(jié)合自車時(shí)空（spatio-temporal，S-T）圖及約束條件獲得最優(yōu)軌跡。李旭川等^[6]在長(zhǎng)短期記憶（long short-term memory， LSTM）軌跡預(yù)測(cè)模型基礎(chǔ)上，考慮車型和車輛輪廓，利用五次多項(xiàng)式生成軌跡簇，并通過(guò)目標(biāo)優(yōu)化函數(shù)選出最優(yōu)軌跡。劉啟冉等^[7]采用改進(jìn)的社會(huì)力模型對(duì)周圍車輛進(jìn)行軌跡預(yù)測(cè)，并將軌跡投影到時(shí)空占用圖，結(jié)合加速度和曲率篩選最優(yōu)軌跡。王安杰等^[8]通過(guò)Frenet坐標(biāo)系下的運(yùn)動(dòng)學(xué)模型建立預(yù)測(cè)風(fēng)險(xiǎn)場(chǎng)，考慮車輛動(dòng)力學(xué)、速度、加速度與曲率約束，采用多項(xiàng)式曲線和二次規(guī)劃方法優(yōu)化決策軌跡。

此外，還有學(xué)者采用圖卷積神經(jīng)網(wǎng)絡(luò)- 循環(huán)神經(jīng)網(wǎng)絡(luò)（graph neural network-recurrent neural network， GNN-RNN）編碼的網(wǎng)絡(luò)^[9]、人工勢(shì)場(chǎng)（artificial potential field，APF）和模糊神經(jīng)網(wǎng)絡(luò)相結(jié)合的方法^[10]、彎道引力勢(shì)能場(chǎng)^[11]、無(wú)人駕駛“行車意圖—風(fēng)險(xiǎn)復(fù)合場(chǎng)”^[12]，以及重新構(gòu)建包含汽車縱向和側(cè)向車速的道路勢(shì)場(chǎng)^[13]等方法開(kāi)展了相關(guān)研究。這些研究成果為實(shí)現(xiàn)自主駕駛提供了重要的理論支持和技術(shù)基礎(chǔ)。

盡管有大量研究積累，但基于軌跡預(yù)測(cè)的車輛軌跡規(guī)劃方法仍然存在顯著的局限性。首先，現(xiàn)有方法忽略了自車規(guī)劃軌跡與周圍車輛之間的交互影響；其次，一些方法如人工勢(shì)場(chǎng)算法僅適用于特定道路場(chǎng)景，難以滿足結(jié)構(gòu)化道路場(chǎng)景的需求。這些局限性不僅影響了自動(dòng)駕駛汽車的安全性，還限制了其通行效率。因此，迫切需要進(jìn)一步探索新的解決方案。

本研究旨在從車輛軌跡預(yù)測(cè)和運(yùn)動(dòng)規(guī)劃兩個(gè)關(guān)鍵角度進(jìn)行探討，提出一系列創(chuàng)新方法以應(yīng)對(duì)這一挑戰(zhàn)。首先，通過(guò)對(duì)歷史軌跡進(jìn)行編碼并提取時(shí)空特征，添加規(guī)劃耦合模塊，以考慮自動(dòng)駕駛車輛規(guī)劃軌跡對(duì)周圍車輛的交互影響，實(shí)現(xiàn)更準(zhǔn)確的周圍車輛軌跡預(yù)測(cè)；其次，構(gòu)建 Frenet 坐標(biāo)系下的人工勢(shì)場(chǎng)模型，以提高在結(jié)構(gòu)化道路上的主動(dòng)避碰性能。該方法結(jié)合梯度下降法的變道策略和目標(biāo)障礙物及道路邊界的勢(shì)能分布，以便實(shí)現(xiàn)自動(dòng)車輛的安全變道路徑規(guī)劃。

1"" 考慮周邊車輛交互的軌跡預(yù)測(cè)

軌跡預(yù)測(cè)任務(wù)可以表示為基于歷史軌跡估計(jì)自動(dòng)駕駛場(chǎng)景中周圍所有車輛未來(lái)位置的問(wèn)題。本文在時(shí)空?qǐng)D卷積網(wǎng)絡(luò)（spatiotemporal graph convolutional network，STGCN）軌跡預(yù)測(cè)模型的基礎(chǔ)上，添加了規(guī)劃耦合模塊，構(gòu)建了規(guī)劃耦合預(yù)測(cè)計(jì)劃（graph planning interaction prediction plan，GPIP-plan），以考慮自動(dòng)駕駛車輛規(guī)劃軌跡對(duì)周圍車輛的交互作用。GPIP-plan網(wǎng)絡(luò)結(jié)構(gòu)如圖1所示，藍(lán)色車輛是一輛自動(dòng)駕駛汽車，當(dāng)前時(shí)刻的規(guī)劃軌跡用綠色線條表示，車輛歷史軌跡用黑色線條表示，車輛預(yù)測(cè)軌跡用紅色線條表示。

通過(guò)圖卷積網(wǎng)絡(luò)提取車輛之間的時(shí)空交互特征，然后通過(guò)2 個(gè)LSTM 網(wǎng)絡(luò)分別對(duì)圖卷積神經(jīng)網(wǎng)絡(luò)（graph convolutional network，GCN） 提取的時(shí)空交互特征和規(guī)劃耦合模塊對(duì)自動(dòng)駕駛汽車的未來(lái)規(guī)劃軌跡特征進(jìn)行編碼。接著，將規(guī)劃特征合并到交通場(chǎng)景的時(shí)空?qǐng)D特征中，使軌跡預(yù)測(cè)算法能夠更好地捕捉自動(dòng)駕駛汽車與周圍車輛的交互關(guān)系。最后，將耦合好的時(shí)空交互特征輸入 LSTM 網(wǎng)絡(luò)進(jìn)行解碼，生成該場(chǎng)景下所有車輛的預(yù)測(cè)軌跡。

模型的輸入為周邊車輛的歷史軌跡點(diǎn)，可表示為

X = [p₁， p₂，…， p_th]." ""（1）

其中：p_th表示th時(shí)刻場(chǎng)景中所有車輛的橫縱坐標(biāo)，表達(dá)式如下：

pth = [xth1， yth1; xth2， yth2;…; xthn， ythn]." ""（2）

此外，模型的規(guī)劃輸入 X_p 為自動(dòng)駕駛汽車的規(guī)劃軌跡：

Xp = [pth+1， pth+2，…， pth+tp]，" " （3）

pth+tp = [xth+tp， yth+tp]." " （4）

其中：n是距離本車100 m范圍內(nèi)的車輛個(gè)數(shù)；th表示輸入的歷史時(shí)間步長(zhǎng)，tp表示輸出的預(yù)測(cè)時(shí)間步長(zhǎng)，本文選取范圍為3 s的歷史軌跡序列。

模型的輸出 X_out 是時(shí)間步長(zhǎng) th+1 到 th+tp 期間周圍所有車輛的位置信息：

Xout = [pth+1， pth+2，…， pth+tp]，" " （5）

pth+tp = [xth1+tp1， yth1+tp1; xth2+tp2， yth2+tp2;…， xthn+tpn， ythn+tpn]." " （6）

1.1"" 圖卷積神經(jīng)網(wǎng)絡(luò)（GCN）

軌跡特征提取通常使用卷積神經(jīng)網(wǎng)絡(luò) （CNN），但 CNN 需要較為復(fù)雜的結(jié)構(gòu)來(lái)描述車輛之間的交互作用。因此，將自動(dòng)駕駛應(yīng)用場(chǎng)景中的所有可能影響自動(dòng)駕駛車輛的軌跡數(shù)據(jù)映射到無(wú)向圖中，以表示車輛間的交互作用。當(dāng)場(chǎng)景中有 n 輛車行駛時(shí)，則構(gòu)造出 n×c×th 維數(shù)組作為模型的輸入節(jié)點(diǎn)，其中 c 代表車輛的特征維度。只考慮車輛位置之間的交互關(guān)系，因此 c = 2。如圖1 所示，用 V 作為節(jié)點(diǎn)集，表示交通場(chǎng)景中車輛軌跡的集合。由于每個(gè)車輛在不同時(shí)刻的狀態(tài)不同，因此節(jié)點(diǎn)集可表示為

V = {v_it|i = 1，2，…，n; t = 1，2，…，th}." " （7）

以自動(dòng)駕駛車輛為中心構(gòu)建圖，以生成一條數(shù)據(jù)。 n 表示以自動(dòng)駕駛車輛為中心的區(qū)域內(nèi)的車輛數(shù)量。如圖1 所示，選擇藍(lán)色車輛作為自動(dòng)駕駛車輛，虛線框內(nèi)為中心區(qū)域。

為了模擬車輛之間的空間相互作用，GCN 中通過(guò)邊連接表示同一時(shí)刻相鄰車輛之間的交互作用。E 表示各車輛在空間維度上的交互特征：

如圖1 所示，圖卷積神經(jīng)網(wǎng)絡(luò)主要由卷積層和圖結(jié)構(gòu)組成。卷積層用于提取時(shí)間特征，而圖結(jié)構(gòu)用于捕捉車輛間的交互特征：

將歷史軌跡數(shù)據(jù)處理成節(jié)點(diǎn)特征矩陣和鄰接矩陣，完成了圖1 所示的特征提取和交互關(guān)系構(gòu)建。

時(shí)空?qǐng)D卷積網(wǎng)絡(luò)以交通場(chǎng)景中所有車輛的歷史軌跡作為輸入。通過(guò)批歸一化層對(duì)歷史軌跡進(jìn)行歸一化，并通過(guò)具有1×1大小的卷積層將二維坐標(biāo)映射到高維空間。卷積層后得到形狀為（b，C，th，n）的特征f_conv，其中： b為批大小，C為新的通道數(shù)。

空間圖卷積網(wǎng)絡(luò)以歸一化的歷史軌跡作為輸入，提取交通場(chǎng)景中所有車輛的交互特征。為了確定車輛節(jié)點(diǎn)之間的連接類型 （ 自連接或與其他節(jié)點(diǎn)的連接 ），并考慮間接二次連接，將空間鄰接矩陣 A_ij 擴(kuò)展如下：

A_ext = [A₀， A₁， A₂]，" " （12）

fgra = （Anorm + Atrain） fconv ." " （13）

采用時(shí)間圖卷積網(wǎng)絡(luò)對(duì)車輛在不同時(shí)間步長(zhǎng)之間的時(shí)間相互作用進(jìn)行建模。對(duì)于每一輛車，相鄰時(shí)間步長(zhǎng)通過(guò)邊相連，從而得到了其時(shí)空特征。空間圖卷積網(wǎng)絡(luò)和時(shí)間圖卷積網(wǎng)絡(luò)交替進(jìn)行 3 次，以充分提取時(shí)空特征。

1.2"" 規(guī)劃耦合網(wǎng)絡(luò)

規(guī)劃耦合網(wǎng)絡(luò)以自動(dòng)駕駛汽車的規(guī)劃軌跡和時(shí)空?qǐng)D卷積網(wǎng)絡(luò)生成的交通場(chǎng)景圖特征作為輸入。首先，將規(guī)劃軌跡的維度從 （b，2，th） 擴(kuò)展到 （b，2，th，n），使其可以應(yīng)用于每輛周圍車輛。然后，對(duì)規(guī)劃軌跡進(jìn)行規(guī)范化，以增加模型的訓(xùn)練穩(wěn)定性。

接著，通過(guò)線性化層處理歸一化的規(guī)劃軌跡，將通道數(shù)從 2 增加到 C，以學(xué)習(xí)規(guī)劃軌跡。然后，將圖特征 f_gra 和規(guī)劃特征 f_plan 的形狀都變?yōu)椋?em>b×n，th，C），并將它們輸入 LSTM 編碼器。最后，將圖隱特征與規(guī)劃隱特征進(jìn)行串聯(lián)，得到耦合特征。

f_cpl = LSTM_gra（ f_gra）|| LSTM_plan（ f_plan）." " （14）

1.3"" 長(zhǎng)短時(shí)循環(huán)神經(jīng)網(wǎng)絡(luò)（LSTM）

在車輛軌跡預(yù)測(cè)中，常用的方法之一是基于長(zhǎng)短期記憶網(wǎng)絡(luò) （LSTM） 的模型。這些模型能夠利用歷史軌跡數(shù)據(jù)，結(jié)合當(dāng)前環(huán)境信息，對(duì)車輛未來(lái)的行駛軌跡進(jìn)行預(yù)測(cè)。通過(guò)學(xué)習(xí)歷史數(shù)據(jù)中的模式和規(guī)律，這些模型可以預(yù)測(cè)車輛在未來(lái)幾秒或幾十秒內(nèi)可能采取的行動(dòng)。

LSTM 網(wǎng)絡(luò)由多個(gè)門控單元組成， 包括輸入門 （input gate）、遺忘門 （forget gate）、輸出門 （output gate）和細(xì)胞狀態(tài)（cell state）。這些門控單元通過(guò)激活函數(shù)（ 通常是 sigmoid 函數(shù) ） 來(lái)控制信息的流動(dòng)，從而實(shí)現(xiàn)對(duì)序列數(shù)據(jù)的建模和學(xué)習(xí)。

遺忘門主要解決應(yīng)該丟棄的信息：

f_t = σ（W_f ·[ h_t-1，x_t] + b_f ）." " （15）

輸入門主要用于更新信息：

完成細(xì)胞狀態(tài)更新：

輸出門主要決定將要被輸出的當(dāng)前時(shí)間步的信息：

軌跡預(yù)測(cè)網(wǎng)絡(luò)以規(guī)劃耦合網(wǎng)絡(luò)生成的圖特征與規(guī)劃特征的耦合特征作為輸入。將這些耦合特征輸入LSTM 解碼器，以預(yù)測(cè)每個(gè)時(shí)間步長(zhǎng)的位置變化。

X_out = LSTM_fut（ f_cpl）." " （23）

1.4"" 預(yù)測(cè)效果

選擇美國(guó)高速（next generation simulation，NGSIM）數(shù)據(jù)集對(duì)模型進(jìn)行訓(xùn)練。該數(shù)據(jù)集涵蓋了結(jié)構(gòu)化道路的路口、高速上下閘道等車路協(xié)同研究的熱點(diǎn)區(qū)域，被廣泛用于車輛軌跡預(yù)測(cè)研究。在實(shí)驗(yàn)過(guò)程中，首先提取 NGSIM數(shù)據(jù)集中換道場(chǎng)景的軌跡，選取橫縱向坐標(biāo)2個(gè)維度的數(shù)據(jù)，并對(duì)數(shù)據(jù)進(jìn)行異常值剔除和濾波處理，使其更加符合實(shí)際行駛情況。然后，每隔8 s采集一段數(shù)據(jù)，包括自動(dòng)駕駛車輛和周圍車輛在交通場(chǎng)景中3 s的歷史軌跡和5 s的未來(lái)軌跡。將70%的數(shù)據(jù)用于模型訓(xùn)練， 20%的數(shù)據(jù)用于測(cè)試，10%的數(shù)據(jù)用于評(píng)估。由于訓(xùn)練需要大量的規(guī)劃軌跡，將NGSIM數(shù)據(jù)集中車輛在未來(lái)5 s的軌跡作為規(guī)劃耦合模塊的參考輸入。軌跡預(yù)測(cè)通常使用預(yù)測(cè)軌跡（pre）與實(shí)際軌跡（tru）之間的均方誤差 （root mean square error，RMSE）來(lái)描述算法的精度，均方根誤差為

將訓(xùn)練結(jié)果與V-LSTM（visual LSTM，V-LSTM） [^14]、 CS-LSTM（contextual sequence LSTM，CS-LSTM） ^[15]和 CNN-LSTM（convolutional neural network LSTM，CNN- LSTM） [^16]等軌跡預(yù)測(cè)模型在預(yù)測(cè)時(shí)域分別為1～5 s的情況下進(jìn)行對(duì)比。訓(xùn)練進(jìn)行了80 0 次，使用的設(shè)備為 Windows 10操作系統(tǒng)、Intel i7-10700K CPU、NVIDIA GeForce RTX 3060 GPU，深度學(xué)習(xí)框架為P ytorch 1.11.0，CUDA 11.3和cuDNN 8.2.1。

本文軌跡預(yù)測(cè)模型通過(guò)引入目標(biāo)車輛對(duì)環(huán)境車輛的影響，增加了模型的自由度，從而更細(xì)致地模擬了交通參與者之間的交互關(guān)系。這種增加自由度的方式，使得模型能夠捕捉到更多的動(dòng)態(tài)變化，從而提升了軌跡預(yù)測(cè)的精度。雖然增加自由度可能帶來(lái)一定的計(jì)算復(fù)雜度，但實(shí)驗(yàn)結(jié)果顯示，這種復(fù)雜度的增加是值得的，因?yàn)樗@著提高了模型的預(yù)測(cè)性能和實(shí)用性。

從圖 2 可知：本文軌跡預(yù)測(cè)模型在均方根誤差 （RMSE） 方面明顯優(yōu)于同類模型，這表明了本文模型的預(yù)測(cè)準(zhǔn)確性更高，較低的 RMSE 值意味著本文的模型能夠更精確地預(yù)測(cè)車輛未來(lái)的行駛軌跡，提高了決策規(guī)劃的可靠性和安全性。

從表 1 可知：添加了規(guī)劃耦合特征的模型均方根誤差相比不考慮這些影響的模型平均下降了15%，考慮影響的模型 RMSE 從 0.68下降到 0.44，顯著提高了預(yù)測(cè)精度。這些結(jié)果證明了目標(biāo)車輛對(duì)環(huán)境車輛影響的存在性和引入這些參數(shù)的必要性。

2"" 智能汽車路徑規(guī)劃

2.1"" 坐標(biāo)變換

為了在一個(gè)統(tǒng)一的框架內(nèi)對(duì)駕駛環(huán)境的危險(xiǎn)性進(jìn)行評(píng)價(jià)，安全系統(tǒng)需要一個(gè)固定的坐標(biāo)系。曲線在直角坐標(biāo)系中通常很復(fù)雜，而 Frenet 坐標(biāo)系提供了一個(gè)更自然的框架，可以更簡(jiǎn)單地描述曲線的特性，避免直角坐標(biāo)系中的復(fù)雜數(shù)學(xué)運(yùn)算。

根據(jù)預(yù)先獲得的路點(diǎn)序列，道路參考中心線（x_c，y_c） 可以參數(shù)化為三次多項(xiàng)式樣條：

其中：s是弧長(zhǎng)，（a₀，a₁，a₂，a₃，b₀，b₁，b₂，b₃）表示道路的擬合參數(shù)。其他點(diǎn)可以根據(jù)[x_c（s），x_c（s）]通過(guò)正交向量法映射到Frenet坐標(biāo)中。

如圖3所示，P₁（x₁，y₁）為不在參考中心線上的任意點(diǎn)，P_0i（i"= 1，2，…，n）為參考中心線上的點(diǎn)，其Frenet坐標(biāo)為（s_i，0），笛卡爾坐標(biāo)為（x_ci，y_ci）。P₁和P_0i連接線的向量為P₁P_0i，P_0i點(diǎn)中心線的切向量為[x'_ci（s），y'_ci（s）]。總能在參考中心線上找到一個(gè)對(duì)應(yīng)的點(diǎn)P_0r，其法向量為向量 P₁P_0r。|P₁P_0r|為P₁到參考中心線的最短距離。因此，P₁的Frenet坐標(biāo)為（sr，|P₁P_0r|）。

2.2" " 基于人工勢(shì)場(chǎng)的路徑規(guī)劃

大多數(shù)基于人工勢(shì)場(chǎng) （APF） 的路徑規(guī)劃研究都是在直角坐標(biāo)系下進(jìn)行的，這對(duì)于直線道路場(chǎng)景來(lái)說(shuō)非常方便。然而，在非直線道路場(chǎng)景下，不同的位置需要不斷修正勢(shì)場(chǎng)模型。在 Frenet 坐標(biāo)系中構(gòu)建人工勢(shì)場(chǎng)模型則可以省去這些繁重的修改任務(wù)。無(wú)論何種幾何類型的道路，在 Frenet 坐標(biāo)中總是可以被視為一條“直線道路”。因此，與以往的研究相比，本文在 Frenet 坐標(biāo)系下構(gòu)建算法模型，使算法不拘泥于直線道路的規(guī)劃，對(duì)道路幾何形狀的適應(yīng)性顯著提高。在道路環(huán)境中的危險(xiǎn)勢(shì)能主要包括道路勢(shì)能和障礙物勢(shì)能：

P_U（s，d ） = P_R（s，d ） + P_o（s，d ）." " （26）

道路邊界勢(shì)能模型是一種基于交通規(guī)則的勢(shì)場(chǎng)模型，旨在確保車輛在正常行駛和避障完成后能夠沿著道路中心線行駛。當(dāng)車輛靠近道路兩端邊界時(shí)，勢(shì)能應(yīng)逐漸增加，當(dāng)車輛到達(dá)道路邊界時(shí)，勢(shì)能最大。因此，道路邊界勢(shì)能如式 （27） 所示：

P_R（s，d ） = A（s）A（d） + P_A（s，d ）." " （27）

其中：P_A（s，d ）表示目標(biāo)點(diǎn)的引力勢(shì)能，主要是吸引自車向期望點(diǎn)移動(dòng)。A（s），A（d）表示振幅路在s軸和d軸方向上的勢(shì)能值，其數(shù)學(xué)表達(dá)式為：

其中：P_m是道路中心線的勢(shì)能，它在防止自動(dòng)駕駛車輛產(chǎn)生不必要的變道機(jī)動(dòng)方面起著至關(guān)重要的作用。L 是車道寬度。D_l和D_r分別是左右車道相對(duì)于參考中心線的偏移量，障礙物的位置是（s ，s ）。在本文中，會(huì)將預(yù)測(cè)出的周圍障礙物的位置點(diǎn)（s ，d ）代替障礙物位置（s ，d ）進(jìn)行規(guī)劃。S_t和S_d表示斥力勢(shì)能在s軸方向上的范圍，且S_t總是比S_d小。S_d為駕駛員期望的跟車距離，由自動(dòng)駕駛車輛的速度決定，它可以表示為

S_d = t_hv_h + d₀." " （31）

其中：t_h為車頭時(shí)距，v_h為自動(dòng)駕駛車輛的速度，d₀為安全距離。

斥力勢(shì)能主要使自我車輛與障礙物保持安全距離。本文引入了二維 Gauss 函數(shù)來(lái)描述碰撞風(fēng)險(xiǎn)，并將其在 Frenet 坐標(biāo)系下進(jìn)行了轉(zhuǎn)換。當(dāng)自動(dòng)駕駛車輛靠近障礙物時(shí)，斥力勢(shì)能變大，對(duì)自動(dòng)駕駛車輛產(chǎn)生排斥勢(shì)能，從而避免碰撞。斥力勢(shì)能表示為

其中：P_t和P_s為斥力勢(shì)能的閾值，c₁和c₂是2個(gè)系數(shù)，決定了障礙物三維勢(shì)能的范圍和形狀。

APF 算法主要是通過(guò)梯度下降來(lái)尋找最優(yōu)路徑。梯度下降的原理是計(jì)算勢(shì)能場(chǎng)的梯度，并沿著梯度的負(fù)方向逐步調(diào)整位置，以尋找最優(yōu)路徑或達(dá)到目標(biāo)點(diǎn)。

其中：F_t為總勢(shì)場(chǎng)力，F_s和F_d分別表示APF在s和d方向上的力。在2個(gè)方向合力的作用下，自動(dòng)駕駛車輛總是朝著梯度下降較快的方向行駛。根據(jù)當(dāng)前的位置（s_k，d_k）和s、d方向上的力，可以得到車輛下一步的位置（s_k+1，d_k+1）：

其中，γ為每個(gè)規(guī)劃步驟的步長(zhǎng)。

大量與駕駛員轉(zhuǎn)向避障行為相關(guān)的研究表明^[17]，車輛行駛過(guò)程中的換道軌跡近似于五次多項(xiàng)式曲線。五次多項(xiàng)式用于規(guī)劃避碰參考路徑，各點(diǎn)的位移和曲率曲線均具有連續(xù)性和光滑性，符合駕駛員的習(xí)慣。因此，本文采用五次多項(xiàng)式對(duì)規(guī)劃出的路徑點(diǎn)進(jìn)行擬合，得到最終的軌跡曲線：

d（s） = a + a s + a s² + a s³ + a s⁴ + a s⁵." " （36）

3"" 不同場(chǎng)景下的仿真驗(yàn)證

3.1"" 仿真平臺(tái)及場(chǎng)景建立

為了驗(yàn)證軌跡規(guī)劃算法在動(dòng)態(tài)交通場(chǎng)景中的有效性，本文在高速公路場(chǎng)景中進(jìn)行了軌跡預(yù)測(cè)和軌跡規(guī)劃算法的聯(lián)合仿真測(cè)試。具體而言，軌跡規(guī)劃和控制算法使用 C++ 編程并在 ROS 環(huán)境中運(yùn)行；PreScan 提供了動(dòng)態(tài)交通場(chǎng)景及傳感器模擬；CarSim 則提供了車輛動(dòng)力學(xué)模型。通過(guò) Simulink 提供的通訊模塊，這三者被集成在一起進(jìn)行聯(lián)合仿真，如圖 4 所示。

這種聯(lián)合仿真環(huán)境能夠真實(shí)地再現(xiàn)復(fù)雜的交通狀況，使得對(duì)軌跡規(guī)劃算法在實(shí)際應(yīng)用中的性能進(jìn)行評(píng)估成為可能。通過(guò)該仿真平臺(tái)，可以更全面地測(cè)試算法在不同交通場(chǎng)景下的響應(yīng)和適應(yīng)能力，從而驗(yàn)證其在提高行車安全性和效率方面的效果。

3.2"" 實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

為了確保駕駛的安全性和舒適性，自動(dòng)駕駛車輛必須具備高效的響應(yīng)機(jī)制和合理的路徑規(guī)劃能力，以規(guī)避潛在的危險(xiǎn)。本文通過(guò)圖 5、圖 6 和表 2 展示了不同駕駛場(chǎng)景下的路徑規(guī)劃效果和性能對(duì)比。

圖 5 展示了3 種不同駕駛場(chǎng)景下自動(dòng)駕駛車輛的路徑規(guī)劃效果。

通過(guò)使用本文提出的 GPIP-plan 算法，車輛在各種復(fù)雜駕駛環(huán)境中都能有效地進(jìn)行路徑規(guī)劃。在圖 5a中，自動(dòng)駕駛車輛在檢測(cè)到前方車輛變道意圖后，及時(shí)調(diào)整自身軌跡，成功避免了碰撞。圖 5b 展示了多車共行的場(chǎng)景，車輛通過(guò) 2 次變道操作，成功完成超車，展示了算法在動(dòng)態(tài)交通環(huán)境中的靈活性。圖 5c 展示了車輛在S 型彎道上的規(guī)劃軌跡，車輛能夠平穩(wěn)通過(guò)復(fù)雜彎道，證明了算法在應(yīng)對(duì)不同幾何形狀道路上的適應(yīng)性和有效性。這些結(jié)果表明，本文算法能夠在多種駕駛場(chǎng)景下保持車輛行駛的穩(wěn)定性和安全性。

圖 6 詳細(xì)展示了自動(dòng)駕駛車輛在不同場(chǎng)景下的航向角和曲率變化。

圖 6a 和圖 6b 分別展示了場(chǎng)景 1 中車輛的航向角和曲率變化，結(jié)果顯示：車輛的航向角和曲率變化均保持在合理范圍內(nèi)，確保了車輛的穩(wěn)定性。圖 6c 和圖 6d展示了場(chǎng)景 2 中車輛的航向角和曲率變化，車輛在多車共行時(shí)，通過(guò)精確的軌跡規(guī)劃，確保了變道操作的平穩(wěn)性，航向角和曲率沒(méi)有劇烈變化。圖 6e 和圖 6f 展示了場(chǎng)景 3 中車輛的航向角和曲率變化，車輛在 S 型彎道上的航向角和曲率變化均勻，確保了行車的舒適性。這些數(shù)據(jù)表明，本文算法能夠在不同復(fù)雜駕駛場(chǎng)景中保持穩(wěn)定的軌跡規(guī)劃，提升了駕駛的安全性和舒適性。

表 2 對(duì)比了3 種實(shí)驗(yàn)場(chǎng)景下不同路徑規(guī)劃算法的性能指標(biāo)，包括最大曲率、最大前輪轉(zhuǎn)角、平均曲率和平均前輪轉(zhuǎn)角等關(guān)鍵指標(biāo)。

結(jié)果顯示，加入預(yù)測(cè)的路徑規(guī)劃在所有指標(biāo)上均表現(xiàn)出顯著優(yōu)勢(shì)。預(yù)測(cè)其他車輛的行為使路徑規(guī)劃更加靈活，避免了突然的曲率變化和轉(zhuǎn)向，使得規(guī)劃出的路徑更加平穩(wěn)、合理。相比之下，未加入預(yù)測(cè)的路徑規(guī)劃可能導(dǎo)致路徑曲率波動(dòng)較大，車輛轉(zhuǎn)向頻繁，增加了駕駛的難度和不確定性。因此，加入預(yù)測(cè)的路徑規(guī)劃顯著提高了車輛行駛的安全性和舒適性，適應(yīng)復(fù)雜的交通環(huán)境，證明了其在實(shí)際應(yīng)用中的潛力和優(yōu)勢(shì)。

4"" 結(jié)""" 論

本文提出了一種適用于高速公路場(chǎng)景的實(shí)時(shí)軌跡規(guī)劃算法，該算法綜合考慮了自車與周圍車輛的交互情況。與現(xiàn)有研究相比，本文的方法在多個(gè)方面取得了顯著提升：

不僅提高了車輛軌跡預(yù)測(cè)的準(zhǔn)確性，還優(yōu)化了路徑規(guī)劃的安全性、舒適性和通行效率。通過(guò)引入規(guī)劃耦合模塊，本文的算法能夠更加精確地預(yù)測(cè)周圍車輛的行駛軌跡，從而規(guī)劃出更穩(wěn)定、更安全的行車路線。此外，本文在 Frenet 坐標(biāo)系下改進(jìn)了APF 算法，使其更適應(yīng)結(jié)構(gòu)化道路場(chǎng)景的需求，提高了算法的適用性和效率。

最后，將車輛軌跡預(yù)測(cè)模型的預(yù)測(cè)結(jié)果與改進(jìn)的人工勢(shì)場(chǎng)算法相結(jié)合，實(shí)現(xiàn)了路徑的動(dòng)態(tài)更新，進(jìn)一步提升了行車的安全性和舒適性。

然而，需要指出的是，本文所提出的自動(dòng)駕駛車輛軌跡規(guī)劃方法目前僅適用于高速公路場(chǎng)景。未來(lái)的研究將致力于擴(kuò)展該方法，以適應(yīng)更復(fù)雜的城市道路場(chǎng)景，如環(huán)形交叉口和十字路口等。