孫柯陽,王兆強,梁威,劉斌山,孫崇智

（1.上海工程技術(shù)大學機械與汽車工程學院,上海 201620；2.甘肅省特種設備檢驗檢測研究院,甘肅 蘭州 730050）

自動駕駛汽車因具有可靠性高、行駛速度高及通行效率高等特點,而受到政府與汽車相關產(chǎn)業(yè)的廣泛關注[1].自動駕駛系統(tǒng)主要有車輛定位與環(huán)境感知、決策規(guī)劃與跟蹤控制三大模塊[2].跟蹤控制模塊作為自動駕駛核心模塊之一,保證了車輛在不同環(huán)境和車況下,可以準確、穩(wěn)定及魯棒地跟蹤參考路徑[3].其中路徑跟蹤模塊在保證行車安全與操作穩(wěn)定性方面占有主導地位, 因此對于路徑跟蹤控制器的研究有著實際的應用價值.

在真實的環(huán)境中,自動駕駛車輛不會完全按照規(guī)定的路線行駛,實際行駛路徑和規(guī)劃路徑之間存在誤差,控制器的作用就是盡可能地降低此類誤差.現(xiàn)有的路徑跟蹤控制算法有：斯坦福大學開發(fā)的stanly控制算法、純跟蹤控制算法（PP）、PID 控制算法、線性二次調(diào)節(jié)器（LQR）、滑?？刂扑惴ǎ⊿MC）及模型預測控制算法（MPC）等,其中線性二次規(guī)劃算法與PID 控制算法由于其良好的魯棒性和精確性被廣泛地應用于自動駕駛跟蹤控制系統(tǒng)中.本文設計了一種基于模糊線性二次規(guī)劃算法和PID 控制算法的聯(lián)合跟蹤控制器,運用Carsim/simulink 進行聯(lián)合仿真,驗證了控制器的有效性.

1 動力學誤差模型

1.1 動力學模型

車輛是由發(fā)動機、車身及底盤所組成的復雜的非線性系統(tǒng),用忽略了力學影響的車輛運動學模型難以精確描述車輛的運動特性,因此本文搭建了車輛動力學模型,且根據(jù)阿克曼轉(zhuǎn)向幾何原理將四輪模型簡化為二自由度的自行車模型.簡化后的動力學模型如圖1 所示.

圖1 車輛動力學模型Fig.1 Vehicle dynamics model

圖1 中a 和b 分別為前后軸到車輛質(zhì)心的距離,Vf和Vr是前后車輪的速度.由于前后輪所受側(cè)向力Fyf 和Fyr 的影響,前后輪的速度方向相對于輪胎指向發(fā)生了一定的偏轉(zhuǎn),因此產(chǎn)生的偏轉(zhuǎn)角αf和αr即為前后輪的側(cè)偏角,δ 是車輛的前輪轉(zhuǎn)角,V 是車輛質(zhì)心處的速度.對車輛整體做受力分析有

式（1）（2）中：m 為整車質(zhì)量,Iz為汽車繞大地坐標系Z 軸的轉(zhuǎn)動慣量,ay為車輛在自身坐標系y 軸方向上的加速度,φ 為汽車的橫擺角,為汽車的角加速度,為了防止高速行駛時轉(zhuǎn)向過度導致車輛側(cè)翻,前輪轉(zhuǎn)角δ 一般較小,因此式（1）和（2）可以化簡為

考慮到前輪轉(zhuǎn)角δ 和輪胎側(cè)偏角α 較小,根據(jù)圖1 所示關系,可以得到如下公式

式（7）（8）中：Vx和Vy分別為車輛的縱向與橫向速度.與此同時,車輛沿自身坐標系y 軸方向的側(cè)向加速度ay為

結(jié)合公式（3）（4）（7）（8）和（9）,可以得到一個關于Vy和的狀態(tài)空間表達式

1.2 橫向和縱向誤差模型

