本發(fā)明屬于無(wú)人駕駛及其控制技術(shù)領(lǐng)域，特別涉及一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制方法及系統(tǒng)。

背景技術(shù)：

客車一旦發(fā)生交通事故，則損失較為慘重，群死群傷大都發(fā)生在客車的交通事故上。而很多交通事故是人為、天氣、路況等因素造成，為了擺脫這些不安全因素，越來(lái)越多地安全技術(shù)出現(xiàn)在世人面前。僅中國(guó)每年就有超過(guò)8萬(wàn)人成為交通事故的犧牲品，碰撞經(jīng)濟(jì)損失每年數(shù)千億元，而駕駛員的錯(cuò)誤駕駛占碰撞事故原因的90％。自動(dòng)駕駛汽車則可以有效減少因駕駛員疲勞或醉駕、開(kāi)車素質(zhì)造成的不規(guī)范行駛等因素造成的交通事故?？蛙嚤绕渌嚫菀讓?shí)現(xiàn)自動(dòng)駕駛，客車的線路既定，更容易自動(dòng)駕駛車輛記憶和學(xué)習(xí)。智慧城市的建立，也使公交系統(tǒng)能更快地納入城市物聯(lián)網(wǎng)絡(luò)中。借助一系列智能感知等手段，自動(dòng)駕駛的智能型汽車可大大提升車輛的主動(dòng)安全性能，糾正駕駛員錯(cuò)誤的駕駛行為，在未來(lái)智能化交通發(fā)展的前景下，智能型汽車將最終徹底消滅交通事故

運(yùn)動(dòng)控制是智能電動(dòng)車輛研究的重要內(nèi)容之一，主要包括橫向控制和縱向控制。智能電動(dòng)車輛為非完整運(yùn)動(dòng)約束系統(tǒng)，具有高度非線性動(dòng)態(tài)特性、強(qiáng)耦合性以及其執(zhí)行機(jī)構(gòu)存在冗余等特點(diǎn)。目前，關(guān)于智能車輛運(yùn)動(dòng)控制的研究，主要通過(guò)橫縱向動(dòng)力學(xué)系統(tǒng)解耦，設(shè)計(jì)兩個(gè)獨(dú)立的控制系統(tǒng)來(lái)實(shí)現(xiàn)對(duì)車輛的橫向控制和縱向控制，也有部分學(xué)者嘗試采用橫縱向耦合控制方法。但是，采用橫縱向耦合的控制方法，使得控制器的設(shè)計(jì)復(fù)雜，而在實(shí)際應(yīng)用過(guò)程中難度大，效果不明顯。而設(shè)計(jì)兩個(gè)獨(dú)立的控制系統(tǒng)則切斷了二者之間的聯(lián)系，導(dǎo)致控制精度降低。

在無(wú)人駕駛及其控制領(lǐng)域，對(duì)跟蹤控制器的控制精度提出了比較高的要求。無(wú)人駕駛應(yīng)用的車輛平臺(tái)多種多樣，包括大客車、公交車以及巴士。這類長(zhǎng)軸距車型要求跟蹤控制方法具有高精度且克服其固有的大慣性和滯后性。為此，需研究一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制方法，使得該方法能夠簡(jiǎn)化控制結(jié)構(gòu)并提高控制精度。并對(duì)于大慣性和滯后性，在保障實(shí)時(shí)和安全的同時(shí)，盡量預(yù)測(cè)車輛的未來(lái)趨勢(shì)，以期提升大慣性無(wú)人駕駛客車控制的響應(yīng)速度和跟蹤效果。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制方法及系統(tǒng)，以解決上述現(xiàn)有理論與設(shè)計(jì)上存在的缺陷或不足；該方法為了克服大客車的大慣性，通過(guò)在橫向控制器中加入預(yù)瞄和預(yù)估算法，對(duì)于速度的給定充分考慮路況及客車轉(zhuǎn)向系統(tǒng)的大慣性，實(shí)現(xiàn)橫縱向解耦控制，簡(jiǎn)化控制器的設(shè)計(jì)，提升控制精度。

一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制方法，包括以下步驟：

步驟1：根據(jù)車輛坐標(biāo)系給定車輛行駛的期望軌跡和期望速度；

根據(jù)行駛狀態(tài)而實(shí)時(shí)變化給定的期望速度，如在轉(zhuǎn)彎、直道行駛、超車、前車跟隨等不同的狀態(tài)下，給定的期望速度均不同，同時(shí)保證其安全性和良好的轉(zhuǎn)向性能，從而達(dá)到實(shí)現(xiàn)橫縱向解耦控制的目的；

步驟2：根據(jù)GPS反饋的車輛實(shí)時(shí)速度，通過(guò)預(yù)瞄算法在期望軌跡上尋找采樣點(diǎn)，并計(jì)算采樣點(diǎn)的期望前輪航向角；

