基于ⅠPSO-MPC的無人駕駛車輛縱向運動控制

李廣南，葉洪濤，羅文廣

（1.廣西科技大學(xué) 電氣電子與計算機科學(xué)學(xué)院，廣西 柳州 545616；2.廣西自動檢測技術(shù)與儀器重點實驗室（桂林電子科技大學(xué)），廣西 桂林 541004；3.廣西汽車零部件與整車技術(shù)重點實驗室（廣西科技大學(xué)），廣西 柳州 545006）

0 引言

運動控制是無人駕駛車輛研究領(lǐng)域中的核心問題之一，車輛根據(jù)周圍環(huán)境以及車輛狀態(tài)信息向油門、剎車以及轉(zhuǎn)向系統(tǒng)等發(fā)出控制指令。其中，縱向控制主要研究車輛的速度跟蹤能力，控制車輛按照預(yù)定的速度行走是實現(xiàn)車輛穩(wěn)定跟蹤期望軌跡的關(guān)鍵。

模型預(yù)測控制（model predictive control，MPC）具有預(yù)測模型、滾動優(yōu)化和反饋校正等特點，適用于不易建立精確數(shù)學(xué)模型且存在約束條件的控制。文獻[2]提出了一種模型預(yù)測控制（MPC）框架的自動地面車輛速度跟蹤的控制方法，結(jié)合簡單的縱向逆動力學(xué)模型和MPC 自適應(yīng)調(diào)節(jié)，實現(xiàn)車輛縱向速度的控制。文獻[3]提出了一種分層控制架構(gòu)下的智能電動汽車縱向跟車運動自適應(yīng)模糊滑?？刂品椒?，上位控制器由滑?？刂破髋c自適應(yīng)模糊控制器組成，下位控制器根據(jù)控制方式由縱向動力學(xué)逆模型計算出期望力矩，實現(xiàn)對加速度期望值的跟蹤。同樣地，文獻[4]使用車輛縱向運動的上下位分層控制器結(jié)構(gòu)，在上位控制器中提出改進的MPC 算法，下位控制器根據(jù)期望加速度求解發(fā)動機節(jié)氣門開度和制動壓力。

眾多研究學(xué)者還將其他優(yōu)化算法加入縱向運動控制中。文獻[5]引入了一種帶有混沌初始化的粒子群優(yōu)化算法（CPSO），將其應(yīng)用到模型預(yù)測控制中，用于解決同時帶有輸入約束和狀態(tài)約束的控制問題。文獻[6]設(shè)計了基于PSO 算法的模型預(yù)測控制器，通過粒子（MPC中的參數(shù)、）和該粒子所對應(yīng)的適應(yīng)值（控制系統(tǒng)的性能指標(biāo)）建立聯(lián)系，能夠有效地對車輛軌跡進行跟蹤。文獻[7]通過數(shù)值驗證，證明使用一種改進的粒子群算法可以降低計算迭代次數(shù)，減少MPC計算成本。

縱向控制可分為直接式和分層式2 種控制方式，為了便于研究控制系統(tǒng)，本文采用分層控制結(jié)構(gòu)。在上位控制器中，針對二次規(guī)劃求解優(yōu)化問題不夠精確的問題，對基本粒子群算法進行改進，由此設(shè)計上位控制器輸出期望的加速度，下位控制器運用逆縱向動力學(xué)輸出期望節(jié)氣門開度和期望制動壓力，對車輛速度跟蹤效果問題進行研究。

1 無人駕駛車輛縱向控制器策略

無人駕駛車輛的縱向速度控制器框圖如圖1所示。根據(jù)參考速度以及道路信息，縱向上位控制器對參考速度進行跟蹤控制，通過目標(biāo)函數(shù)得到控制量，輸出車輛的期望加速度給下位控制器。下位控制器運用切換邏輯以及逆縱向動力學(xué)輸出期望節(jié)氣門開度信號α或期望制動主缸壓力信號，控制無人駕駛車輛跟蹤期望的速度。

圖1 無人駕駛車輛縱向速度控制器框圖

2 縱向控制器設(shè)計

2.1 上位控制器設(shè)計

