粒子群優化算法在智能車輛軌跡跟蹤的應用

丁志成，王甜甜

（1.鄭州工業應用技術學院機電工程學院，河南 鄭州 451100；2.鄭州西亞斯學院電子工程學院，河南 鄭州 451150）

1 引言

隨著自動化駕駛技術的不斷發展，智能車輛的控制研究正逐漸成為研究的熱點。路徑規劃和軌跡跟蹤都被視為自動駕駛技術的兩個重要任務，直接影響到車輛運行過程中的安全性。

國內外研究學者在自動駕駛車輛的路徑軌跡跟蹤方面開展了大量研究，當前在不確定性和干擾方面，模型預測控制（MPC）已經逐漸得到了廣泛的應用。針對自動駕駛汽車，文獻[2]建立了一個通用運動學和動力學運動模型，設計了基于MPC的主動轉向控制器。文獻[3]考慮到運動過程中的干擾，提出魯棒MPC控制器，以保證控制精度和穩定性。文獻[4]基于MPC模型提出了一種自適應軌跡跟蹤器，該控制器在不同車速工況下具有較小的跟蹤誤差，仿真結構表明該控制方法能有效控制軌跡跟蹤精度。文獻[5]針對車速和車道信息，提出了車輛自抗擾控制方法，仿真了不同工況下的軌跡跟蹤路徑，結構表明該方法具有較強的魯棒性能。文獻[6]針對農用車輛的軌跡控制，在單神經元理論基礎上提出了自適應PID路徑跟蹤方法，通過仿真表明該控制方法具有較高的控制精度和魯棒性。文獻[7]基于MPC模型研究了履帶式車輛軌跡的跟蹤策略，通過建立Bezier曲線的參考擬合路徑，設計了軌跡跟蹤控制器并進行實時反饋校正，該方法在實車試驗結果得到了有效驗證。文獻[8]提出了基于配點法的MPC對車輛的軌跡跟蹤問題進行了優化，該方法采用二次規劃序列方法進行求解覅線性動力學問題，在自動駕駛仿真環境中對所提方法的精度控制優勢進行了驗證。文獻[9]在MPC模型的基礎上研究了智能車輛在低附著條件下的軌跡跟蹤問題，設計了軌跡跟蹤器，通過仿真表明該跟蹤器具有良好的控制精度和魯棒性。在模型預測控制方面的研究還有比較典型的參考文獻[10-12]所述，這些研究成果為自動化駕駛技術的發展提供了良好的參考價值。

基于以上分析，本研究提出一種漸進模型預測控制方案（PMPCS）來同時處理車輛路徑規劃和軌跡跟蹤問題，提出了一種基于改進粒子群優化（IPSO）的模型預測控制（MPC）方法。

將模型預測控制與人工勢場（APF）相結合，將時變安全約束作為排斥力的范圍和非對稱的車道勢場函數，通過將時變安全約束視為排斥力的范圍和非對稱車道勢場函數來獲得無碰撞路徑。在規劃模塊和跟蹤模塊采用了相同的非線性運動學模型，設計了一種考慮交通信號燈約束的偽速度規劃策略，在此基礎上采用改進的粒子群算法（IPSO）來解決非線性MPC問題，最后通過仿真結果驗證了該算法的有效性，與一般算法相比具有明顯的優越性。

2 模型預測控制結構描述

2.1 車輛路徑規劃和跟蹤系統結構

路徑規劃和跟蹤系統的結構原理圖，如圖1所示。

圖1 路徑規劃和跟蹤系統結構示意圖Fig.1 Structure Diagram of Path Planning and Tracking System

系統的輸入包括環境信息、智能車輛的當前狀態和最新控制時間的最優解。同時，控制層需要一個合適的求解范圍，該范圍可以設置為合適的常數值，也可以根據路徑的曲率進行計算。

本研究提出了基于IPSO模型的預測控制算法，以尋找規劃和控制模塊的解決方案。該方案能有效地傳遞中間變量，減少了計算量。與已有結果不同，本研究還考慮了交通信號燈的影響。為了在綠燈時安全地通過十字路口，需要計算新的參考速度，以確保車輛能在預設時刻之前到達目標點。

2.2 車輛運動模型參數化描述

（1）車輛運動學模型：路徑規劃和跟蹤過程根據車輛運動學模型計算最優控制序列。假設車輛為剛體，側滑角β為0，圖2中參考坐標的數學表達式為：

圖2 車輛運動學模型Fig.2 Vehicle Kinematics Model