步驟3：根據(jù)GPS反饋的車輛航向角和車輛方向盤反饋的轉(zhuǎn)角，計(jì)算車輛的實(shí)時(shí)前輪航向角；

步驟4：利用預(yù)估算法對(duì)車輛的實(shí)時(shí)前輪航向角進(jìn)行預(yù)估，獲取車輛的預(yù)估前輪航向角；

步驟5：利用車輪的期望前輪航向角和預(yù)估前輪航向角之間的偏差，以及實(shí)時(shí)采集的方向盤轉(zhuǎn)角反饋信號(hào)，獲得方向盤控制量，完成橫向控制；

步驟6：利用車輛行駛的期望速度，對(duì)車輛實(shí)時(shí)速度進(jìn)行修正，完成縱向控制。

由于客車車長(zhǎng)較長(zhǎng)，車頭與車尾在轉(zhuǎn)彎時(shí)存在較大的角度誤差，GPS/IMU接收器安裝在車頭位置，有效減少車輛轉(zhuǎn)向誤差及GPS反饋誤差；因此定義車輛坐標(biāo)系，其中坐標(biāo)原點(diǎn)為車頭中心位置，車輛正前方為X軸，車輛正左方為Y軸，車輛正上方為Z軸；

進(jìn)一步地，所述步驟2中的采樣點(diǎn)的期望前輪航向角為θ_d：

其中，為y的一階導(dǎo)數(shù)，y表示車輛行駛的期望軌跡，y＝A₃x³+A₂x²+A₁x+A₀，A₃為期望軌跡曲率變化率，A₂為期望軌跡曲率，A₁為期望軌跡航向角，A₀為期望軌跡Y軸距離，A₃,A₂,A₁,A₀均為給定值；x表示期望軌跡上的點(diǎn)在X軸上的坐標(biāo)；

利用l＝v_rT_p和求解當(dāng)前的采樣點(diǎn)在X軸上的坐標(biāo)x_d；

其中，l為預(yù)瞄距離，屬于預(yù)瞄時(shí)間內(nèi)期望軌跡對(duì)應(yīng)的軌跡弧長(zhǎng)；T_p為設(shè)定的預(yù)瞄時(shí)間；v_r為車輛實(shí)時(shí)速度，x₀為GPS反饋的車輛實(shí)時(shí)位置坐標(biāo)(x₀,y₀)中橫坐標(biāo)值。

期望軌跡參考全局路徑，每隔控制周期1.2s重新刷新一次，實(shí)現(xiàn)實(shí)時(shí)滾動(dòng)優(yōu)化，即便控制過(guò)程中存在橫向的位置偏差，1.2s后也將重新給定一條以消除橫向誤差為目的同時(shí)保證安全性的新軌跡；控制器的采樣時(shí)間為100ms。

進(jìn)一步地，所述車輛的實(shí)時(shí)前輪航向角為θ：θ＝δ_f+θ_h；

其中，θ_h為GPS反饋的車輛實(shí)時(shí)車身航向角，δ_f為車輛實(shí)時(shí)前輪偏角，δ_s為實(shí)時(shí)采集的方向盤轉(zhuǎn)角，b為方向盤轉(zhuǎn)角與前輪偏角的比例系數(shù)。

進(jìn)一步地，所述方向盤控制量為u：u＝δ_s+Δu；

其中，Δu為前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量，Δu＝ke，k為航向偏差對(duì)應(yīng)方向盤轉(zhuǎn)角變化量的比例系數(shù)，且與車輛實(shí)時(shí)速度成反比；e為前輪航向的偏差量，e＝θ_d-θ_r；θ_r表示車輛的預(yù)估前輪航向角。

進(jìn)一步地，所述車輛的預(yù)估前輪航向角采用以下公式計(jì)算獲得：

其中，為車輛前輪航向角在一個(gè)采樣周期內(nèi)的變化量，T為采樣周期，v為車輛實(shí)時(shí)速度，R為前輪繞運(yùn)動(dòng)中心的半徑。

由于大客車執(zhí)行機(jī)構(gòu)的大滯后特性，根據(jù)當(dāng)前車輛航向角θ與期望航向角θ_d的偏差來(lái)計(jì)算方向盤轉(zhuǎn)角變化量Δu，容易造成控制的滯后，因此為橫向控制器設(shè)計(jì)一種預(yù)估控制模型，來(lái)預(yù)測(cè)車輛的航向變化趨勢(shì)并將其計(jì)入控制偏差，以此模型來(lái)優(yōu)化并替代原始偏差(即θ_d-θ)；

通過(guò)控制方向盤角速度，一方面使車輛轉(zhuǎn)向平緩；另一方面減小速度對(duì)轉(zhuǎn)向的影響，實(shí)現(xiàn)橫縱向解耦控制。

