智能汽車規(guī)劃控制常用控制方法詳解
控制是驅(qū)使車輛前行的策略?？刂频哪繕?biāo)是使用可行的控制量，最大限度地降低與目標(biāo)軌跡的偏差、最大限度地提供乘客的舒適度等。
如上圖所示，與控制模塊輸入相關(guān)聯(lián)的模塊有規(guī)劃模塊、定位模塊和車輛信息等。其中定位模塊提供車輛的位置信息，規(guī)劃模塊提供目標(biāo)軌跡信息，車輛信息則包括檔位、速度、加速度等?？刂戚敵隽縿t為轉(zhuǎn)向、加速和制動(dòng)量。
控制模塊主要分為橫向控制和縱向控制，根據(jù)耦合形式的不同可以分為獨(dú)立和一體化兩種方法。
1 控制方法
1.1 解耦控制
所謂解耦控制，就是將橫向和縱向控制方法獨(dú)立分開進(jìn)行控制。
1.2 耦合控制
耦合控制考慮到了橫縱向控制時(shí)存在的耦合問(wèn)題。一個(gè)典型的例子是汽車不能高速過(guò)彎，因?yàn)楫?dāng)縱向速度過(guò)高時(shí)，橫向角速度需要有一定的限制，否則向心力無(wú)法滿足向心加速度。
橫縱一體的典型代表方法是線性時(shí)變模型預(yù)測(cè)控制，該方法在模型預(yù)測(cè)控制的基礎(chǔ)上增加橫縱向的聯(lián)合約束。如最大向心加速度約束等。
1.3 橫向控制
如上圖橫向控制可以分為幾何方法、基于運(yùn)動(dòng)學(xué)模型方法和基于動(dòng)力學(xué)模型的方法。
1.3.1 前饋控制
所謂前饋控制，即根據(jù)跟蹤點(diǎn)信息對(duì)控制量進(jìn)行提前適當(dāng)?shù)难a(bǔ)償。一個(gè)典型的例子是利用跟蹤序列點(diǎn)中的曲率信息，對(duì)轉(zhuǎn)角進(jìn)行補(bǔ)償。
1.3.2 Chained Form
鏈?zhǔn)较到y(tǒng)將非線性系統(tǒng)進(jìn)行多層線性化，它將系統(tǒng)進(jìn)行逐層分解，進(jìn)而可以將系統(tǒng)快慢化，類似于濾波系統(tǒng)[3]。
系統(tǒng)在frenet坐標(biāo)下的模型：
經(jīng)過(guò)反向積分預(yù)算，可以得到控制率：
1.3.3 Lyapunov
基于李雅普諾夫穩(wěn)定性方法的設(shè)計(jì)可以運(yùn)用在運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型上。其基本思想是首先建立運(yùn)動(dòng)學(xué)或者動(dòng)力學(xué)模型，根據(jù)模型提出跟蹤方法，然后建立李雅普諾夫函數(shù)，通過(guò)李雅普諾夫穩(wěn)定性證明閉環(huán)系統(tǒng)的漸近穩(wěn)定[4]。
- 運(yùn)動(dòng)學(xué)模型
如上圖所示，小車當(dāng)前點(diǎn)為P，跟蹤目標(biāo)點(diǎn)為Pr。 是當(dāng)前位置和目標(biāo)點(diǎn)的位姿差， 和 分別為參考速度和角速度。設(shè)計(jì)李雅普諾夫函數(shù)：
跟蹤率設(shè)計(jì)：
最終通過(guò)限定約束設(shè)計(jì)參數(shù)，進(jìn)而證明了跟蹤率的漸近穩(wěn)定性，即當(dāng) → ∞時(shí)， → 0。
- 動(dòng)力學(xué)模型
首先建立動(dòng)力學(xué)模型：
其中：
令
則誤差為：
設(shè)計(jì)代價(jià)函數(shù)：
設(shè)計(jì)控制率：
最終證明漸進(jìn)穩(wěn)定性。
1.3.4 Pure Pursuit
純跟蹤是一種幾何路徑跟蹤控制器。該控制器使用車輛運(yùn)動(dòng)和參考路徑的幾何關(guān)系來(lái)跟蹤參考路徑的控制器。該控制方法使用車輛后軸中心用作參考點(diǎn)。
根據(jù)上圖可推到出前輪轉(zhuǎn)角指令：
其中R為轉(zhuǎn)彎半徑，L為車輛軸距，e為車輛當(dāng)前姿態(tài)和目標(biāo)路點(diǎn)在橫向上的誤差， 為前視距離且 。
根據(jù)上圖實(shí)驗(yàn)數(shù)據(jù)所示前視距離增加，跟蹤抖動(dòng)越來(lái)越小。較短的前視距離可提供更精確的跟蹤，而較長(zhǎng)的前視距離可提供更平滑的跟蹤。PurePursuit的另一個(gè)特征是，在轉(zhuǎn)彎跟蹤時(shí)，過(guò)大的前視距離會(huì)導(dǎo)致“cutting corners”。Pure Pursuit很難在穩(wěn)定性和跟蹤性能之間進(jìn)行權(quán)衡。
1.3.5 Stanley
與以后軸為參考點(diǎn)的純追趕純跟蹤方法不同，Stanley控制器以前軸為參考點(diǎn)。它同時(shí)考慮了航向誤差和橫向誤差。Stanley控制器不僅考慮航向誤差，而且還考慮了橫向誤差。
根據(jù)上圖可推到出前輪轉(zhuǎn)角指令：
根據(jù)上圖實(shí)驗(yàn)數(shù)據(jù)所示隨著k的增加，跟蹤性能也會(huì)提高。當(dāng)車速變大時(shí)Stanley不像Pure Pursuit有足夠的穩(wěn)定性。
1.3.6 LQR
基于車輛運(yùn)動(dòng)學(xué)模型的方法忽略了車輛的動(dòng)力學(xué)特性，因此在車速過(guò)快或者曲率變化率過(guò)大的情況下該算法無(wú)法滿足車輛的穩(wěn)定性控制要求。對(duì)于基于車輛動(dòng)力學(xué)模型的控制方法，首要的工作是對(duì)車輛動(dòng)力學(xué)進(jìn)行建模。由于精確的二自由度動(dòng)力學(xué)模型是非線性的，為了便于進(jìn)行實(shí)時(shí)的跟蹤控制計(jì)算，通常還需要在精確的二自由度動(dòng)力學(xué)模型基礎(chǔ)上進(jìn)行一些簡(jiǎn)化近似，得到線性二自由度動(dòng)力學(xué)模型。
- 車輛二自由度動(dòng)力學(xué)模型：
- LQR:
線性二次調(diào)節(jié)器（Linear Quadratic Regulator或LQR）是基于模型的控制器，它使用車輛的狀態(tài)來(lái)使誤差最小化。LQR理論是現(xiàn)代控制理論中發(fā)展最早也最為成熟的一種狀態(tài)空間設(shè)計(jì)法。LQR可得到狀態(tài)線性反饋的最優(yōu)控制規(guī)律，易于構(gòu)成閉環(huán)最優(yōu)控制。
LQR 最優(yōu)設(shè)計(jì)是指設(shè)計(jì)出的狀態(tài)反饋控制器 K 要使二次型目標(biāo)函數(shù) J 取最小值，而 K 由權(quán)矩陣 Q 與 R 唯一決定，故此 Q、R的選擇尤為重要。如下公式為L(zhǎng)QR代價(jià)函數(shù)：
根據(jù)車輛動(dòng)力學(xué)模型和LQR代價(jià)函數(shù)可以推到出代數(shù)里卡提方程：
最終通過(guò)迭代黎卡提方程計(jì)算反饋矩陣，再根據(jù)反饋矩陣獲取最優(yōu)控制量。
1.3.7 MPC
MPC(Model Prediction Control)是一種致力于將更長(zhǎng)時(shí)間跨度,甚至于無(wú)窮時(shí)間的最優(yōu)化控制問(wèn)題, 分解為若干個(gè)更短時(shí)間跨度,或者有限時(shí)間跨度的最優(yōu)化控制問(wèn)題, 并且在一定程度上仍然追求最優(yōu)解。
MPC由如下三個(gè)要素組成:
- 預(yù)測(cè)模型: 預(yù)測(cè)模型能夠在短時(shí)間內(nèi)很好的預(yù)測(cè)系統(tǒng)狀態(tài)的變化;
- 在線滾動(dòng)優(yōu)化: 由于預(yù)測(cè)模型得到的結(jié)果與實(shí)際仍然存在偏差, 所以采用滾動(dòng)優(yōu)化找到每個(gè)時(shí)刻下的局部最優(yōu)解,通常情況會(huì)設(shè)計(jì)一個(gè)目標(biāo)(損失)函數(shù)并將其轉(zhuǎn)化為二次規(guī)劃問(wèn)題進(jìn)而找到最優(yōu)解;
- 反饋校正: 到下一個(gè)時(shí)間點(diǎn)根據(jù)新的狀態(tài)重新進(jìn)行預(yù)測(cè)和優(yōu)化。
- 預(yù)測(cè)模型:
根據(jù)LQR中的車輛動(dòng)力學(xué)模型可以推出預(yù)測(cè)模型。
- 滾動(dòng)優(yōu)化:
MPC代價(jià)函數(shù)：
根據(jù)預(yù)測(cè)模型、車輛橫向約束、代價(jià)函數(shù)進(jìn)行優(yōu)化求解可以得到相應(yīng)的控制指令。
1.3.8 橫向控制算法對(duì)比
1.4 縱向
如上圖所示，縱向控制一般采用采用串級(jí)pid控制方法。
2 詳細(xì)設(shè)計(jì)
控制器的設(shè)計(jì)如上圖所示，其中Controller為基類，LonController、LonController和MPCController繼承該基類。LonController又衍生出LQRController、LyapunovController和StanleyController等子類。