式中：[x，y，φ]T—在車輛中心的位置和偏航角；［v，δf］T—控制輸入，即速度和前輪轉向角；L—軸距。

通過歐拉法對式（1）離散為：

（2）障礙物車輛描述：為保證所生成路徑的安全，相鄰障礙物的運動狀態可用兩自由度運動學模型來描述：

其中，輸入為第i個障礙物的角速度ωobs和切向速度vobs，狀態為位置和航向角。

（3）縱向安全距離：在典型超車場景中，智能車輛以較高的速度行駛在障礙車后面。規劃模塊需要生成無碰撞路徑，以便安全超車。智能車輛在超越障礙物后應沿原車道中心行駛。為了確定合適的超車距離，將安全距離Ds定義為[13]：

式中：vh—智能汽車的車速；vobs—障礙車輛的車速；amax—智能車輛的最大減速帶；aobs，max—障礙車輛的最大減速度，對緊急情況有顯著影響；tx—系統的響應延時，兩車之間的最小縱向距離用d0表示。

3 基于模型預測控制的軌跡規劃

由于交通的動態特性，在樣本周期T內需要計算智能車輛的動態響應。本研究提出了APF方法來處理時變安全約束，并將勢函數作為代價函數的懲罰項。

3.1 時變安全約束

為防止車輛與障礙物、道路邊界相干涉，應約束智能車輛的橫向位移，使車輛保持在車道中心行駛，則存在以下約束條件：

其中，ymin和ymax—最小和最大橫向位移。此外，受轉向系統和電機的限制，車輛的控制輸入也受到限制，即：

其中，vmin和vmax受交通規則限制。當智能車輛接近障礙物時，需要考慮安全距離約束［14］：

式中：xr，i（k）—在時間k時，兩車沿縱向之間的相對距離；yr，i（k）—兩車沿橫向之間的相對距離；Lx—縱向安全距離，等于前述的Ds。Ly—橫向安全距離。在式（7）中，如果選擇左側車道超車且左側車道可用，則不等式符號為“-”，否則為“+”

3.2 與車道相關的勢場函數

為了找到一個無碰撞的路徑，在考慮到上述約束條件下，首先將安全約束轉換為勢場的范圍［15］（表示不可駕駛區域，并將該區域定義為Ω），如式（8）所示。此時，不等式的符號由“≤”替換為“≥”，得到不可行區域。此外還應解決輸出約束問題，勢場的范圍，如圖3所示。

圖3 障礙物的勢場描述Fig.3 Description of the Obstacle Potential Field

在本研究中，ymin，Ly（k）和ymax，Ly（k）需要滿足橫向安全距離。此外，智能車輛與第i個障礙車輛的相對距離用do，i表示，在該區域中，排斥函數可以表述為：

其中，Ao和γo分別表示為勢場的幅度和標準差。此外，還定義了車道勢場函數。為了從左側超車，對要超車一側的車道線給予較低的權重。通過這種方式，可以通過啟發式的方式將決策過程部分引入路徑規劃模塊中，如圖4所示。

圖4 不同振幅車道邊界的APF值Fig.4 APF Values of the Lane Boundaries with Different Amplitude

與障礙物勢場函數類似，車道勢場定義為：

式中：γr—車道收斂系數；Ar，j—第j車道的勢場幅值，本研究只考慮dr，j（k）≤Ds時的車道勢函數，即車輛超車。

3.3 特定情況下的參考速度計算

本研究考慮典型的運行工況，當司機駕駛車輛靠近交通燈附近時，會進行適當的加速∕減速動作，為了使所設計的路徑規劃算法達到與人類駕駛員相似的性能，在分析過程中做一些簡化。

首先，將交通燈問題視為避障物問題，即紅燈可以描述為某一時刻發生的靜態障礙物（燈障）。進一步構造二維狀態空間ΨTL，包括時間t和縱向位置x，可以定義為：

同樣地，燈障狀態OTL被描述為：

式中：s、Δt—光障的縱向位置和持續時間。因此，涉及燈障的避障問題可表述為：

即車輛q'的實際狀態不屬于車輛狀態所描述的障礙狀態集，因此可以在優化過程中引入狀態約束來實現避障。為了簡化避障問題，本研究構建車輛縱向距離的時序圖，如圖5所示。紅線和綠線表示交通信號燈的狀態，藍線表示車輛運行狀態。

圖5 不同速度下的交通燈狀態Fig.5 Traffic Light Status at Different Speeds

圖中虛線斜率較大，該狀態可以在紅燈狀態前通過目標點，同時保持相同的斜率。實線由于速度較慢，需要等待紅燈狀態（斜率為0，速度為0），當信號燈變為綠色時，車輛將以原來的相同速度運行。