進(jìn)一步地，所述縱向控制按以下步驟進(jìn)行：

步驟6.1：起步控制

若v_r<v_th,v_d<(v_th+1)，則令輸出的車輛目標(biāo)速度v_target等于車輛期望速度v_d：v_target＝v_d；

若v_r≥v_th,v_d<(v_th+1)，則起步控制結(jié)束；

若v_r<v_th,v_d≥(v_th+1)，則令輸出的車輛目標(biāo)速度等于車輛速度閾值：v_target＝v_th；

若v_r≥v_th,v_d≥(v_th+1),v_r<v_target，則令v_target＝v_r+1，且v_target≤v_d；

若v_r≥v_d，則起步控制結(jié)束；

步驟6.2：加速控制

若v_d≤(v_r+1)，則令v_target＝v_d；

若v_d>(v_r+1)，則令v_target＝v_r+1；

步驟6.3：減速控制

計(jì)算減速度a＝(v_d-v_r)/t_th，其中，t_th為自定義的減速時(shí)間閾值；

若v_r≥v_d，a<a_th，則令a＝a_th，其中，a_th為自定義舒適性高的減速度閾值，不超過(guò)3m/s²；

若v_r≥v_d，a≥a_th，則保持a不變；

若v_r<v_d，則a＝0，車輛減速結(jié)束；

其中，v_th為車輛速度閾值，且v_th不超過(guò)3m/s，車輛目標(biāo)速度v_target為控制器傳給執(zhí)行層的速度指令。

一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制系統(tǒng)，包括：

車輛行駛期望軌跡和期望速度給定單元，根據(jù)車輛坐標(biāo)系給定車輛行駛的期望軌跡和期望速度；

采樣點(diǎn)生成單元，根據(jù)GPS反饋的車輛實(shí)時(shí)速度，通過(guò)在期望軌跡上預(yù)瞄尋找采樣點(diǎn)；

采樣點(diǎn)處車輛期望前輪航向角計(jì)算單元，用于計(jì)算采樣點(diǎn)處的車輛期望前輪航向角；

車輛的預(yù)估前輪航向角計(jì)算單元，根據(jù)GPS反饋的車輛航向角和車輛方向盤反饋的轉(zhuǎn)角，計(jì)算車輛的實(shí)時(shí)前輪航向角，利用預(yù)估算法對(duì)車輛的實(shí)時(shí)前輪航向角進(jìn)行預(yù)估，獲取車輛的預(yù)估前輪航向角；

橫向控制單元，通過(guò)利用車輪的期望前輪航向角和預(yù)估前輪航向角之間的偏差，以及實(shí)時(shí)采集的方向盤轉(zhuǎn)角反饋信號(hào)，獲得方向盤控制量，完成橫向控制；

縱向控制單元，利用車輛行駛的期望速度，對(duì)車輛實(shí)時(shí)速度進(jìn)行修正，完成縱向控制；

所述采樣點(diǎn)處車輛期望前輪航向角計(jì)算單元和車輛的預(yù)估前輪航向角計(jì)算單元采用上述的方法進(jìn)行計(jì)算獲得。

有益效果

本發(fā)明提供了一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制方法及系統(tǒng)，其步驟包括：首先，給定平緩的期望軌跡以及根據(jù)路況的期望速度；其次，過(guò)預(yù)瞄算法在期望軌跡上尋找采樣點(diǎn)，并計(jì)算該點(diǎn)的期望前輪航向角；采用GPS反饋的車輛航向角和車輛方向盤反饋的轉(zhuǎn)角之和(前輪航向角)作為控制器反饋量；根據(jù)預(yù)估算法預(yù)測(cè)車輛前輪航向變化趨勢(shì)，計(jì)算期望的前輪航向與預(yù)估后前輪航向的偏差，并根據(jù)橫向控制器的控制率計(jì)算控制偏差，得到方向盤控制量；最后，計(jì)算縱向控制驅(qū)動(dòng)和制動(dòng)的速度控制量,并將所有控制量輸出至執(zhí)行機(jī)構(gòu)實(shí)現(xiàn)控制響應(yīng)；

該控制方法相對(duì)于現(xiàn)有技術(shù)而言具有以下幾點(diǎn)優(yōu)點(diǎn)：

1.橫向控制過(guò)程包含航向角預(yù)估算法，期望軌跡采樣點(diǎn)預(yù)瞄算法，而控制器的反饋量采用前輪航向，有效的克服了大客車的大慣性特性。

2.對(duì)于給定的期望速度，是基于當(dāng)前的路況及車輛的轉(zhuǎn)彎性能，簡(jiǎn)化橫縱向控制的關(guān)系，實(shí)現(xiàn)橫縱向的解耦控制；而且，易于實(shí)際應(yīng)用，經(jīng)過(guò)實(shí)際檢驗(yàn)，在各種行駛路況下的橫向誤差與縱向誤差小，控制效果良好。