自動駕駛車輛在跟蹤規(guī)劃好的軌跡時,由于道路因素及自身信號傳遞的延遲,導致汽車不能很好地沿著預定軌跡行駛,此時車輛產(chǎn)生的橫向位置誤差和航向誤差就構(gòu)成了橫向誤差,因此需要設計一個橫向控制器,降低橫向誤差給自動駕駛車輛帶來的影響,使得車輛能夠精確地沿著期望軌跡行駛.橫向誤差的定義如圖2 所示.

圖2 橫向誤差示意圖Fig.2 Schematic diagram of lateral error

為了得到橫向誤差模型,還需對ed進行求導

運用Frenet-Serret 公式對公式（12）進行化簡

式（13）中：θ=φ+β,同時,定義eφ=φ-θr,且假設其為小量,對公式（13）展開化簡可得

根據(jù)eφ=φ-θr與公式（14）,可以推出以下幾個關系式

為了方便描述,公式（16）可以簡寫成

車輛在跟蹤期望軌跡時產(chǎn)生的縱向誤差主要由車輛當前位置與目標點之間的縱向位置誤差和當前車速與規(guī)劃的目標點之間的速度誤差.縱向誤差如圖3 所示.

圖3 縱向誤差示意圖Fig.3 Schematic diagram of longitudinal error

圖3 中es是車輛與目標點之間的縱向位置誤差,可由以下公式求出

對于速度誤差eV=V?,由于,結(jié)合公式（12）可得到式

式（19）中：Kr為規(guī)劃軌跡的曲率.

2 橫向和縱向控制器設計

2.1 基于模糊線性二次規(guī)劃的橫向控制

基于第二節(jié)建立的橫向誤差模型, 通過線性二次規(guī)劃理論和模糊控制算法設計了一種模糊橫向跟蹤控制器,整個橫向控制系統(tǒng)的設計原理是：結(jié)合軌跡信息和車輛的航向信息和橫向位置信息,計算出兩者之間的誤差,將誤差輸入到模糊控制模塊得到控制因子r,利用r 值求得合適的權(quán)重系數(shù)并通過設計好的線性二次規(guī)劃器求得前輪轉(zhuǎn)角,將計算結(jié)果輸入給被控車輛,達到對車輛進行橫向跟蹤控制的目的.

在本文研究的橫向控制系統(tǒng)中,目標是算出最優(yōu)的輸入,即前輪轉(zhuǎn)角,使車輛與目標軌跡之間的橫向位置誤差和航向誤差快速減少直至趨近于零.根據(jù)公式（17）車輛橫向誤差模型,對車輛的橫向跟蹤控制可以轉(zhuǎn)化為求解LQR 問題,即

式（20）中：Q 為對角矩陣,Q=[q1,q2,q3,q4],其中q1、q2、q3、q4分別對應于公式（16）中誤差變量ed、、e?、的權(quán)重值.變量的權(quán)重值越大,表示橫向控制器在求解時更側(cè)重讓此變量快速收斂.而R 是表示前輪轉(zhuǎn)角在橫向控制系統(tǒng)的影響程度.為了優(yōu)化計算方式,提升計算效率,控制器處理的都是規(guī)劃好的一系列離散的軌跡點,因此需要將線性二次型控制器離散化,根據(jù)公式（20）可得

式（21）中：ζ∈（t,t+dt）.在求解E（ζ）和u（ζ）的過程中,采用向前歐拉法求解u（ζ）、E（ζ）,采用中點歐拉法進行求解,運用混合歐拉法可以得到如下公式

I 表示單位矩陣,式（22）是離散化后的車輛橫向誤差公式.根據(jù)式（20）（22）,可以構(gòu)造一個帶有約束條件的代價函數(shù)

式（23）中：n→∞,再通過構(gòu)造Hamilton 方程,進一步簡化公式（23）

求公式（24）中代價函數(shù)J 的極值,可以得到如下方程組