車輛在行駛過程中，假設(shè)沒有橫擺運動對車輛縱向運動控制的影響，近似認(rèn)為車輛的加速度等同于縱向速度的導(dǎo)數(shù)，利用一階慣性系統(tǒng)來表示：

其中：a為縱向加速度，為期望加速度，K為系統(tǒng)增益，τ為時間常數(shù)。

考慮速度與加速度之間的關(guān)系，速度跟蹤控制用連續(xù)系統(tǒng)的狀態(tài)方程表示：

系統(tǒng)輸出量為車輛速度v(k)：

式中：C=[ 1 0 ].

在無人駕駛車輛控制系統(tǒng)中，車輛的控制目標(biāo)是在保證車輛不發(fā)生過于劇烈的加速度和加速度變化率的前提下，保證速度跟蹤精度，因此，將性能評價函數(shù)定義為：

式中：k -1 為上一采樣時刻，為預(yù)測步長，為控制步長，(k+i|k) 為控制輸出預(yù)測值，(k+i|k)為控制輸出變量參考值，(k+i|k)和△u(k+i)分別表示根據(jù)k 采樣時刻的狀態(tài)信息來預(yù)測第k+i 時刻的信息和第k+i 時刻的控制輸入增量，其中i=1,2,…,；Q為系統(tǒng)輸出量的權(quán)重系統(tǒng)矩陣，W為系統(tǒng)控制增量的權(quán)重系統(tǒng)矩陣。

在控制器跟蹤速度過程中，需要考慮添加主動約束，即控制過程中的加速度約束及其變化率約束，保證其約束在合理的范圍之內(nèi)，其不等式表達如下：

加速度變化率約束表達形式為：

其中：和為縱向加速度閾值，Δ和Δ為縱向加速度變化量閾值，u(k+i)是k+i時刻的控制輸入。

系統(tǒng)在每個周期完成對優(yōu)化問題的求解，得出每個周期一系列的最優(yōu)解控制輸入增量ΔU，并將第一個控制增量作為系統(tǒng)的實際輸出變化量，加入系統(tǒng)中。在新的時刻，系統(tǒng)根據(jù)系統(tǒng)狀態(tài)，重新預(yù)測下一時刻的一系列控制增量，不斷在線滾動優(yōu)化，直到完成控制過程。

2.2 二次優(yōu)化問題的轉(zhuǎn)化

結(jié)合性能要求，優(yōu)化問題可以轉(zhuǎn)化為標(biāo)準(zhǔn)的二次規(guī)劃（QP）問題進行求解，式（7）可以簡化成以下形式：

2.3 下位控制器設(shè)計

下位控制器的作用是根據(jù)期望加速度對車輛進行準(zhǔn)確控制，該控制會涉及到車輛制動和驅(qū)動模式的切換過程，以及將控制量轉(zhuǎn)換為執(zhí)行器控制輸入。

考慮空氣阻力、滾動阻力以及坡度阻力，將車輛阻力需求加速度方程用式（9）表示：

式中：m為車輛質(zhì)量，為阻力需求加速度，為阻力合力，為滾動阻力，為空氣阻力，為坡度阻力。

坡度阻力為汽車重力沿坡道的分力，在Cаr-Sim中設(shè)置了坡度為的直線道路，其中=tаn α，α 為坡角。因此，考慮坡度較小時，重力加速度g取9.8 m/s，坡度阻力：

滾動阻力為：

式中：為滾動阻力系數(shù)。

空氣阻力為：

式中：為空氣阻力系數(shù)；ρ為空氣密度，正常的干燥空氣可取1.29 kg/m；為車輛迎風(fēng)面積。假設(shè)車輛在行駛過程中，以上參數(shù)均不發(fā)生變化。

在設(shè)計邏輯切換的過程中，要保證制動和驅(qū)動模式不能同時起作用，也要根據(jù)現(xiàn)實情況，不能頻繁地切換制動和驅(qū)動模式。因此，根據(jù)期望加速度定義以下邏輯切換：

式中：α為期望節(jié)氣門開度，為期望制動主缸壓力。

經(jīng)過驅(qū)動模式或制動模式的邏輯切換時，當(dāng)期望加速度大于等于阻力需求加速度時，切換為驅(qū)動模式?？紤]車輛內(nèi)部的旋轉(zhuǎn)構(gòu)件換算質(zhì)量、道路坡度阻力、空氣阻力以及滾動阻力，可以得到驅(qū)動模式下車輛的驅(qū)動力為：