3.縱向控制過(guò)程中考慮了起步控制的加速度緩沖、驅(qū)動(dòng)過(guò)程的緩慢加速以及制動(dòng)過(guò)程中限制減速度，提高車輛控制的舒適性和穩(wěn)定性。

附圖說(shuō)明

圖1為大慣性客車橫縱向解耦控制框圖；

圖2為橫縱向解耦控制算法流程圖；

圖3為車輛坐標(biāo)系；

圖4為實(shí)施例1直角彎?rùn)M向控制效果圖；

圖5為縱向邏輯控制框圖；

圖6為實(shí)施例2橫縱向聯(lián)合控制效果圖。

具體實(shí)施方式

下面將結(jié)合附圖和實(shí)施例1對(duì)本發(fā)明做進(jìn)一步的說(shuō)明。

本實(shí)施例采用長(zhǎng)12m，寬2.5m大客車改裝的無(wú)人駕駛車，裝有激光雷達(dá)，毫米波雷達(dá)，攝像頭以及GPS/IMU系統(tǒng)，在標(biāo)準(zhǔn)直角彎道路展開(kāi)橫向控制實(shí)驗(yàn)。

參見(jiàn)圖1所示，本發(fā)明用于大慣性客車橫縱向解耦控制的控制框圖。

參見(jiàn)圖2所示橫向控制算法流程圖，一種大慣性電動(dòng)客車的智能駕駛橫縱向解耦控制方法，每控制周期(1.2s)內(nèi)采用等采樣周期(100ms)的計(jì)算機(jī)控制方式來(lái)實(shí)現(xiàn)對(duì)預(yù)定軌跡的跟蹤，每采樣周期具體包括以下步驟：

步驟一：根據(jù)車輛坐標(biāo)系給定車輛行駛的期望軌跡和恒定的期望速度；

步驟二：根據(jù)GPS反饋的車輛實(shí)時(shí)速度，通過(guò)預(yù)瞄算法在期望軌跡上尋找采樣點(diǎn)，并計(jì)算該點(diǎn)的期望前輪航向角；

步驟三：根據(jù)GPS反饋的車輛航向角和車輛方向盤反饋的轉(zhuǎn)角，計(jì)算車輛實(shí)時(shí)的前輪航向角；

步驟四：根據(jù)預(yù)估算法預(yù)測(cè)車輛前輪航向變化趨勢(shì)，并計(jì)算執(zhí)行層(主要指控制車輛方向、驅(qū)動(dòng)與制動(dòng)的電機(jī)執(zhí)行機(jī)構(gòu))響應(yīng)時(shí)車輛預(yù)估前輪航向角。

步驟五：計(jì)算期望航向角(即步驟二計(jì)算結(jié)果)與預(yù)估實(shí)際航向角(即步驟四計(jì)算結(jié)果)的偏差，并根據(jù)橫向控制器的控制率計(jì)算控制偏差，得到方向盤控制量。

所述的步驟一具體包括以下步驟：

(a)定義車輛坐標(biāo)系，其中以車頭安裝GPS/IMU定位系統(tǒng)處為坐標(biāo)原點(diǎn)，車輛正前方為X軸，車輛正左方為Y軸，車輛正上方為Z軸，滿足右手定則，參見(jiàn)圖3；

(b)給定車輛期望軌跡，其表達(dá)形式為三次多項(xiàng)式：

y＝A₃x³+A₂x²+A₁x+A₀ (1)

期望軌跡參考全局路徑，每控制周期1.2s重新刷新一次，實(shí)現(xiàn)實(shí)時(shí)滾動(dòng)優(yōu)化，即便控制過(guò)程中存在橫向的位置偏差，1.2s后也將重新給定一條以消除橫向誤差為目的同時(shí)保證安全性的新軌跡；控制器的采樣時(shí)間為100ms；

(c)給定恒定的車輛期望速度v_d＝2m/s。

所述的步驟二具體包括以下步驟：

(a)根據(jù)車輛實(shí)時(shí)速度和預(yù)瞄時(shí)間，計(jì)算合適的預(yù)瞄距離，其計(jì)算公式為：

l＝v_rT_p (2)

其中，l為預(yù)瞄距離(對(duì)應(yīng)軌跡的弧長(zhǎng))，v_r為從GPS反饋、由CAN總線讀取的車輛實(shí)時(shí)速度，T_p為預(yù)瞄時(shí)間，設(shè)置T_p＝1.2s。

(b)根據(jù)期望軌跡，計(jì)算軌跡上滿足預(yù)瞄距離的采樣點(diǎn)坐標(biāo)，其計(jì)算公式為：