根據(jù)式（25）,可求得Riccati 方程,對此方程求解后,即可求得橫向控制系統(tǒng)的輸入

式（26）中：Pk+1為Riccati 方程收斂后的定值.將公式（26）簡寫為：

式（27）中：k=[k1,k2,k3,k4]為線性二次型控制器求解所得的系數(shù)矩陣,用于前輪轉(zhuǎn)角和消除穩(wěn)態(tài)誤差的計算.將公式（27）代入橫向誤差狀態(tài)空間表達式（17）中,可以得到

令誤差變化率E˙為0,則可求得誤差E

至此,可以推出系統(tǒng)的穩(wěn)態(tài)誤差為

首先,考慮消除橫向位置誤差的穩(wěn)態(tài)誤差,即令公式（31）右側(cè)的第一行方程式等于0,求得

對于航向誤差的穩(wěn)態(tài)誤差,根據(jù)式（25）右側(cè)的第三行方程式,可以求得eφ=-β.當航向誤差θ-θr=0 時,φ+β-θr=0,即eφ+β=0,所以系統(tǒng)的航向誤差的穩(wěn)態(tài)誤差,不需要通過前饋控制模塊消除.

在上述線性二次規(guī)劃控制器的設計過程中,研究發(fā)現(xiàn)目標函數(shù)的權(quán)重矩陣直接影響最優(yōu)控制的性能,因此有許多關于如何選擇這些矩陣的研究已經(jīng)被討論.由于智能化技術(shù)的發(fā)展,各類優(yōu)化算法被應用于權(quán)重矩陣的選擇.如蟻群優(yōu)化算法[5]、遺傳算法[6]及粒子群算法[7]用于解決此類問題.為了更好地研究權(quán)重矩陣中的各個權(quán)重系數(shù)對控制效果的影響,本文在一定范圍內(nèi)設置不同的權(quán)重系數(shù)進行仿真測試.如圖4 所示.

從圖4 中的a、b、c、d 四圖中可以看出,當權(quán)重系數(shù)范圍在[1,1 000]內(nèi)時q1、q3和q4對于提高控制精度有著較大的影響,其中q1提升效果最為明顯.而從圖4 中的e、f、g、h 四圖可以看出權(quán)重系數(shù)范圍在[0.001,1]內(nèi)時,q2、q3和q4對于提高控制精度有著較大的影響.綜合考慮各個權(quán)重系數(shù)對車輛控制精度、操作穩(wěn)定性及行駛平順性的影響,本文主要研究權(quán)重系數(shù)q1和q3.而對于權(quán)重系數(shù)R 的選擇,由圖5可知,隨著R 值的增大,車輛的實際路徑與規(guī)劃路徑之間的軌跡越接近,因此取R 值為100.

模糊邏輯控制（FLC）是一種基于人類經(jīng)驗模擬人工行為的智能控制方法,其過程有三步：模糊化、模糊推理和去模糊化[8].首先,根據(jù)制定的隸屬度函數(shù),將系統(tǒng)的輸入值模糊化.然后,通過人為制定的模糊規(guī)則進行去模糊化操作,得到一個實際的輸出值.據(jù)此,本文將橫向誤差ed和航向誤差eφ作為輸入并模糊化為五個模糊集：負大（NB）、負?。∟S）、零（ZO）、正?。≒S）和正大（PB）.然后輸出五個模糊集：負大（NB）、負?。∟S）、零（ZO）、正?。≒S）和正大（PB）.最后去模糊化輸出得到red和reφ,并利用q=10r改變權(quán)重系數(shù)q.

制定的模糊控制輸入輸出的隸屬度函數(shù)如圖6 所示.

模糊規(guī)則庫是由人的經(jīng)驗制定的,在制定規(guī)則庫時,要考慮控制精度和車輛操作平順性.本文制定的規(guī)則庫如表1 和表2 所示.

red eφ ed NB NS ZO PS PB NB PB PB PS NB NS NS PB PS ZO NS ZO ZO PS PS NS PS PB PS ZO NS ZO PB PB PB NS NB PS PB PB