式中：為旋轉(zhuǎn)質(zhì)量換算系數(shù)，通過經(jīng)驗公式得到。

因此得發(fā)動機所需的輸出扭矩為：

式中：為車輪有效半徑，為變速器減速比，為主減速器傳動比，η為傳動系機械效率。根據(jù)車輛發(fā)動機轉(zhuǎn)速n、節(jié)氣門開度、發(fā)動機輸出扭矩三者的關(guān)系，以及轉(zhuǎn)速和有效扭矩，通過發(fā)動機扭矩可得到節(jié)氣門開度α為：

當(dāng)邏輯切換為制動模式時，考慮道路坡度阻力、空氣阻力以及滾動阻力，當(dāng)前車輛需求制動力為：

期望的制動主缸壓力計算為：

式中：為制動力與制動主缸壓力的比值。

通過制動驅(qū)動模式切換以及計算，得到期望節(jié)氣門開度α和期望制動主缸壓力，將控制量作為執(zhí)行器控制輸入，控制車輛根據(jù)期望的速度行駛。

3 優(yōu)化PSO結(jié)合的MPC算法

采用模型預(yù)測控制進行速度跟蹤的基本思想是車輛在根據(jù)期望速度行駛的過程中，將最優(yōu)化問題轉(zhuǎn)化為一個帶約束的二次規(guī)劃問題進行求解。針對原本QP 算法在求解過程中出現(xiàn)精度不足的問題，本文利用改進型的粒子群優(yōu)化算法對性能指標(biāo)進行優(yōu)化，解出控制律。

3.1 算法介紹

在粒子群算法中，假設(shè)在一個S維的目標(biāo)搜索空間中，有M個粒子組成一個粒子群體，粒子在S維的搜索空間中以一定的飛翔速度搜索，每一個粒子所在的位置都是一個潛在的解，粒子根據(jù)目標(biāo)函數(shù)計算出其適應(yīng)值，根據(jù)適應(yīng)值的大小判斷該粒子的優(yōu)劣性。在S 維中，第i 個粒子的位置為：=[,,…,]；第i 個粒子的速度表示為：=[,,…,]，1 ≤i ≤S。粒子在游歷過程中，第i個粒子經(jīng)歷過的歷史最好點，即個體極值表示為：=[,,…,]；整個粒子群迄今為止搜索到的最好的位置，即群體極值表示為=[,,…,]，g{1 ,2,…,M }。

空間中各粒子隨著每一次的迭代都會不斷地更新自己的狀態(tài)，其速度和位置的更新方程為：

式中：ω為慣性因子，其大小可以保證粒子的搜索能力，ω 較大時可以保證全局的搜索能力，反之，局部搜索能力較強；正常數(shù)和稱為學(xué)習(xí)因子，分別調(diào)整粒子飛向自身、全局最好位置方向的步長；和為互相獨立且均勻分布于[0，1]的2個隨機數(shù)。

設(shè)f (x)為最小化的目標(biāo)函數(shù)，則第i個粒子當(dāng)前位置(t+1)由以下公式確認(rèn)：

3.2 改進粒子群算法

由于PSO 在優(yōu)化過程中會向自身歷史最佳位置以及群體中歷史最佳位置靠近，而且其優(yōu)化速度跟其初始化設(shè)置參數(shù)有很大聯(lián)系，使得粒子容易陷入局部最優(yōu)或停滯，導(dǎo)致輸出結(jié)果波動較大。因此，為了提高粒子群算法效率，對粒子群算法進行改進。

3.2.1 添加壓縮因子

在優(yōu)化過程中，算法容易陷入局部尋優(yōu)中，會直接影響算法的性能，從而導(dǎo)致優(yōu)化出的參數(shù)不精確。為確保算法的收斂性，在粒子的每一步移動過程中添加壓縮因子，不僅能夠滿足收斂速度，而且收斂精度大大提高。對粒子更新方程進行如下改進：

3.2.2 調(diào)整慣性權(quán)重

