使用模型預測控制和PID實現自動駕駛的車道保持

本文為 AI 研習社編譯的技術博客，原標題 ：

Lane keeping in autonomous driving with Model Predictive Control & PID

作者 |Jonathan Hui

翻譯 | 韌明

校對 | 醬番梨        整理 | 菠蘿妹

https://medium.com/@jonathan_hui/lane-keeping-in-autonomous-driving-with-model-predictive-control-50f06e989bc9

照片來自 Campbell Boulanger

使用模型預測控制和PID實現自動駕駛的車道保持

（視頻）黃色的路徑是目標軌跡，綠色的路徑是我們的汽車如何使用MPC移動。

自動駕駛的3大核心科技是定位（在哪裡），感知（周圍是啥）以及控制（咋開車呢）。通過車道檢測，我們可以對車的行進路線進行路徑規劃。本篇文章主要通過一個自行車的動力學模型討論車輛的加速、剎車和轉向的模型預測控制。目的不僅在於儘可能地控制車輛軌跡，同時也還要儘可能使速度平滑以避免暈車和頻繁的剎車。

模型預測控制主要在約束條件下使損失函數最小。例如，我們想要以100ms的周期調整轉向和速度，在轉向角度不能超過25°的約束下，最小化以規劃的路徑和實際路徑之間的誤差。我們通過感測器獲取車輛的狀態，比如速度，而我們的動作基於感測器讀數以一個短的周期執行（例如1s）。例如，我們順時針轉向20°，然後每100ms周期減小1°。加入這些動作可以1秒鐘之後的損失函數最小，我們將會採用第一個動作：順時針轉動20°，但是卻並不執行後續的動作，而是在100ms後，重複優化過程。100ms後，有了新的讀數，我們就重新計算下一個最優動作。模型預測控制通過預測接下來一段較長時間（1s）的損失函數，來計算選擇出下一個較短周期（100ms）的最優動作。相比於短視的貪心演算法，模型預測控制更加健壯，因此能夠控制得更好。

本篇是自動駕駛5篇系列之一，這一系列包括：

自動駕駛感知：基於卡爾曼濾波得感測器數據融合

自動駕駛感知：擴展卡爾曼濾波和無損卡爾曼濾波

自動駕駛定位：基於粒子濾波的定位

自動駕駛控制：模型預測控制和PID控制

自動駕駛路徑尋優

  雙輪運動學模型

首先，我們定義一個模型來描述我們的車輛。

（圖源：http://www.me.berkeley.edu/~frborrel/pdfpub/IV_KinematicMPC_jason.pdf）

（x, y）是車輛的質心，ψ是當前車身的角度，v是當前車輛的速度，lf是當前車輛質心到原點的距離， β是速度和車身的角度。在我們的例子中，我們假設β為零，也就是沒有側滑。

在我們的模型中，我們可以通過控制前輪的轉角δf 以及車輛的加速度a來控制車輛軌跡。簡單起見，我們只考慮前輪驅動的車輛，並且將δf記作δ。

  模型預測控制的細節

每個控制周期，我們都從感測器讀取數據並得到車輛狀態量：

車輛的位置（x，y）

速度v

車身角度 ψ

轉向角（舵角） δ

加速度a

軌跡模型：

我們的道路檢測系統應該能夠為我們規劃好路線，比如，以接下來6個航點的坐標的形式。在我們的例子中，我們使用6個航點去逼近一個3階多項式函數。我們用這個模型去計算y坐標和相對於x軸的車身角度ψ。

動態模型：

接下來，我們要創建動態模型利用t時刻的狀態去預測在t+1拍時刻的車輛狀態。利用動力學模型，我們可以輕易地從最新時刻地採樣推導出下一時刻的位置，車身角度和速度。

我們可以在添加另外2個狀態去衡量軌跡跟蹤誤差和車身角度誤差ψ：

損失函數：

在模型預測控制中，我們需要定義損失函數來優化路徑。如果模型不能保持目標速度，那麼我們就要懲罰模型。如果可能的話，我們並不想要突然的加減速或者突然的轉向。但是既然這些實際上是不可避免的，我們可以儘可能地抑制加減速和轉向地變化率。這減輕了暈車同時更加省油（也省人民幣）。模型的損失函數應當包含：雷鋒網

跟蹤誤差

轉向誤差

速度損失函數項（盡量保持在100英里每小時）

轉向損失函數項（盡量避免轉向）

加速度損失函數項（盡量保持0加速度）

轉向變化率（越小越好）

加速度變化率（越小越好）

因為這些目標也許會相互衝突，我們需要給這些損失項定義權重以體現優先順序。損失函數如下：

總而言之：

我們需要用模型預測控制來尋找最優路徑，那麼就需要動力學模型來預測下一拍的狀態，以下是動力學模型和系統約束：

  優化模型預測控制

我們通過解決一個約束條件下優化損失函數的問題來解決了控制問題。這些約束條件包括油門和轉向的控制。

從道路中檢測下6個航點，並且計算3次插值的來建立行駛軌跡

從感測器讀取當前速度v,  方向ψ,  轉向角 δ 以及加速度  a

使用感測器讀取的數據和動力學模型計算出第一個車輛狀態

根據1秒內的車輛狀態響應優化控制動作，控制的周期為100ms，所以1s內有10個周期

模型預測控制的兩個變數（也是控制量）：加速度（油門對應正加速度，剎車對應負加速度）和轉向角

給出加速度和轉向角的約束範圍

我們將動態模型計算9次，得到未來9個時間拍的系統狀態

給出每個採樣計算周期的損失函數

用1個優化器解算出在約束定義下周期1到周期9的最小總損失（注意，在我們的定義中，時間周期並不從0開始，而是從1開始到10）

我們僅僅選擇周期1給出的控制量

但是，我們延時100ms後再將控制量給模擬器。這樣能夠模擬現實世界，畢竟處理計算（讀取感測器）和執行都需要時間。

從步驟1開始重複，尋找下一個最優控制量。

可調性：

在我們的例子中，我們計算了1秒中內的最優解，這個參數是可以調節的。長時間窗口的優化會給控制器的動作漂亮的曲線，但是也會積累過多的誤差。實際上，如果這個優化時間窗口太大，汽車反而會脫離期望軌跡。

對於那些數學大神，以下有一個介紹自動駕駛會怎樣出錯的視頻（原文並沒有給出->_->）。雷鋒網

PID

比例-積分-微分(PID)控制器是另一種在無人駕駛中應用的控制器，下方視頻解釋了它的工作原理。

在我們的例子中，控制函數由以下組成：

交叉跟蹤誤差的比例部分

用於平滑運動過程的交叉跟蹤誤差微分部分

用於消除穩態干擾的交叉跟蹤誤差積分部分

來源：維基百科

我們用下方的PID控制器進行了模擬。由於損失函數相對簡單，所以運動存在一定的抖振。

（最後一段短視頻由於公眾號限制，請長按下方二維碼移步到騰訊視頻查看）

https://v.qq.com/x/page/o0805qgzbli.html

Credit

視頻由MPC使用Udacity自動駕駛課程提供的模擬器生成。

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！