其中，A₃,A₂,A₁,A₀系數(shù)由上層軌跡規(guī)劃提供，A₃為軌跡曲率變化率，A₂為軌跡曲率，A₁為軌跡航向角，A₀為軌跡Y軸距離。x₀為GPS反饋的車輛實(shí)時(shí)位置坐標(biāo)(x₀,y₀)中橫坐標(biāo)值，x_d表示求解的采樣點(diǎn)X軸位置坐標(biāo)，θ_d表示該采樣點(diǎn)的期望前輪航向角，表示軌跡的一階導(dǎo)，x為軌跡X軸變量且x∈[0,x_f]，x_f為軌跡終點(diǎn)(由上層軌跡規(guī)劃提供)。

將式(2)代入式(3)，求解x_d并將其代入式(1)得到采樣點(diǎn)坐標(biāo)(x_d,y_d)，同時(shí)求解期望前輪航向角θ_d。

所述的步驟三具體包括以下步驟：

(a)計(jì)算車輛實(shí)時(shí)的前輪航向角，其計(jì)算公式為：

其中，θ_h為GPS反饋的車輛實(shí)時(shí)航向角，δ_s為采集的方向盤轉(zhuǎn)角反饋，δ_f車輛當(dāng)前前輪偏角，b為方向盤與前輪偏角的比例系數(shù)，θ為車輛實(shí)時(shí)的前輪航向角。對(duì)于大客車，車身的航向存在大滯后的特性，而前輪航向滯后小，易于快速控制?？刂频目刂屏恐苯幼饔糜诜较虮P，而通過(guò)大量實(shí)驗(yàn)得到，方向盤轉(zhuǎn)角與前輪偏角的比例系數(shù)b為23.1。根據(jù)從CAN總線讀取的θ_h、δ_s代入式(4)，可得到車輛實(shí)時(shí)的前輪航向角θ。

所述的步驟四具體包括以下步驟：

(4a)由于大客車執(zhí)行機(jī)構(gòu)的大滯后特性，根據(jù)當(dāng)前車輛航向角θ與期望航向角θ_d的偏差來(lái)計(jì)算方向盤轉(zhuǎn)角變化量Δu，容易造成控制的滯后，因此為橫向控制器設(shè)計(jì)一種預(yù)估控制模型，來(lái)預(yù)測(cè)車輛的航向變化趨勢(shì)并將其計(jì)入控制偏差，以此模型來(lái)優(yōu)化并替代原始偏差(即θ_d-θ)，其模型可表示為：

其中，T為采樣周期，v為縱向速度(以前輪速度v_r近似)，R為前輪繞運(yùn)動(dòng)中心的半徑，L為車軸長(zhǎng)度，δ_f為前輪偏角，為車輛前輪航向在一個(gè)控制周期的變化量。設(shè)置T＝1.2s，v＝2m/s，L＝6m，將式(4)解得的δ_f代入式(5)，解得R,

(4b)計(jì)算車輛的預(yù)估前輪航向角，其計(jì)算公式為：

其中，θ為車輛當(dāng)前的前輪航向角，θ_r為車輛的預(yù)估前輪航向角。將式(4)(5)解得的θ，代入式(6)，解得車輛預(yù)估前輪航向角θ_r,通過(guò)對(duì)未來(lái)航向變化趨勢(shì)的預(yù)測(cè)，能夠在橫向控制中有效克服車輛的大慣性。

所述步驟五具體包括以下步驟：

(5a)計(jì)算前輪航向的偏差量e，其計(jì)算公式為：

e＝θ_d-θ_r (7)

(5b)計(jì)算前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量，其公式為：

其中，Δu為前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量，k為航向偏差對(duì)應(yīng)方向盤轉(zhuǎn)角變化量的比例系數(shù)并與v_r成反比，c為k和v_r的比例常量，經(jīng)大量實(shí)驗(yàn)測(cè)得c＝24。本實(shí)施例車輛實(shí)時(shí)速度恒定v_r＝2m/s，代入式(7)(8)可求解前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量Δu。

(5c)計(jì)算輸出的方向盤控制量，其計(jì)算公式為：

u＝δ_s+Δu (9)

其中，δ_s為采集的方向盤轉(zhuǎn)角反饋，u為輸出的方向盤控制量。根據(jù)車輛性能確定約束條件，包括u_min≤u≤u_max，u_min,u_max為實(shí)際行駛過(guò)程中方向盤轉(zhuǎn)角的上下限，本實(shí)例中，u_min,u_max分別為-720°～+720°，當(dāng)方向盤控制量為正時(shí)，方向盤向左旋轉(zhuǎn)，反之方向盤向右旋轉(zhuǎn)。將式(8)的Δu代入式(9)可得橫向控制器的輸出：方向盤控制量u。同時(shí)，也給定方向盤轉(zhuǎn)角的角速度ω，其中，ω∈[100°/s,540°/s]，通過(guò)大量實(shí)驗(yàn)測(cè)得有經(jīng)驗(yàn)駕駛員平均最大轉(zhuǎn)角速度值，給定ω＝350°/s。