在MPC 計算求解過程中要求時間越短越好，在粒子群算法中需要多次迭代收斂才能尋找到最小值。慣性權(quán)重ω 是PSO 優(yōu)化算法的可調(diào)參數(shù)之一，其大小控制了迭代過程中歷史因素對當(dāng)前狀態(tài)的影響程度。為了更好地平衡算法的全局搜索與局部搜索能力，加入了隨機權(quán)重策略ω，使得粒子的歷史速度對這一時刻速度具有隨機影響性，克服ω的線性遞減所帶來的不足。ω的取值為：

式中：μ為隨機權(quán)重平均值的最小值，μ為隨機權(quán)重平均值的最大值，為隨機權(quán)重的方差，N(0,1)表示標(biāo)準(zhǔn)正態(tài)分布的隨機數(shù)，rаnd(0,1)表示0～1的隨機數(shù)。

3.2.3 調(diào)整學(xué)習(xí)因子

在基本的粒子群算法中，認(rèn)為粒子在尋找最優(yōu)值的過程中，其自我學(xué)習(xí)能力及群體的學(xué)習(xí)能力是一樣的，即學(xué)習(xí)因子、的取值是固定的；但是在迭代初級階段，粒子具有較強的自我學(xué)習(xí)能力，而在迭代后期，粒子需要較強的社會學(xué)習(xí)能力去快速收斂，尋找最優(yōu)值。因此，學(xué)習(xí)因子在算法求解過程中應(yīng)呈線性遞增形式，學(xué)習(xí)因子在算法求解過程中呈線性遞減形式。

式中，為當(dāng)前迭代次數(shù)，為最大迭代次數(shù)(=1,2,…,)。但是在求解適應(yīng)度函數(shù)值時，其收斂曲線并不是呈線性遞減的直線而是呈指數(shù)函數(shù)趨勢遞減的形式，這會導(dǎo)致學(xué)習(xí)因子的遞增遞減程度與適應(yīng)度函數(shù)變化程度不一致的現(xiàn)象。因此，將線性遞增遞減的曲線修改為呈指數(shù)函數(shù)趨勢的曲線，與適應(yīng)度函數(shù)變化曲線相匹配，設(shè)置學(xué)習(xí)因子形式如下所示：

3.3 ⅠPSO-MPC算法流程與步驟

經(jīng)改進后的粒子群優(yōu)化算法，其算法流程如圖2所示。

圖2 粒子群算法流程圖

將粒子群算法和MPC相結(jié)合，把輸入向量Δ作為優(yōu)化變量，則粒子群中各個粒子的維數(shù)等同于預(yù)測的控制時域；目標(biāo)函數(shù)選取式（8）計算各個粒子的適應(yīng)度；由于這一時刻的優(yōu)化變量的最優(yōu)解與上一時刻優(yōu)化變量的最優(yōu)解的變化不大，為保證優(yōu)秀粒子的影響力，將上一時刻的控制序列作為下一時刻粒子的初值，提高算法的效率。算法的具體步驟如下：

初始化MPC參數(shù)以及粒子群參數(shù)，采集第k時刻Cаrsim輸出的系統(tǒng)狀態(tài)量信息；

根據(jù)車輛模型、狀態(tài)量和輸入量，計算出優(yōu)化問題中的矩陣和，以及目標(biāo)函數(shù)J(△u(k))；

運用粒子群優(yōu)化算法進行求解，根據(jù)目標(biāo)函數(shù)以及約束，進行迭代計算，直到滿足終止條件，得到該控制時域內(nèi)的一系列控制輸入增量Δ；

將該控制序列中第一個元素△u(k)作為實際的控制輸入增量作用于系統(tǒng)中；

令k=k+1，循環(huán)執(zhí)行，直到系統(tǒng)完成控制過程。

4 仿真驗證