如果可以通過V2X或車載傳感器得到交通燈與智能車輛的相對距離Sleft，以及剩余的通過時間Tleft，則可計算出最小速度為：

其中，Atra—增益系數，使參考速度略高于最小速度；Vref，old—最新樣本周期計算出的參考速度。常數ε是在剩余時間不足時，避免超速的閾值（在這種情況下，駕駛員可能會減速或者停車）。此外，考慮到超車時間的約束，可以通過增大初始速度以使得車輛加速，直到超車動作結束，因此有：

式中：γv，o—障礙車輛速度收斂系數；γv，tra—交通信號燈速度收斂系數；ζ—指定的距離，用于確定是否需要考慮紅綠燈；α、ρ—加速因子。

3.4 軌跡規劃的模型預測控制

為了得到無碰撞路徑，提出的優化問題為：

成本函數為：

式中：Np—預測層；Nc—控制層；Ψ—預測狀態；U—控制輸入，表述為：

在每個采樣瞬間k處，將計算出最優控制序列，并得到預測范圍內相應的車輛狀態Np。為了達到跟蹤的目的，使用這兩個序列來初始化跟蹤控制器的優化問題和跟蹤控制器的參考路徑。這種處理方法允許在兩層優化之間進行無縫連接，從而可以大大減少計算量。

4 跟蹤控制器設計

本節針對PMPCS設計了一種基于改進的粒子群優化算法的MPC跟蹤控制器，傳統的MPC方法需要根據成本函數預測狀態并計算最優控制，計算量較大。因此，一般采用上層基于MPC的規劃控制算法來提供滿足要求的參考控制量。另一方面，我們使用諸如PID這樣的低計算負擔的控制算法在底層實現跟蹤過程。

4.1 軌跡跟蹤的模型預測控制

與上述規劃部分類似，本研究設計了特定的橫向控制器，采用相同的運動學模型來預測智能車輛的未來狀態，以確保車輛能夠跟蹤參考路徑。由于優化準則和優化目標不同，因此需要設計跟蹤控制器。為了在跟蹤過程中獲得良好的性能，提出以下優化問題：

式中：Ψt—控制模塊的預測狀態；Ψp—參考狀態；Uc—控制輸入。定義成本函數為：

其中第一項的目的是使輸出狀態盡可能接近參考狀態，第二項是控制輸入增量的約束，以保證乘客的舒適性。Ξ和Θ是每部分的權重。較小的J表示控制器輸出位置與規劃軌跡的偏差趨于0，轉向更平穩。

4.2 基于改進粒子群優化的控制方案

考慮式（16）和式（19）中的優化問題，采用粒子群優化算法（PSO）將兩個獨立的粒子初始化為控制輸入δf和v，由式（2）計算相應的狀態輸出，并將適應度定義為成本（20）的負值。選取全局最優粒子來更新粒子，當算法迭代次數達到最大或滿足適應度閾值時，輸出執行器的最優控制序列。速度和位置的更新公式可表示為：

其中，在p.δ和p.S是兩個獨立的粒子，分別對應于控制輸入δf和v。p.vδ和p.vs表示相應的速度。正整數c1，c2，c3，c4表示學習因子。r1、r2、r3、r4是在區間［0，1］內隨機分布的常數。νδ和νs分別代表δf和v的慣性因子，pb和gb分別為當前迭代的最佳粒子和全局最佳粒子。

為了解決優化問題，將每個粒子的長度擴展到Nc來計算成本，對每個粒子進行迭代計算，以找到最優粒子。在每個樣本周期內，最優解不會發生顯著變化，本研究存在兩個改進的方面：

（1）PMPCS：PMPCS是一種漸進式方案，具有規劃和跟蹤的能力，可以在兩層之間有效地傳輸信息。在該方案中，規劃模塊從最優問題和運動學模型中計算出最優序列和對應的狀態，然后將每個采樣周期的狀態作為跟蹤模塊的參考。由于相同的預測模型和相同的優化變量，這兩個優化解位于狹窄的區域，使得初始粒子成本較低。

由于多項式的導數是一階和二階連續的，本研究采用四次多項式擬合參考偏航角和橫向位移隨預測水平Np的變化曲線，即加速度和角加速度是連續的，則有：

采樣獲取的擬合曲線，如圖6所示。可以看出這些曲線的偏差較小，可作為下一次優化的初始值。對于每個粒子，可以根據給出的初始值U'=［U（2：Np），0］進行迭代計算，在后續的迭代中將基于最新的最優解開始，以減少搜索空間。