每采樣周期均輸出該采樣周期內(nèi)計(jì)算的方向盤控制量u，并由CAN總線傳入車輛執(zhí)行機(jī)構(gòu)進(jìn)行控制響應(yīng)。

當(dāng)前控制周期結(jié)束后，在新的控制周期內(nèi)重復(fù)步驟一至步驟五，依次循環(huán)實(shí)現(xiàn)大慣性客車的橫向控制。參見(jiàn)圖4所示，實(shí)施例的全局路徑為細(xì)實(shí)線，車輛行駛的實(shí)際軌跡為×點(diǎn)，虛圓表示的是直角彎道部分，可以看出車輛的實(shí)際行駛軌跡與給定的全局路徑基本吻合。

下面將結(jié)合附圖和實(shí)施例2對(duì)本發(fā)明做進(jìn)一步的說(shuō)明。

本實(shí)施例采用長(zhǎng)12m，寬2.5m大客車改裝的無(wú)人駕駛車，裝有激光雷達(dá)，毫米波雷達(dá)，攝像頭以及GPS/IMU系統(tǒng)，在該測(cè)試場(chǎng)地開(kāi)展大慣性電動(dòng)客車的智能駕駛橫縱向解耦軌跡跟蹤控制綜合實(shí)驗(yàn)。參見(jiàn)圖1所示，本發(fā)明用于大慣性客車橫縱向解耦控制的控制框圖。

參見(jiàn)圖5所示縱向控制邏輯框圖，每控制周期(1.2s)內(nèi)采用等采樣周期(100ms)的計(jì)算機(jī)控制方式來(lái)實(shí)現(xiàn)對(duì)預(yù)定軌跡的跟蹤，每采樣周期具體包括以下步驟：

步驟一：根據(jù)車輛坐標(biāo)系給定車輛行駛的期望軌跡和變化的期望速度；

步驟二：根據(jù)GPS反饋的車輛實(shí)時(shí)速度，通過(guò)預(yù)瞄算法在期望軌跡上尋找采樣點(diǎn)，并計(jì)算該點(diǎn)的期望前輪航向角；

步驟三：根據(jù)GPS反饋的車輛航向角和車輛方向盤反饋的轉(zhuǎn)角，計(jì)算車輛實(shí)時(shí)的前輪航向角；

步驟四：根據(jù)預(yù)估算法預(yù)測(cè)車輛前輪航向變化趨勢(shì)，并計(jì)算執(zhí)行層(主要指控制車輛方向、驅(qū)動(dòng)與制動(dòng)的電機(jī)執(zhí)行機(jī)構(gòu))響應(yīng)時(shí)車輛預(yù)估前輪航向角；

步驟五：計(jì)算期望前輪航向角與實(shí)際預(yù)估前輪航向角的偏差，并根據(jù)橫向控制器的控制率計(jì)算控制偏差，得到方向盤控制量；

步驟六：計(jì)算縱向控制器驅(qū)動(dòng)速度和制動(dòng)減速度。

所述的步驟一具體包括以下步驟：

(a)定義車輛坐標(biāo)系，其中以車頭安裝GPS/IMU定位系統(tǒng)處為坐標(biāo)原點(diǎn)，車輛正前方為X軸，車輛正左方為Y軸，車輛正上方為Z軸，滿足右手定則，參見(jiàn)圖3；

(b)給定車輛期望軌跡，其表達(dá)形式為三次多項(xiàng)式：

y＝A₃x³+A₂x²+A₁x+A₀ (1)

期望軌跡參考全局路徑，每控制周期1.2s重新刷新一次，實(shí)現(xiàn)實(shí)時(shí)滾動(dòng)優(yōu)化，即便控制過(guò)程中存在橫向的位置偏差，1.2s后也將重新給定一條以消除橫向誤差為目的同時(shí)保證安全性的新軌跡；控制器的采樣時(shí)間為100ms；

(c)給定恒定的車輛期望速度v_d＝2m/s。

所述的步驟二具體包括以下步驟：

(a)根據(jù)車輛實(shí)時(shí)速度和預(yù)瞄時(shí)間，計(jì)算合適的預(yù)瞄距離，其計(jì)算公式為：

l＝v_rT_p (2)

其中，l為預(yù)瞄距離(對(duì)應(yīng)軌跡的弧長(zhǎng))，v_r為從GPS反饋、由CAN總線讀取的車輛實(shí)時(shí)速度，T_p為預(yù)瞄時(shí)間，設(shè)置T_p＝1.2s。