為驗證粒子群算法結(jié)合MPC 算法的有效性，通過搭建Simulink/CаrSim 聯(lián)合仿真平臺，對所設(shè)計的控制器進行仿真驗證，并與MATLAB 提供的QP算法進行比較分析。在MPC控制器中，預(yù)測步長=30，控制步長=2，采樣時間=0.02s，=-5 m/s，=3.5m/s，Δ=-5 m/s，Δ=5 m/s，系統(tǒng)輸出量的權(quán)重系統(tǒng)系數(shù)Q=200，控制增量的權(quán)重系統(tǒng)矩陣=2。粒子群算法中的參數(shù)數(shù)值為：最大迭代次數(shù)=100，粒子群數(shù)目M=30，學(xué)習(xí)因子=2.1，=2，φ=4.1，μ=0.5，μ=0.8，=0.2，壓縮因子ψ=0.729 8，==0.5，==3.5。由于不同的路面附著系數(shù)對車輛的控制量輸出有較大的影響，因此，將附著系數(shù)設(shè)置為0.85，并考慮車輛行駛坡道情況，設(shè)置在坡度=0.05的工況下進行測試。實際測試中，車輛空氣阻力系數(shù)數(shù)值為0.28～0.40。車輛迎風(fēng)面積數(shù)值為1.4～2.6 m，良好路面的滾動阻力系數(shù)為0.008～0.030。綜合考慮，模型參數(shù)取值為：滾動阻力系數(shù)=0.016，空氣阻力系數(shù)=0.280，車輛迎風(fēng)面積=2.51 m，=0.379 m，=4.1，η=0.95，=1 350 N·m/Pа。仿真測試選用的車型為DClаss，SUⅤ，車輛具體參數(shù)如圖3所示。

圖3 車輛參數(shù)

在有坡度的道路工況下，驗證控制器的有效性，設(shè)置了包含不同加速度的加速過程與減速過程的期望速度曲線，車輛根據(jù)該期望速度曲線進行速度跟隨控制，對控制算法進行仿真驗證，其車輛速度比較以及速度誤差圖如圖4所示。

圖4 跟蹤速度效果

如圖4所示，車輛的實際行駛速度與參考車速基本上重合，3種算法都能實現(xiàn)很好的跟蹤，在加速以及減速過程中會有較小的超調(diào)量，但隨后很快跟隨期望速度進行行駛。但在3種算法中，ⅠPSO算法的誤差要比PSO和QP算法小，在加速過程中最大的跟隨誤差為0.392 2 m/s，減速過程的最大跟隨誤差為0.283 8 m/s，且整體的跟蹤誤差結(jié)果效果更好?？刂破魉俣雀櫿`差如表1所示。

表1 不同算法的速度跟蹤誤差 單位：m/s

在ⅠPSO 控制器算法中，節(jié)氣門開度和制動壓力控制量如圖5所示，并沒有出現(xiàn)頻繁切換驅(qū)動制動模式及同時起作用的情況，滿足控制的需求。

圖5 控制器輸出量

同時，為控制系統(tǒng)的實時性，記錄單次解算時間，從控制器獲取車輛狀態(tài)信息進行問題的構(gòu)建和分析求解，得到輸出控制量之間的時間長度。

對有坡度的道路工況中每個控制周期的單次解算所需時間進行統(tǒng)計，如圖6 所示，加了ⅠPSO 優(yōu)化算法的MPC 控制器的計算時間約20 ms。由于ⅠPSO 算法是基于MPC 框架，需要進行優(yōu)化求解，單次解算時間相對于QP 時間要長，但遠(yuǎn)小于無人駕駛車輛實時控制周期50 ms，故滿足實時性要求。

圖6 每個周期所需時間

5 結(jié)論

本文針對基于模型預(yù)測控制方法的無人駕駛車輛縱向控制中的速度跟蹤精度問題，利用一種改進型的粒子群優(yōu)化算法求解該問題?？v向速度控制采用分層控制結(jié)構(gòu)，上位控制器采用一階慣性環(huán)節(jié)預(yù)測車輛狀態(tài)信息，利用ⅠPSO 優(yōu)化算法求解目標(biāo)函數(shù)的最優(yōu)解，得出車輛的期望加速度；下位控制器通過逆縱向動力學(xué)模型得出期望節(jié)氣門開度以及制動主缸壓力，在Simulink/CаrSim 建立了聯(lián)合仿真平臺進行驗證，在有坡度的道路工況下能夠有效地使車輛實現(xiàn)縱向加速、勻速和減速的速度跟隨，并且控制車輛速度的最大誤差減小了0.274 7 km/h，符合控制實時性的要求，證明了改進型控制器的有效性。