圖6 規劃模塊在超車過程中計算的參考路徑Fig.6 Reference Paths Calculated by the Planning Module During Overtaking

（2）改進粒子群算法（Improved PSO）：為了提高粒子群算法（PSO）的收斂速度，采用以下公式來計算線性慣性因子［17］：

式中：k—迭代次數；M—最大迭代次數；wmax—慣性因子的最大值；wmin—慣性因子的最小值，一般取值為0.9和0.4。

為了驗證所提出的IPSO算法能夠有效地加快收斂速度，本研究采用式（24）進行優化：

在模擬中，最大迭代次數為500，種群大小為50。迭代終止條件為適應度誤差小于0.00001或達到最大迭代量。兩種優化算法的函數圖像和收斂速度，如圖7所示。可以看出IPSO可以更快地達到較優的適應度，而PSO的迭代精度較差。10次測試數據的均值，如表1 所示。可以看出PSO 算法的平均迭代次數為339.3，遠遠大于IPSO算法的84.9。因此可以看出IPSO算法可以有效地提高算法的收斂速度。

表1 PSO與IPSO的比較Tab.1 Comparison for PSO and IPSO

圖7 粒子群優化算法的比較Fig.7 Comparison of Particle Swarm Optimization Algorithms

5 仿真結果

本研究在所提模型的基礎進行仿真，以驗證算法的有效性，參數，如表2所示。如果首選左側超車，可以將Ar，2=0和Ar，3=15，否則選擇Ar，2=15，Ar，3=0。

表2 算法的設計參數Tab.2 Design Parameters of Algorithm

超車軌跡，如圖8 所示。車輛以18m∕s 的速度在中心車道行駛，障礙車輛以12m∕s 的速度在同一車道行駛，直道長150m，首選從左側超車。智能車輛采用紅線和符號表示，障礙用黑色標記表示。可以看出兩輛車沒有發生碰撞，智能車輛也很少偏離車道中心。

圖8 路徑規劃的仿真結果Fig.8 Simulation Results for Path Planning

在仿真中還考慮了交通燈，交通燈距離原點80m，綠燈在前4s工作。在圖9（a）中，實線表示智能車輛首先加速通過紅綠燈，即實現穿過綠色部分，虛線穿過紅色部分。隨后速度下降到最初的參考速度。每個采樣瞬間的最佳參考速度，如圖9（b）所示。

圖9 考慮交通燈的仿真結果Fig.9 Simulation Results by Considering Traffic Lights

為了證明控制算法的有效性，提出了Simulink∕CarSim 聯合仿真。場景是在慣性坐標系上跟蹤一條幅度為3.6m 的步進軌跡，即跟蹤一條初始誤差為3.6m的直線。縱向速度為5m∕s。控制性能，如圖10所示。平均橫向誤差為0.0272m，可以看出所提的IPSO-NMPC 跟蹤控制器是有效的。

圖10 跟蹤算法的仿真結果Fig.10 .Simulation Results of Tracking Algorithm

優化后的規劃結果與最終跟蹤結果的對比，如圖11 所示。其中圖11（a）、圖11（b）為規劃和跟蹤結果。由于跟蹤算法限制了控制增量的幅度，因此速度曲線存在較大差異。結果表明，當參考速度變化較大時，輸出速度呈斜坡狀。

圖11 協同控制仿真結果Fig.11 Simulation Results for Cooperative Control

從圖11（c）可以看出，最終的輸出曲線比計劃的輸出曲線更加平滑。在不考慮規劃模塊傳遞到跟蹤參數（這種情況相當于給定跟蹤軌跡）的情況下，圖11（d）和圖11（e）顯示了相應的計算時間，可以看出由于更大的搜索空間和迭代次數，計算時間明顯高于PMPCS。這表明所提的協調策略和IPSO 方法大幅減少了計算時間。

6 結論

本研究針對智能車輛的局部規劃與路徑跟蹤的協同控制問題，提出了一種基于改進粒子群優化（IPSO）的模型預測控制（MPC）方法。在規劃模塊中提出了由時變約束確定的非對稱車道勢場函數和障礙場函數的范圍，以獲得無碰撞路徑。

在此基礎上，將APF與IPSO-MPC相結合，采用偽速度規劃方法對交通燈進行規劃，從而有效地解決了路徑優化問題。采用考慮線性慣性因素的粒子群算法（IPO），在解決非線性規劃和系統計算效率方面具有一定優勢。最后該算法的有效性通過仿真得到了驗證。