(b)根據(jù)期望軌跡，計(jì)算軌跡上滿足預(yù)瞄距離的采樣點(diǎn)坐標(biāo)，其計(jì)算公式為：

其中，x_d表示求解的采樣點(diǎn)X軸位置坐標(biāo)，θ_d表示該采樣點(diǎn)的期望前輪航向角，表示軌跡的一階導(dǎo)，x為軌跡X軸變量且x∈[0,x_f]，x_f為軌跡終點(diǎn)(由上層軌跡規(guī)劃提供)。

將式(2)代入式(3)，求解x_d并將其代入式(1)得到采樣點(diǎn)坐標(biāo)(x_d,y_d)，同時(shí)求解期望前輪航向角θ_d。

所述的步驟三具體包括以下步驟：

(a)計(jì)算車輛實(shí)時(shí)的前輪航向角，其計(jì)算公式為：

其中，θ_h為GPS反饋的車輛實(shí)時(shí)航向角，δ_s為采集的方向盤轉(zhuǎn)角反饋，δ_f車輛當(dāng)前前輪偏角，b為方向盤與前輪偏角的比例系數(shù)，θ為車輛實(shí)時(shí)的前輪航向角。對(duì)于大客車，車身的航向存在大滯后的特性，而前輪航向滯后小，易于快速控制?？刂频目刂屏恐苯幼饔糜诜较虮P，而通過(guò)大量實(shí)驗(yàn)得到，方向盤轉(zhuǎn)角與前輪偏角的比例系數(shù)b為23.1。根據(jù)從CAN總線讀取的θ_h、δ_s代入式(4)，可得到車輛實(shí)時(shí)的前輪航向角θ。

所述的步驟四具體包括以下步驟：

(4a)由于大客車執(zhí)行機(jī)構(gòu)的大滯后特性，根據(jù)當(dāng)前車輛航向角θ與期望航向角θ_d的偏差來(lái)計(jì)算方向盤轉(zhuǎn)角變化量Δu，容易造成控制的滯后，因此為橫向控制器設(shè)計(jì)一種預(yù)估控制模型，來(lái)預(yù)測(cè)車輛的航向變化趨勢(shì)并將其計(jì)入控制偏差，以此模型來(lái)優(yōu)化并替代原始偏差(即θ_d-θ)，其模型可表示為：

其中，T為采樣周期，v為縱向速度(以前輪速度v_r近似)，R為前輪繞運(yùn)動(dòng)中心的半徑，L為車軸長(zhǎng)度，δ_f為前輪偏角，為車輛前輪航向在一個(gè)控制周期的變化量。設(shè)置T＝1.2s，v＝2m/s，L＝6m，將式(4)解得的δ_f代入式(5)，解得R,

(4b)計(jì)算車輛的預(yù)估前輪航向角，其計(jì)算公式為：

其中，θ為車輛當(dāng)前的前輪航向角，θ_r為車輛的預(yù)估前輪航向角。將式(4)(5)解得的θ，代入式(6)，解得車輛預(yù)估前輪航向角θ_r,通過(guò)對(duì)未來(lái)航向變化趨勢(shì)的預(yù)測(cè)，能夠在橫向控制中有效克服車輛的大慣性。

所述步驟五具體包括以下步驟：

(5a)計(jì)算前輪航向的偏差量e，其計(jì)算公式為：

e＝θ_d-θ_r (7)

(5b)計(jì)算前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量，其公式為：

其中，Δu為前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量，k為航向偏差對(duì)應(yīng)方向盤轉(zhuǎn)角變化量的比例系數(shù)并與v_r成反比，c為k和v_r的比例常量，經(jīng)大量實(shí)驗(yàn)測(cè)得c＝24。本實(shí)施車輛實(shí)時(shí)速度恒定v_r＝2m/s，代入式(7)(8)可求解前輪航向偏差對(duì)應(yīng)的方向盤轉(zhuǎn)角變化量Δu。

(5c)計(jì)算輸出的方向盤控制量，其計(jì)算公式為：

u＝δ_s+Δu (9)

其中，δ_s為采集的方向盤轉(zhuǎn)角反饋，u為輸出的方向盤控制量。根據(jù)車輛性能確定約束條件，包括u_min≤u≤u_max，u_min,u_max為實(shí)際行駛過(guò)程中方向盤轉(zhuǎn)角的上下限，本實(shí)例中，u_min,u_max分別為-720°～+720°，當(dāng)方向盤控制量為正時(shí)，方向盤向左旋轉(zhuǎn)，反之方向盤向右旋轉(zhuǎn)。將式(8)的Δu代入式(9)可得橫向控制器的輸出：方向盤控制量u。同時(shí)，也給定方向盤轉(zhuǎn)角的角速度ω，其中，ω∈[100°/s,540°/s]，通過(guò)大量實(shí)驗(yàn)測(cè)得有經(jīng)驗(yàn)駕駛員平均最大轉(zhuǎn)角速度值，給定ω＝350°/s。