圖7 表示模糊控制最后一步,根據(jù)模糊規(guī)則去模糊化后得到的red和reφ的實際數(shù)值.

2.2 縱向控制器設計

縱向控制是一個上下層系統(tǒng)結(jié)合的一個非線性系統(tǒng),因此難以用線性系統(tǒng)準確描述系統(tǒng)實際運行的狀態(tài)[9].可以采用智能控制理論準確有效地控制車輛.在燃油車的控制過程中涉及發(fā)動機油門角的控制和變速器速比的選擇,為了減少控制的復雜度,我們選取電動汽車作為研究對象.電動車的驅(qū)動電機為永磁同步電機,最大功率Pmax=175 kW,最大扭矩Mmax=430 N·m.以油門信號thr 和電機轉(zhuǎn)速n 為索引的電機的轉(zhuǎn)速扭矩特性如圖8 所示.

將選取的電機參數(shù)代入仿真軟件,需要分別做0～50 m/s 的加速實驗以及50～0 m/s 的減速實驗,通過仿真得到油門與制動查找表.擬合的查找表如圖9 所示.

通過查找表系統(tǒng)可以控制車輛的油門剎車,達到對車輛縱向控制的目的.相較于其他控制方式,傳統(tǒng)PID 精確性更好[10],因此采用雙閉環(huán)PID 的控制方式.整個控制流程如圖10 所示.

圖10 縱向控制流程Fig.10 Vertical control process

PID 控制器的設計如下

式（34）中：C 為控制器輸出,e 為誤差量,kp、ki和kd分別為比例項,積分項和微分項的權(quán)重系數(shù).車輛通過外環(huán)計算出縱向位置誤差es輸入到第一個PID 控制器,得到一個速度補償量Δv,結(jié)合內(nèi)環(huán)系統(tǒng)可以得到車輛的縱向速度誤差ev,將ev輸入到第二個控制器,可以得到加速度補償量Δa,聯(lián)立期望加速度,即可求得車輛所需的加速度.將加速度和速度輸入到查找表,就能達到控制車輛加速或剎車的目的.

將上述橫向控制器的車輪轉(zhuǎn)角信號和縱向控制器中的油門剎車信號輸入到被控車輛, 即可實現(xiàn)對車的橫縱向耦合控制.

3 實驗驗證與分析

Simulink/Carsim 進行聯(lián)合仿真驗證改進后的線性二次型控制器的控制效果,整個控制流程如圖11所示.

為了更清楚地分析改進后的橫縱向耦合控制器的效果,本節(jié)設置了三種測試場景：一種是橫向控制,二是橫縱向聯(lián)合控制,三是采用改進橫縱向聯(lián)合控制,在速度為60 km/h 的條件下,以雙移線作為參考路徑,進行對比分析,仿真結(jié)果如圖12 所示.

從圖12-a 中可以看出在純橫向控制的條件下,高速行駛時,車輛的跟蹤誤差較大,最大值約為0.5 m左右,而加入了縱向控制后,車輛控制效果得到了改善,跟蹤誤差下降到了0.4 m 左右,跟蹤精度提升了20%,最后通過模糊控制改進的橫縱向控制的跟蹤效果具有明顯提升,最大值僅為0.25 m.而圖12-b 中純橫向控制的條件下,車輛橫擺角誤差波動明顯,控制器魯棒性較低,改進后的橫擺角誤差明顯下降,控制穩(wěn)定.

4 小結(jié)

本文提出了模糊線性二次型與PID 聯(lián)合控制系統(tǒng),實驗證明改進后的控制器可以自適應調(diào)節(jié)線性二次規(guī)劃器的權(quán)重系數(shù),使得控制器的魯棒性和精度都有所提升.在后續(xù)的研究工作中,將設計相應的更新策略,用以提升模糊算法在控制器中應用的運算速度.