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