步驟六具體包括以下步驟：

(6a)由于車輛驅(qū)動(dòng)裝置啟動(dòng)時(shí)，車輛加速度從0增加到最大，直至車輛速度達(dá)到目標(biāo)速度才停止驅(qū)動(dòng)，降低了車輛的穩(wěn)定性和舒適性，因此為其設(shè)計(jì)了一種梯形起步控制來(lái)限制驅(qū)動(dòng)時(shí)候的加速度，使其保持在一個(gè)舒適的范圍內(nèi)，其控制模型如下：

若v_r<v_th,v_d<(v_th+1)，則令輸出目標(biāo)速度等于期望速度v_target＝v_d；

若v_r≥v_th,v_d<(v_th+1)，則起步控制結(jié)束；

若v_r<v_th,v_d≥(v_th+1)，則令目標(biāo)速度等于閾值速度v_target＝v_th；

若v_r≥v_th,v_d≥(v_th+1),v_r<v_target，則令v_target＝v_r+1，且v_target≤v_d；

若v_r≥v_d，則起步控制結(jié)束；

本實(shí)施例2設(shè)置v_th＝2m/s，起步控制流程為：(1)若v_r＝0，v_d＝2m/s，v_th＝1m/s，令v_target＝v_th＝1m/s；(2)車輛逐漸提速，當(dāng)v_r≥v_th，起步控制結(jié)束，在v_d不變化的前提下一直給定v_target＝v_d＝2m/s。

(6b)車輛在行駛的過(guò)程中需要提速，則控制模型如下：

若v_d≤(v_r+1)，則令v_target＝v_d；

若v_d>(v_r+1)，則令v_target＝v_r+1；

本實(shí)施例2當(dāng)車輛從彎道進(jìn)入直道后，目標(biāo)速度v_d從1m/s變化為2m/s，此時(shí)令v_target＝v_d＝2m/s，車輛開(kāi)始加速；當(dāng)v_r≥v_d時(shí)，車輛結(jié)束加速行為，v_target＝2m/s保持不變；同時(shí)，在加速和勻速保持階段均令a＝0。

(6c)車輛在行駛的過(guò)程中收到減速信號(hào)，則控制模型如下：

計(jì)算減速度a＝(v_d-v_r)/t_th，其中t_th為自定義的減速時(shí)間閾值；

若v_r≥v_d，a<a_th，其中a_th為自定義舒適性高的減速度閾值，則令a＝a_th；

若v_r≥v_d，a≥a_th，則保持a不變。

若v_r<v_d，則a＝0，車輛減速結(jié)束。

本實(shí)施例2當(dāng)車輛從彎道進(jìn)入彎道時(shí)，目標(biāo)速度v_d從2m/s變化為1m/s，此時(shí)v_r＝2m/s，定義減速時(shí)間閾值t_th＝2s，則令a＝(v_d-v_r)/t_th＝-0.5m/s²，此時(shí)a>a_th＝-2.5m/s²；當(dāng)v_r≤v_d時(shí)，減速結(jié)束且令a＝0；同時(shí)，在減速過(guò)程中，令v_target＝0。

(6d)若車輛在行駛的過(guò)程中收到急停信號(hào)，則減速度為最大減速度a＝a_max＝2m/s²,v_target＝0；

每采樣周期均輸出該采樣周期內(nèi)計(jì)算的方向盤控制量u、驅(qū)動(dòng)目標(biāo)速度v_target以及制動(dòng)減速度a，并由CAN總線傳入車輛執(zhí)行機(jī)構(gòu)進(jìn)行控制響應(yīng)。

當(dāng)前控制周期結(jié)束后，在新的控制周期內(nèi)重復(fù)步驟一至步驟五，依次循環(huán)實(shí)現(xiàn)大慣性客車的橫向控制。參見(jiàn)圖6所示，實(shí)施例的全局路徑為細(xì)實(shí)線，車輛行駛的實(shí)際軌跡為×點(diǎn)，多邊形代表全局終點(diǎn)，粗實(shí)線代表未走完的全局路徑，車輛的實(shí)際行駛軌跡與給定的全局路徑基本吻合。

以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，本發(fā)明的保護(hù)范圍并不僅局限于上述實(shí)施例，凡屬于本發(fā)明思路下的技術(shù)方案均屬于本發(fā)明的保護(hù)范圍。應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明原理前提下的若干改進(jìn)和潤(rùn)飾，這些改進(jìn)和潤(rùn)飾也應(yīng)視為本發(fā)明的保護(hù)范圍。